roslaunch.launch

225 """ 226 Runs a roslaunch. The normal sequence of API calls is L{launch()} 227 followed by L{spin()}. An external thread can call L{stop()}; otherwise 228 the runner will block until an exit signal. Another usage is to 229 call L{launch()} followed by repeated calls to L{spin_once()}. This usage 230 allows the main thread to continue to do work while processes are 231 monitored. 232 """ 233

234 - def __init__(self, run_id, config, server_uri=None, pmon=None, is_core=False, remote_runner=None, is_child=False, is_rostest=False):

235 """ 236 @param run_id: /run_id for this launch. If the core is not 237 running, this value will be used to initialize /run_id. If 238 the core is already running, this value will be checked 239 against the value stored on the core. L{ROSLaunchRunner} will 240 fail during L{launch()} if they do not match. 241 @type run_id: str 242 @param config: roslauch instance to run 243 @type config: L{ROSLaunchConfig} 244 @param server_uri: XML-RPC URI of roslaunch server. 245 @type server_uri: str 246 @param pmon: optionally override the process 247 monitor the runner uses for starting and tracking processes 248 @type pmon: L{ProcessMonitor} 249 250 @param is_core: if True, this runner is a roscore 251 instance. This affects the error behavior if a master is 252 already running -- aborts if is_core is True and a core is 253 detected. 254 @type is_core: bool 255 @param remote_runner: remote roslaunch process runner 256 @param is_rostest: if True, this runner is a rostest 257 instance. This affects certain validation checks. 258 @type is_rostest: bool 259 """ 260 if run_id is None: 261 raise RLException("run_id is None") 262 self.run_id = run_id 263 264 # In the future we should can separate the notion of a core 265 # config vs. the config being launched. In that way, we can 266 # start to migrate to a model where a config is a parameter to 267 # a launch() method, rather than a central thing to the 268 # runner. 269 self.config = config 270 self.server_uri = server_uri 271 self.is_child = is_child 272 self.is_core = is_core 273 self.is_rostest = is_rostest 274 self.logger = logging.getLogger('roslaunch') 275 self.pm = pmon or start_process_monitor() 276 277 # wire in ProcessMonitor events to our listeners 278 # aggregator. We similarly wire in the remote events when we 279 # create the remote runner. 280 self.listeners = _ROSLaunchListeners() 281 if self.pm is None: 282 raise RLException("unable to initialize roslaunch process monitor") 283 if self.pm.is_shutdown: 284 raise RLException("bad roslaunch process monitor initialization: process monitor is already dead") 285 286 self.pm.add_process_listener(self.listeners) 287 288 self.remote_runner = remote_runner

289

290 - def add_process_listener(self, l):

291 """ 292 Add listener to list of listeners. Not threadsafe. Must be 293 called before processes started. 294 @param l: listener 295 @type l: L{ProcessListener} 296 """ 297 self.listeners.add_process_listener(l)

298

299 - def _load_parameters(self):

300 """ 301 Load parameters onto the parameter server 302 """ 303 self.logger.info("load_parameters starting ...") 304 config = self.config 305 param_server = config.master.get() 306 p = None 307 try: 308 # multi-call style xmlrpc 309 param_server_multi = config.master.get_multi() 310 311 # clear specified parameter namespaces 312 # #2468 unify clear params to prevent error 313 for p in _unify_clear_params(config.clear_params): 314 if param_server.hasParam(_ID, p)[2]: 315 #printlog("deleting parameter [%s]"%p) 316 param_server_multi.deleteParam(_ID, p) 317 r = param_server_multi() 318 for code, msg, _ in r: 319 if code != 1: 320 raise RLException("Failed to clear parameter: %s"%(msg)) 321 322 # multi-call objects are not reusable 323 param_server_multi = config.master.get_multi() 324 for p in config.params.itervalues(): 325 # suppressing this as it causes too much spam 326 #printlog("setting parameter [%s]"%p.key) 327 param_server_multi.setParam(_ID, p.key, p.value) 328 r = param_server_multi() 329 for code, msg, _ in r: 330 if code != 1: 331 raise RLException("Failed to set parameter: %s"%(msg)) 332 except RLException: 333 raise 334 except Exception as e: 335 printerrlog("load_parameters: unable to set parameters (last param was [%s]): %s"%(p,e)) 336 raise #re-raise as this is fatal 337 self.logger.info("... load_parameters complete")

338

339 - def _launch_nodes(self):

340 """ 341 Launch all the declared nodes/master 342 @return: two lists of node names where the first 343 is the nodes that successfully launched and the second is the 344 nodes that failed to launch. 345 @rtype: [[str], [str]] 346 """ 347 config = self.config 348 succeeded = [] 349 failed = [] 350 self.logger.info("launch_nodes: launching local nodes ...") 351 local_nodes = config.nodes 352 353 # don't launch remote nodes 354 local_nodes = [n for n in config.nodes if is_machine_local(n.machine)] 355 356 for node in local_nodes: 357 proc, success = self.launch_node(node) 358 if success: 359 succeeded.append(str(proc)) 360 else: 361 failed.append(str(proc)) 362 363 if self.remote_runner: 364 self.logger.info("launch_nodes: launching remote nodes ...") 365 r_succ, r_fail = self.remote_runner.launch_remote_nodes() 366 succeeded.extend(r_succ) 367 failed.extend(r_fail) 368 369 self.logger.info("... launch_nodes complete") 370 return succeeded, failed

371

372 - def _launch_master(self):

373 """ 374 Launches master if requested. 375 @raise RLException: if master launch fails 376 """ 377 m = self.config.master 378 is_running = m.is_running() 379 380 if self.is_core and is_running: 381 raise RLException("roscore cannot run as another roscore/master is already running. \nPlease kill other roscore/master processes before relaunching.\nThe ROS_MASTER_URI is %s"%(m.uri)) 382 383 if not is_running: 384 validate_master_launch(m, self.is_core, self.is_rostest) 385 386 printlog("auto-starting new master") 387 p = create_master_process(self.run_id, m.type, get_ros_root(), m.get_port()) 388 self.pm.register_core_proc(p) 389 success = p.start() 390 if not success: 391 raise RLException("ERROR: unable to auto-start master process") 392 timeout_t = time.time() + _TIMEOUT_MASTER_START 393 while not m.is_running() and time.time() < timeout_t: 394 time.sleep(0.1) 395 396 if not m.is_running(): 397 raise RLException("ERROR: could not contact master [%s]"%m.uri) 398 399 printlog_bold("ROS_MASTER_URI=%s"%m.uri) 400 # TODO: although this dependency doesn't cause anything bad, 401 # it's still odd for launch to know about console stuff. This 402 # really should be an event. 403 update_terminal_name(m.uri) 404 405 # Param Server config params 406 param_server = m.get() 407 408 # #773: unique run ID 409 self._check_and_set_run_id(param_server, self.run_id) 410 411 if self.server_uri: 412 # store parent XML-RPC URI on param server 413 # - param name is the /roslaunch/hostname:port so that multiple roslaunches can store at once 414 hostname, port = rosgraph.network.parse_http_host_and_port(self.server_uri) 415 hostname = _hostname_to_rosname(hostname) 416 self.logger.info("setting /roslaunch/uris/%s__%s' to %s"%(hostname, port, self.server_uri)) 417 param_server.setParam(_ID, '/roslaunch/uris/%s__%s'%(hostname, port),self.server_uri)

418

419 - def _check_and_set_run_id(self, param_server, run_id):

420 """ 421 Initialize self.run_id to existing value or setup parameter 422 server with /run_id set to default_run_id 423 @param default_run_id: run_id to use if value is not set 424 @type default_run_id: str 425 @param param_server: parameter server proxy 426 @type param_server: xmlrpclib.ServerProxy 427 """ 428 code, _, val = param_server.hasParam(_ID, '/run_id') 429 if code == 1 and not val: 430 printlog_bold("setting /run_id to %s"%run_id) 431 param_server.setParam('/roslaunch', '/run_id', run_id) 432 else: 433 # verify that the run_id we have been set to matches what's on the parameter server 434 code, _, val = param_server.getParam('/roslaunch', '/run_id') 435 if code != 1: 436 #could only happen in a bad race condition with 437 #someone else restarting core 438 raise RLException("ERROR: unable to retrieve /run_id from parameter server") 439 if run_id != val: 440 raise RLException("run_id on parameter server does not match declared run_id: %s vs %s"%(val, run_id))

441 #self.run_id = val 442 #printlog_bold("/run_id is %s"%self.run_id) 443

444 - def _launch_executable(self, e):

445 """ 446 Launch a single L{Executable} object. Blocks until executable finishes. 447 @param e: Executable 448 @type e: L{Executable} 449 @raise RLException: if exectuable fails. Failure includes non-zero exit code. 450 """ 451 try: 452 #kwc: I'm still debating whether shell=True is proper 453 cmd = e.command 454 cmd = "%s %s"%(cmd, ' '.join(e.args)) 455 print "running %s"%cmd 456 local_machine = self.config.machines[''] 457 env = setup_env(None, local_machine, self.config.master.uri) 458 retcode = subprocess.call(cmd, shell=True, env=env) 459 if retcode < 0: 460 raise RLException("command [%s] failed with exit code %s"%(cmd, retcode)) 461 except OSError as e: 462 raise RLException("command [%s] failed: %s"%(cmd, e))

463 464 #TODO: _launch_run_executables, _launch_teardown_executables 465 #TODO: define and implement behavior for remote launch

466 - def _launch_setup_executables(self):

467 """ 468 @raise RLException: if exectuable fails. Failure includes non-zero exit code. 469 """ 470 exes = [e for e in self.config.executables if e.phase == PHASE_SETUP] 471 for e in exes: 472 self._launch_executable(e)

473

474 - def _launch_core_nodes(self):

475 """ 476 launch any core services that are not already running. master must 477 be already running 478 @raise RLException: if core launches fail 479 """ 480 config = self.config 481 master = config.master.get() 482 tolaunch = [] 483 for node in config.nodes_core: 484 node_name = rosgraph.names.ns_join(node.namespace, node.name) 485 code, msg, _ = master.lookupNode(_ID, node_name) 486 if code == -1: 487 tolaunch.append(node) 488 elif code == 1: 489 print "core service [%s] found"%node_name 490 else: 491 print >> sys.stderr, "WARN: master is not behaving well (unexpected return value when looking up node)" 492 self.logger.error("ERROR: master return [%s][%s] on lookupNode call"%(code,msg)) 493 494 for node in tolaunch: 495 node_name = rosgraph.names.ns_join(node.namespace, node.name) 496 name, success = self.launch_node(node, core=True) 497 if success: 498 print "started core service [%s]"%node_name 499 else: 500 raise RLException("failed to start core service [%s]"%node_name)

501

502 - def launch_node(self, node, core=False):

503 """ 504 Launch a single node locally. Remote launching is handled separately by the remote module. 505 If node name is not assigned, one will be created for it. 506 507 @param node Node: node to launch 508 @param core bool: if True, core node 509 @return obj, bool: Process handle, successful launch. If success, return actual Process instance. Otherwise return name. 510 """ 511 self.logger.info("... preparing to launch node of type [%s/%s]", node.package, node.type) 512 513 # TODO: should this always override, per spec?. I added this 514 # so that this api can be called w/o having to go through an 515 # extra assign machines step. 516 if node.machine is None: 517 node.machine = self.config.machines[''] 518 if node.name is None: 519 node.name = rosgraph.names.anonymous_name(node.type) 520 521 master = self.config.master 522 import roslaunch.node_args 523 try: 524 process = create_node_process(self.run_id, node, master.uri) 525 except roslaunch.node_args.NodeParamsException as e: 526 self.logger.error(e) 527 if node.package == 'rosout' and node.type == 'rosout': 528 printerrlog("\n\n\nERROR: rosout is not built. Please run 'rosmake rosout'\n\n\n") 529 else: 530 printerrlog("ERROR: cannot launch node of type [%s/%s]: %s"%(node.package, node.type, str(e))) 531 if node.name: 532 return node.name, False 533 else: 534 return "%s/%s"%(node.package,node.type), False 535 536 self.logger.info("... created process [%s]", process.name) 537 if core: 538 self.pm.register_core_proc(process) 539 else: 540 self.pm.register(process) 541 node.process_name = process.name #store this in the node object for easy reference 542 self.logger.info("... registered process [%s]", process.name) 543 544 # note: this may raise FatalProcessLaunch, which aborts the entire launch 545 success = process.start() 546 if not success: 547 if node.machine.name: 548 printerrlog("launch of %s/%s on %s failed"%(node.package, node.type, node.machine.name)) 549 else: 550 printerrlog("local launch of %s/%s failed"%(node.package, node.type)) 551 else: 552 self.logger.info("... successfully launched [%s]", process.name) 553 return process, success

554

555 - def is_node_running(self, node):

556 """ 557 Check for running node process. 558 @param node Node: node object to check 559 @return bool: True if process associated with node is running (launched && !dead) 560 """ 561 #process_name is not set until node is launched. 562 return node.process_name and self.pm.has_process(node.process_name)

563

564 - def spin_once(self):

565 """ 566 Same as spin() but only does one cycle. must be run from the main thread. 567 """ 568 if not self.pm: 569 return False 570 return self.pm.mainthread_spin_once()

571

572 - def spin(self):

573 """ 574 spin() must be run from the main thread. spin() is very 575 important for roslaunch as it picks up jobs that the process 576 monitor need to be run in the main thread. 577 """ 578 self.logger.info("spin") 579 580 # #556: if we're just setting parameters and aren't launching 581 # any processes, exit. 582 if not self.pm or not self.pm.get_active_names(): 583 printlog_bold("No processes to monitor") 584 self.stop() 585 return # no processes 586 self.pm.mainthread_spin() 587 #self.pm.join() 588 self.logger.info("process monitor is done spinning, initiating full shutdown") 589 self.stop() 590 printlog_bold("done")

591

592 - def stop(self):

593 """ 594 Stop the launch and all associated processes. not thread-safe. 595 """ 596 self.logger.info("runner.stop()") 597 if self.pm is not None: 598 printlog("shutting down processing monitor...") 599 self.logger.info("shutting down processing monitor %s"%self.pm) 600 self.pm.shutdown() 601 self.pm.join() 602 self.pm = None 603 printlog("... shutting down processing monitor complete") 604 else: 605 self.logger.info("... roslaunch runner has already been stopped")

606

607 - def _setup(self):

608 """ 609 Setup the state of the ROS network, including the parameter 610 server state and core services 611 """ 612 # this may have already been done, but do just in case 613 self.config.assign_machines() 614 615 if self.remote_runner: 616 # hook in our listener aggregator 617 self.remote_runner.add_process_listener(self.listeners) 618 619 # start up the core: master + core nodes defined in core.xml 620 self._launch_master() 621 self._launch_core_nodes() 622 623 # run exectuables marked as setup period. this will block 624 # until these executables exit. setup executable have to run 625 # *before* parameters are uploaded so that commands like 626 # rosparam delete can execute. 627 self._launch_setup_executables() 628 629 # no parameters for a child process 630 if not self.is_child: 631 self._load_parameters()

632

633 - def launch(self):

634 """ 635 Run the launch. Depending on usage, caller should call 636 spin_once or spin as appropriate after launch(). 637 @return ([str], [str]): tuple containing list of nodes that 638 successfully launches and list of nodes that failed to launch 639 @rtype: ([str], [str]) 640 @raise RLException: if launch fails (e.g. run_id parameter does 641 not match ID on parameter server) 642 """ 643 try: 644 self._setup() 645 succeeded, failed = self._launch_nodes() 646 return succeeded, failed 647 except KeyboardInterrupt: 648 self.stop() 649 raise

650

651 - def run_test(self, test):

652 """ 653 Run the test node. Blocks until completion or timeout. 654 @param test: test node to run 655 @type test: Test 656 @raise RLTestTimeoutException: if test fails to launch or test times out 657 """ 658 self.logger.info("... preparing to run test [%s] of type [%s/%s]", test.test_name, test.package, test.type) 659 proc_h, success = self.launch_node(test) 660 if not success: 661 raise RLException("test [%s] failed to launch"%test.test_name) 662 663 #poll until test terminates or alloted time exceed 664 timeout_t = time.time() + test.time_limit 665 pm = self.pm 666 while pm.mainthread_spin_once() and self.is_node_running(test): 667 #test fails on timeout 668 if time.time() > timeout_t: 669 raise RLTestTimeoutException("test max time allotted") 670 time.sleep(0.1)

Source Code for Module roslaunch.launch