roslaunch.core

1 # Software License Agreement (BSD License) 2 # 3 # Copyright (c) 2008, Willow Garage, Inc. 4 # All rights reserved. 5 # 6 # Redistribution and use in source and binary forms, with or without 7 # modification, are permitted provided that the following conditions 8 # are met: 9 # 10 # * Redistributions of source code must retain the above copyright 11 # notice, this list of conditions and the following disclaimer. 12 # * Redistributions in binary form must reproduce the above 13 # copyright notice, this list of conditions and the following 14 # disclaimer in the documentation and/or other materials provided 15 # with the distribution. 16 # * Neither the name of Willow Garage, Inc. nor the names of its 17 # contributors may be used to endorse or promote products derived 18 # from this software without specific prior written permission. 19 # 20 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 # POSSIBILITY OF SUCH DAMAGE. 32 33 from __future__ import print_function 34 35 """ 36 Core roslaunch model and lower-level utility routines. 37 """ 38 39 import os 40 import logging 41 42 import re 43 import socket 44 import sys 45 try: 46 from xmlrpc.client import MultiCall, ServerProxy 47 except ImportError: 48 from xmlrpclib import MultiCall, ServerProxy 49 50 import rospkg 51 52 import rosgraph 53 import rosgraph.names 54 import rosgraph.network 55 56 from xml.sax.saxutils import escape 57 try: 58 unicode 59 except NameError: 60 # Python 3: for _xml_escape 61 basestring = unicode = str 62

63 -class RLException(Exception):

64 """Base roslaunch exception type""" 65 pass

66 67 ## Phases allow executables to be assigned to a particular run period 68 PHASE_SETUP = 'setup' 69 PHASE_RUN = 'run' 70 PHASE_TEARDOWN = 'teardown' 71 72 _child_mode = False

73 -def is_child_mode():

74 """ 75 :returns: ``True`` if roslaunch is running in remote child mode, ``bool`` 76 """ 77 return _child_mode

78 -def set_child_mode(child_mode):

79 """ 80 :param child_mode: True if roslaunch is running in remote 81 child mode, ``bool`` 82 """ 83 global _child_mode 84 _child_mode = child_mode

85

86 -def is_machine_local(machine):

87 """ 88 Check to see if machine is local. NOTE: a machine is not local if 89 its user credentials do not match the current user. 90 :param machine: Machine, ``Machine`` 91 :returns: True if machine is local and doesn't require remote login, ``bool`` 92 """ 93 try: 94 # If Python has ipv6 disabled but machine.address can be resolved somehow to an ipv6 address, then host[4][0] will be int 95 machine_ips = [host[4][0] for host in socket.getaddrinfo(machine.address, 0, 0, 0, socket.SOL_TCP) if isinstance(host[4][0], str)] 96 except socket.gaierror: 97 raise RLException("cannot resolve host address for machine [%s]"%machine.address) 98 local_addresses = ['localhost'] + rosgraph.network.get_local_addresses() 99 # check 127/8 and local addresses 100 is_local = ([ip for ip in machine_ips if (ip.startswith('127.') or ip == '::1')] != []) or (set(machine_ips) & set(local_addresses) != set()) 101 102 #491: override local to be ssh if machine.user != local user 103 if is_local and machine.user: 104 import getpass 105 is_local = machine.user == getpass.getuser() 106 return is_local

107 108 _printlog_handlers = [] 109 _printerrlog_handlers = []

110 -def printlog(msg):

111 """ 112 Core utility for printing message to stdout as well as printlog handlers 113 :param msg: message to print, ``str`` 114 """ 115 for h in _printlog_handlers: 116 try: # don't let this bomb out the actual code 117 h(msg) 118 except: 119 pass 120 try: # don't let this bomb out the actual code 121 print(msg) 122 except: 123 pass

124

125 -def printlog_bold(msg):

126 """ 127 Similar to L{printlog()}, but the message printed to screen is bolded for greater clarity 128 :param msg: message to print, ``str`` 129 """ 130 for h in _printlog_handlers: 131 try: # don't let this bomb out the actual code 132 h(msg) 133 except: 134 pass 135 try: # don't let this bomb out the actual code 136 if sys.platform in ['win32']: 137 print('%s' % msg) # windows console is terrifically boring 138 else: 139 print('\033[1m%s\033[0m' % msg) 140 except: 141 pass

142

143 -def printerrlog(msg):

144 """ 145 Core utility for printing message to stderr as well as printerrlog handlers 146 :param msg: message to print, ``str`` 147 """ 148 for h in _printerrlog_handlers: 149 try: # don't let this bomb out the actual code 150 h(msg) 151 except: 152 pass 153 # #1003: this has raised IOError (errno 104) in robot use. Better to 154 # trap than let a debugging routine fault code. 155 try: # don't let this bomb out the actual code 156 print('\033[31m%s\033[0m' % msg, file=sys.stderr) 157 except: 158 pass

159

160 -def add_printlog_handler(h):

161 """ 162 Register additional handler for printlog() 163 """ 164 _printlog_handlers.append(h)

165

166 -def add_printerrlog_handler(h):

167 """ 168 Register additional handler for printerrlog() 169 """ 170 _printerrlog_handlers.append(h)

171

172 -def clear_printlog_handlers():

173 """ 174 Delete all printlog handlers. required for testing 175 """ 176 del _printlog_handlers[:]

177

178 -def clear_printerrlog_handlers():

179 """ 180 Delete all printerrlog handlers. required for testing 181 """ 182 del _printerrlog_handlers[:]

183

184 -def setup_env(node, machine, master_uri, env=None):

185 """ 186 Create dictionary of environment variables to set for launched 187 process. 188 189 setup_env() will only set ROS_*, PYTHONPATH, and user-specified 190 environment variables. 191 192 :param machine: machine being launched on, ``Machine`` 193 :param node: node that is being launched or None, ``Node`` 194 :param master_uri: ROS master URI, ``str`` 195 :param env: base environment configuration, defaults to ``os.environ`` 196 :returns: process env dictionary, ``dict`` 197 """ 198 if env is None: 199 env = os.environ 200 201 d = env.copy() 202 d[rosgraph.ROS_MASTER_URI] = master_uri 203 204 # add node-specific env args last as they have highest precedence 205 if node: 206 if rosgraph.ROS_NAMESPACE in d: 207 del d[rosgraph.ROS_NAMESPACE] 208 ns = node.namespace 209 if ns[-1] == '/': 210 ns = ns[:-1] 211 if ns: 212 d[rosgraph.ROS_NAMESPACE] = str(ns) 213 for name, value in node.env_args: 214 d[str(name)] = str(value) 215 216 return d

217

218 -def rle_wrapper(fn):

219 """ 220 Wrap lower-level exceptions in RLException class 221 :returns:: function wrapper that throws an RLException if the 222 wrapped function throws an Exception, ``fn`` 223 """ 224 def wrapped_fn(*args): 225 try: 226 return fn(*args) 227 except Exception as e: 228 # we specifically catch RLExceptions and print their messages differently 229 raise RLException("ERROR: %s"%e)

230 return wrapped_fn 231 232 get_ros_root = rospkg.get_ros_root 233 get_master_uri_env = rle_wrapper(rosgraph.get_master_uri) 234 get_ros_package_path = rospkg.get_ros_package_path 235

236 -def remap_localhost_uri(uri, force_localhost=False):

237 """ 238 Resolve localhost addresses to an IP address so that 239 :param uri: XML-RPC URI, ``str`` 240 :param force_localhost: if True, URI is mapped onto the local machine no matter what, ``bool`` 241 """ 242 hostname, port = rosgraph.network.parse_http_host_and_port(uri) 243 if force_localhost or hostname == 'localhost': 244 return rosgraph.network.create_local_xmlrpc_uri(port) 245 else: 246 return uri

247 248 ################################################################## 249 # DATA STRUCTURES 250

251 -class Master(object):

252 """ 253 Data structure for representing and querying state of master 254 """ 255 __slots__ = ['type', 'auto', 'uri'] 256 ROSMASTER = 'rosmaster' 257 258 # deprecated 259 ZENMASTER = 'zenmaster' 260

261 - def __init__(self, type_=None, uri=None, auto=None):

262 """ 263 Create new Master instance. 264 :param uri: master URI. Defaults to ROS_MASTER_URI environment variable, ``str`` 265 :param type_: Currently only support 'rosmaster', ``str`` 266 """ 267 self.auto = None # no longer used 268 self.type = type_ or Master.ROSMASTER 269 self.uri = uri or get_master_uri_env()

270

271 - def get_host(self):

272 # parse from the URI 273 host, _ = rosgraph.network.parse_http_host_and_port(self.uri) 274 return host

275

276 - def get_port(self):

277 """ 278 Get the port this master is configured for. 279 """ 280 # parse from the URI 281 _, urlport = rosgraph.network.parse_http_host_and_port(self.uri) 282 return urlport

283

284 - def __eq__(self, m2):

285 if not isinstance(m2, Master): 286 return False 287 else: 288 return m2.type == self.type and m2.uri == self.uri

289

290 - def get(self):

291 """ 292 :returns:: XMLRPC proxy for communicating with master, ``xmlrpc.client.ServerProxy`` 293 """ 294 return ServerProxy(self.uri)

295

296 - def get_multi(self):

297 """ 298 :returns:: multicall XMLRPC proxy for communicating with master, ``xmlrpc.client.MultiCall`` 299 """ 300 return MultiCall(self.get())

301

302 - def is_running(self):

303 """ 304 Check if master is running. 305 :returns:: True if the master is running, ``bool`` 306 """ 307 try: 308 try: 309 to_orig = socket.getdefaulttimeout() 310 # enable timeout 311 socket.setdefaulttimeout(5.0) 312 logging.getLogger('roslaunch').info('master.is_running[%s]'%self.uri) 313 code, status, val = self.get().getPid('/roslaunch') 314 if code != 1: 315 raise RLException("ERROR: master failed status check: %s"%msg) 316 logging.getLogger('roslaunch.core').debug('master.is_running[%s]: True'%self.uri) 317 return True 318 finally: 319 socket.setdefaulttimeout(to_orig) 320 except: 321 logging.getLogger('roslaunch.core').debug('master.is_running[%s]: False'%self.uri) 322 return False

323 324 ## number of seconds that a child machine is allowed to register with 325 ## the parent before being considered failed 326 _DEFAULT_REGISTER_TIMEOUT = 10.0 327

328 -class Machine(object):

329 """ 330 Data structure for storing information about a machine in the ROS 331 system. Corresponds to the 'machine' tag in the launch 332 specification. 333 """ 334 __slots__ = ['name', 'address', 'ssh_port', 'user', 'password', 'assignable', 335 'env_loader', 'timeout']

336 - def __init__(self, name, address, 337 env_loader=None, ssh_port=22, user=None, password=None, 338 assignable=True, env_args=[], timeout=None):

339 """ 340 :param name: machine name, ``str`` 341 :param address: network address of machine, ``str`` 342 :param env_loader: Path to environment loader, ``str`` 343 :param ssh_port: SSH port number, ``int`` 344 :param user: SSH username, ``str`` 345 :param password: SSH password. Not recommended for use. Use SSH keys instead., ``str`` 346 """ 347 self.name = name 348 self.env_loader = env_loader 349 self.user = user or None 350 self.password = password 351 self.address = address 352 self.ssh_port = ssh_port 353 self.assignable = assignable 354 self.timeout = timeout or _DEFAULT_REGISTER_TIMEOUT

355

356 - def __str__(self):

357 return "Machine(name[%s] env_loader[%s] address[%s] ssh_port[%s] user[%s] assignable[%s] timeout[%s])"%(self.name, self.env_loader, self.address, self.ssh_port, self.user, self.assignable, self.timeout)

358 - def __eq__(self, m2):

359 if not isinstance(m2, Machine): 360 return False 361 return self.name == m2.name and \ 362 self.assignable == m2.assignable and \ 363 self.config_equals(m2)

364

365 - def config_key(self):

366 """ 367 Get a key that represents the configuration of the 368 machine. machines with identical configurations have identical 369 keys 370 371 :returns:: configuration key, ``str`` 372 """ 373 return "Machine(address[%s] env_loader[%s] ssh_port[%s] user[%s] password[%s] timeout[%s])"%(self.address, self.env_loader, self.ssh_port, self.user or '', self.password or '', self.timeout)

374

375 - def config_equals(self, m2):

376 """ 377 :returns:: True if machines have identical configurations, ``bool`` 378 """ 379 if not isinstance(m2, Machine): 380 return False 381 return self.config_key() == m2.config_key()

382

383 - def __ne__(self, m2):

384 return not self.__eq__(m2)

385

386 -class Param(object):

387 """ 388 Data structure for storing information about a desired parameter in 389 the ROS system Corresponds to the 'param' tag in the launch 390 specification. 391 """

392 - def __init__(self, key, value):

393 self.key = rosgraph.names.canonicalize_name(key) 394 self.value = value

395 - def __eq__(self, p):

396 if not isinstance(p, Param): 397 return False 398 return p.key == self.key and p.value == self.value

399 - def __ne__(self, p):

400 return not self.__eq__(p)

401 - def __str__(self):

402 return "%s=%s"%(self.key, self.value)

403 - def __repr__(self):

404 return "%s=%s"%(self.key, self.value)

405 406 _local_m = None

407 -def local_machine():

408 """ 409 :returns:: Machine instance representing the local machine, ``Machine`` 410 """ 411 global _local_m 412 if _local_m is None: 413 _local_m = Machine('', 'localhost') 414 return _local_m

415

416 -class Node(object):

417 """ 418 Data structure for storing information about a desired node in 419 the ROS system Corresponds to the 'node' tag in the launch 420 specification. 421 """ 422 __slots__ = ['package', 'type', 'name', 'namespace', \ 423 'machine_name', 'machine', 'args', 'respawn', \ 424 'respawn_delay', \ 425 'remap_args', 'env_args',\ 426 'process_name', 'output', 'cwd', 427 'launch_prefix', 'required', 428 'filename'] 429

430 - def __init__(self, package, node_type, name=None, namespace='/', \ 431 machine_name=None, args='', \ 432 respawn=False, respawn_delay=0.0, \ 433 remap_args=None,env_args=None, output=None, cwd=None, \ 434 launch_prefix=None, required=False, filename='<unknown>'):

435 """ 436 :param package: node package name, ``str`` 437 :param node_type: node type, ``str`` 438 :param name: node name, ``str`` 439 :param namespace: namespace for node, ``str`` 440 :param machine_name: name of machine to run node on, ``str`` 441 :param args: argument string to pass to node executable, ``str`` 442 :param respawn: if True, respawn node if it dies, ``bool`` 443 :param respawn: if respawn is True, respawn node after delay, ``float`` 444 :param remap_args: list of [(from, to)] remapping arguments, ``[(str, str)]`` 445 :param env_args: list of [(key, value)] of 446 additional environment vars to set for node, ``[(str, str)]`` 447 :param output: where to log output to, either Node, 'screen' or 'log', ``str`` 448 :param cwd: current working directory of node, either 'node', 'ROS_HOME'. Default: ROS_HOME, ``str`` 449 :param launch_prefix: launch command/arguments to prepend to node executable arguments, ``str`` 450 :param required: node is required to stay running (launch fails if node dies), ``bool`` 451 :param filename: name of file Node was parsed from, ``str`` 452 453 :raises: :exc:`ValueError` If parameters do not validate 454 """ 455 456 self.package = package 457 self.type = node_type 458 self.name = name or None 459 self.namespace = rosgraph.names.make_global_ns(namespace or '/') 460 self.namespace = re.sub("//+", "/", self.namespace) 461 self.machine_name = machine_name or None 462 self.respawn = respawn 463 self.respawn_delay = respawn_delay 464 self.args = args or '' 465 self.remap_args = remap_args or [] 466 self.env_args = env_args or [] 467 self.output = output 468 self.cwd = cwd 469 if self.cwd == 'ros_home': # be lenient on case 470 self.cwd = 'ROS_HOME' 471 472 self.launch_prefix = launch_prefix or None 473 self.required = required 474 self.filename = filename 475 476 if self.respawn and self.required: 477 raise ValueError("respawn and required cannot both be set to true") 478 479 # validation 480 if self.name and rosgraph.names.SEP in self.name: # #1821, namespaces in nodes need to be banned 481 raise ValueError("node name cannot contain a namespace") 482 if not len(self.package.strip()): 483 raise ValueError("package must be non-empty") 484 if not len(self.type.strip()): 485 raise ValueError("type must be non-empty") 486 if not self.output in ['log', 'screen', None]: 487 raise ValueError("output must be one of 'log', 'screen'") 488 if not self.cwd in ['ROS_HOME', 'node', None]: 489 raise ValueError("cwd must be one of 'ROS_HOME', 'node'") 490 491 # Extra slots for assigning later 492 493 # slot to store the process name in so that we can query the 494 # associated process state 495 self.process_name = None 496 497 # machine is the assigned machine instance. should probably 498 # consider storing this elsewhere as it can be inconsistent 499 # with machine_name and is also a runtime, rather than 500 # configuration property 501 self.machine = None

502 503 504

505 - def xmltype(self):

506 return 'node'

507

508 - def xmlattrs(self):

509 name_str = cwd_str = respawn_str = None 510 if self.name: 511 name_str = self.name 512 if self.cwd: 513 cwd_str = self.cwd 514 515 return [ 516 ('pkg', self.package), 517 ('type', self.type), 518 ('machine', self.machine_name), 519 ('ns', self.namespace), 520 ('args', self.args), 521 ('output', self.output), 522 ('cwd', cwd_str), 523 ('respawn', self.respawn), #not valid on <test> 524 ('respawn_delay', self.respawn_delay), # not valid on <test> 525 ('name', name_str), 526 ('launch-prefix', self.launch_prefix), 527 ('required', self.required), 528 ]

529 530 #TODO: unify with to_remote_xml using a filter_fn

531 - def to_xml(self):

532 """ 533 convert representation into XML representation. Currently cannot represent private parameters. 534 :returns:: XML representation for remote machine, ``str`` 535 """ 536 t = self.xmltype() 537 attrs = [(a, v) for a, v in self.xmlattrs() if v != None] 538 xmlstr = '<%s %s>\n'%(t, ' '.join(['%s="%s"'%(val[0], _xml_escape(val[1])) for val in attrs])) 539 xmlstr += ''.join([' <remap from="%s" to="%s" />\n'%tuple(r) for r in self.remap_args]) 540 xmlstr += ''.join([' <env name="%s" value="%s" />\n'%tuple(e) for e in self.env_args]) 541 xmlstr += "</%s>"%t 542 return xmlstr

543

544 - def to_remote_xml(self):

545 """ 546 convert representation into remote representation. Remote representation does 547 not include parameter settings or 'machine' attribute 548 :returns:: XML representation for remote machine, ``str`` 549 """ 550 t = self.xmltype() 551 attrs = [(a, v) for a, v in self.xmlattrs() if v != None and a != 'machine'] 552 xmlstr = '<%s %s>\n'%(t, ' '.join(['%s="%s"'%(val[0], _xml_escape(val[1])) for val in attrs])) 553 xmlstr += ''.join([' <remap from="%s" to="%s" />\n'%tuple(r) for r in self.remap_args]) 554 xmlstr += ''.join([' <env name="%s" value="%s" />\n'%tuple(e) for e in self.env_args]) 555 xmlstr += "</%s>"%t 556 return xmlstr

557

558 -def _xml_escape(s):

559 """ 560 Escape string for XML 561 :param s: string to escape, ``str`` 562 :returns:: string with XML entities (<, >, \", &) escaped, ``str`` 563 """ 564 # use official escaping to preserve unicode. 565 # see import at the top for py3k-compat 566 if isinstance(s, basestring): 567 return escape(s, entities={'"': '"'}) 568 else: 569 # don't escape non-string attributes 570 return s

571 572 TEST_TIME_LIMIT_DEFAULT = 1 * 60 #seconds 573 574

575 -class Test(Node):

576 """ 577 A Test is a Node with special semantics that it performs a 578 unit/integration test. The data model is the same except the 579 option to set the respawn flag is removed. 580 """ 581 __slots__ = ['test_name', 'time_limit', 'retry'] 582

583 - def __init__(self, test_name, package, node_type, name=None, \ 584 namespace='/', machine_name=None, args='', \ 585 remap_args=None, env_args=None, time_limit=None, cwd=None, 586 launch_prefix=None, retry=None, filename="<unknown>"):

587 """ 588 Construct a new test node. 589 :param test_name: name of test for recording in test results, ``str`` 590 :param time_limit: number of seconds that a test 591 should run before marked as a failure, ``int/float/long`` 592 """ 593 super(Test, self).__init__(package, node_type, name=name, \ 594 namespace=namespace, \ 595 machine_name=machine_name, args=args, \ 596 remap_args=remap_args, 597 env_args=env_args, 598 #output always is log 599 output='log', cwd=cwd, 600 launch_prefix=launch_prefix, filename=filename) 601 self.test_name = test_name 602 603 self.retry = retry or 0 604 time_limit = time_limit or TEST_TIME_LIMIT_DEFAULT 605 number_types = [float, int] 606 try: 607 number_types.append(long) 608 except NameError: 609 pass 610 if not type(time_limit) in number_types: 611 raise ValueError("'time-limit' must be a number") 612 time_limit = float(time_limit) #force to floating point 613 if time_limit <= 0: 614 raise ValueError("'time-limit' must be a positive number") 615 616 self.time_limit = time_limit

617

618 - def xmltype(self):

619 return 'test'

620

621 - def xmlattrs(self):

622 """ 623 NOTE: xmlattrs does not necessarily produce identical XML as 624 to what it was initialized with, though the properties are the same 625 """ 626 attrs = Node.xmlattrs(self) 627 attrs = [(a, v) for (a, v) in attrs if a not in ['respawn', \ 628 'respawn_delay']] 629 attrs.append(('test-name', self.test_name)) 630 631 if self.retry: 632 attrs.append(('retry', str(self.retry))) 633 if self.time_limit != TEST_TIME_LIMIT_DEFAULT: 634 attrs.append(('time-limit', self.time_limit)) 635 return attrs

636 637

638 -class Executable(object):

639 """ 640 Executable is a generic container for executable commands. 641 """ 642

643 - def __init__(self, cmd, args, phase=PHASE_RUN):

644 """ 645 :param cmd: name of command to run, ``str`` 646 :param args: arguments to command, ``(str,)`` 647 :param phase: PHASE_SETUP|PHASE_RUN|PHASE_TEARDOWN. Indicates whether the 648 command should be run before, during, or after launch, ``str`` 649 """ 650 self.command = cmd 651 self.args = args 652 self.phase = phase

653 - def __repr__(self):

654 return "%s %s"%(self.command, ' '.join(self.args))

655 - def __str__(self):

656 return "%s %s"%(self.command, ' '.join(self.args))

657

658 -class RosbinExecutable(Executable):

659 """ 660 RosbinExecutables are executables stored in ROS_ROOT/bin. 661 """

662 - def __init__(self, cmd, args, phase=PHASE_RUN):

663 super(RosbinExecutable, self).__init__(cmd, args, phase)

664 - def __repr__(self):

665 return "ros/bin/%s %s"%(self.command, ' '.join(self.args))

666 - def __str__(self):

667 return "ros/bin/%s %s"%(self.command, ' '.join(self.args))

668 669

670 -def generate_run_id():

671 """ 672 Utility routine for generating run IDs (UUIDs) 673 :returns: guid, ``str`` 674 """ 675 import uuid 676 return str(uuid.uuid1())

677

Source Code for Module roslaunch.core