roslaunch.launch

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

238 - def __init__(self, run_id, config, server_uri=None, pmon=None, is_core=False, remote_runner=None, is_child=False, is_rostest=False, num_workers=NUM_WORKERS, timeout=None, master_logger_level=False):

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

302

303 - def add_process_listener(self, l):

304 """ 305 Add listener to list of listeners. Not threadsafe. Must be 306 called before processes started. 307 @param l: listener 308 @type l: L{ProcessListener} 309 """ 310 self.listeners.add_process_listener(l)

311

312 - def _load_parameters(self):

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

351

352 - def _launch_nodes(self):

353 """ 354 Launch all the declared nodes/master 355 @return: two lists of node names where the first 356 is the nodes that successfully launched and the second is the 357 nodes that failed to launch. 358 @rtype: [[str], [str]] 359 """ 360 config = self.config 361 succeeded = [] 362 failed = [] 363 self.logger.info("launch_nodes: launching local nodes ...") 364 local_nodes = config.nodes 365 366 # don't launch remote nodes 367 local_nodes = [n for n in config.nodes if is_machine_local(n.machine)] 368 369 for node in local_nodes: 370 proc, success = self.launch_node(node) 371 if success: 372 succeeded.append(str(proc)) 373 else: 374 failed.append(str(proc)) 375 376 if self.remote_runner: 377 self.logger.info("launch_nodes: launching remote nodes ...") 378 r_succ, r_fail = self.remote_runner.launch_remote_nodes() 379 succeeded.extend(r_succ) 380 failed.extend(r_fail) 381 382 self.logger.info("... launch_nodes complete") 383 return succeeded, failed

384

385 - def _launch_master(self):

386 """ 387 Launches master if requested. 388 @return: True if a master was launched, False if a master was 389 already running. 390 @rtype: bool 391 @raise RLException: if master launch fails 392 """ 393 m = self.config.master 394 is_running = m.is_running() 395 396 if self.is_core and is_running: 397 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)) 398 399 if not is_running: 400 validate_master_launch(m, self.is_core, self.is_rostest) 401 402 printlog("auto-starting new master") 403 p = create_master_process( 404 self.run_id, m.type, get_ros_root(), m.get_port(), self.num_workers, 405 self.timeout, master_logger_level=self.master_logger_level) 406 self.pm.register_core_proc(p) 407 success = p.start() 408 if not success: 409 raise RLException("ERROR: unable to auto-start master process") 410 timeout_t = time.time() + _TIMEOUT_MASTER_START 411 while not m.is_running() and time.time() < timeout_t: 412 time.sleep(0.1) 413 414 if not m.is_running(): 415 raise RLException("ERROR: could not contact master [%s]"%m.uri) 416 417 printlog_bold("ROS_MASTER_URI=%s"%m.uri) 418 # TODO: although this dependency doesn't cause anything bad, 419 # it's still odd for launch to know about console stuff. This 420 # really should be an event. 421 update_terminal_name(m.uri) 422 423 # Param Server config params 424 param_server = m.get() 425 426 # #773: unique run ID 427 self._check_and_set_run_id(param_server, self.run_id) 428 429 if self.server_uri: 430 # store parent XML-RPC URI on param server 431 # - param name is the /roslaunch/hostname:port so that multiple roslaunches can store at once 432 hostname, port = rosgraph.network.parse_http_host_and_port(self.server_uri) 433 hostname = _hostname_to_rosname(hostname) 434 self.logger.info("setting /roslaunch/uris/%s__%s' to %s"%(hostname, port, self.server_uri)) 435 param_server.setParam(_ID, '/roslaunch/uris/%s__%s'%(hostname, port),self.server_uri) 436 437 return not is_running

438

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

440 """ 441 Initialize self.run_id to existing value or setup parameter 442 server with /run_id set to default_run_id 443 @param default_run_id: run_id to use if value is not set 444 @type default_run_id: str 445 @param param_server: parameter server proxy 446 @type param_server: xmlrpclib.ServerProxy 447 """ 448 code, _, val = param_server.hasParam(_ID, '/run_id') 449 if code == 1 and not val: 450 printlog_bold("setting /run_id to %s"%run_id) 451 param_server.setParam('/roslaunch', '/run_id', run_id) 452 else: 453 # verify that the run_id we have been set to matches what's on the parameter server 454 code, _, val = param_server.getParam('/roslaunch', '/run_id') 455 if code != 1: 456 #could only happen in a bad race condition with 457 #someone else restarting core 458 raise RLException("ERROR: unable to retrieve /run_id from parameter server") 459 if run_id != val: 460 raise RLException("run_id on parameter server does not match declared run_id: %s vs %s"%(val, run_id))

461 #self.run_id = val 462 #printlog_bold("/run_id is %s"%self.run_id) 463

464 - def _launch_executable(self, e):

465 """ 466 Launch a single L{Executable} object. Blocks until executable finishes. 467 @param e: Executable 468 @type e: L{Executable} 469 @raise RLException: if exectuable fails. Failure includes non-zero exit code. 470 """ 471 try: 472 #kwc: I'm still debating whether shell=True is proper 473 cmd = e.command 474 cmd = "%s %s"%(cmd, ' '.join(e.args)) 475 print("running %s" % cmd) 476 local_machine = self.config.machines[''] 477 env = setup_env(None, local_machine, self.config.master.uri) 478 retcode = subprocess.call(cmd, shell=True, env=env) 479 if retcode < 0: 480 raise RLException("command [%s] failed with exit code %s"%(cmd, retcode)) 481 except OSError as e: 482 raise RLException("command [%s] failed: %s"%(cmd, e))

483 484 #TODO: _launch_run_executables, _launch_teardown_executables 485 #TODO: define and implement behavior for remote launch

486 - def _launch_setup_executables(self):

487 """ 488 @raise RLException: if exectuable fails. Failure includes non-zero exit code. 489 """ 490 exes = [e for e in self.config.executables if e.phase == PHASE_SETUP] 491 for e in exes: 492 self._launch_executable(e)

493

494 - def _launch_core_nodes(self):

495 """ 496 launch any core services that are not already running. master must 497 be already running 498 @raise RLException: if core launches fail 499 """ 500 config = self.config 501 master = config.master.get() 502 tolaunch = [] 503 for node in config.nodes_core: 504 node_name = rosgraph.names.ns_join(node.namespace, node.name) 505 code, msg, _ = master.lookupNode(_ID, node_name) 506 if code == -1: 507 tolaunch.append(node) 508 elif code == 1: 509 print("core service [%s] found" % node_name) 510 else: 511 print("WARN: master is not behaving well (unexpected return value when looking up node)", file=sys.stderr) 512 self.logger.error("ERROR: master return [%s][%s] on lookupNode call"%(code,msg)) 513 514 for node in tolaunch: 515 node_name = rosgraph.names.ns_join(node.namespace, node.name) 516 name, success = self.launch_node(node, core=True) 517 if success: 518 print("started core service [%s]" % node_name) 519 else: 520 raise RLException("failed to start core service [%s]"%node_name)

521

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

523 """ 524 Launch a single node locally. Remote launching is handled separately by the remote module. 525 If node name is not assigned, one will be created for it. 526 527 @param node Node: node to launch 528 @param core bool: if True, core node 529 @return obj, bool: Process handle, successful launch. If success, return actual Process instance. Otherwise return name. 530 """ 531 self.logger.info("... preparing to launch node of type [%s/%s]", node.package, node.type) 532 533 # TODO: should this always override, per spec?. I added this 534 # so that this api can be called w/o having to go through an 535 # extra assign machines step. 536 if node.machine is None: 537 node.machine = self.config.machines[''] 538 if node.name is None: 539 node.name = rosgraph.names.anonymous_name(node.type) 540 541 master = self.config.master 542 import roslaunch.node_args 543 try: 544 process = create_node_process(self.run_id, node, master.uri) 545 except roslaunch.node_args.NodeParamsException as e: 546 self.logger.error(e) 547 printerrlog("ERROR: cannot launch node of type [%s/%s]: %s"%(node.package, node.type, str(e))) 548 if node.name: 549 return node.name, False 550 else: 551 return "%s/%s"%(node.package,node.type), False 552 553 self.logger.info("... created process [%s]", process.name) 554 if core: 555 self.pm.register_core_proc(process) 556 else: 557 self.pm.register(process) 558 node.process_name = process.name #store this in the node object for easy reference 559 self.logger.info("... registered process [%s]", process.name) 560 561 # note: this may raise FatalProcessLaunch, which aborts the entire launch 562 success = process.start() 563 if not success: 564 if node.machine.name: 565 printerrlog("launch of %s/%s on %s failed"%(node.package, node.type, node.machine.name)) 566 else: 567 printerrlog("local launch of %s/%s failed"%(node.package, node.type)) 568 else: 569 self.logger.info("... successfully launched [%s]", process.name) 570 return process, success

571

572 - def is_node_running(self, node):

573 """ 574 Check for running node process. 575 @param node Node: node object to check 576 @return bool: True if process associated with node is running (launched && !dead) 577 """ 578 #process_name is not set until node is launched. 579 return node.process_name and self.pm.has_process(node.process_name)

580

581 - def spin_once(self):

582 """ 583 Same as spin() but only does one cycle. must be run from the main thread. 584 """ 585 if not self.pm: 586 return False 587 return self.pm.mainthread_spin_once()

588

589 - def spin(self):

590 """ 591 spin() must be run from the main thread. spin() is very 592 important for roslaunch as it picks up jobs that the process 593 monitor need to be run in the main thread. 594 """ 595 self.logger.info("spin") 596 597 # #556: if we're just setting parameters and aren't launching 598 # any processes, exit. 599 if not self.pm or not self.pm.get_active_names(): 600 printlog_bold("No processes to monitor") 601 self.stop() 602 return # no processes 603 self.pm.mainthread_spin() 604 #self.pm.join() 605 self.logger.info("process monitor is done spinning, initiating full shutdown") 606 self.stop() 607 printlog_bold("done")

608

609 - def stop(self):

610 """ 611 Stop the launch and all associated processes. not thread-safe. 612 """ 613 self.logger.info("runner.stop()") 614 if self.pm is not None: 615 printlog("shutting down processing monitor...") 616 self.logger.info("shutting down processing monitor %s"%self.pm) 617 self.pm.shutdown() 618 self.pm.join() 619 self.pm = None 620 printlog("... shutting down processing monitor complete") 621 else: 622 self.logger.info("... roslaunch runner has already been stopped")

623

624 - def _setup(self):

625 """ 626 Setup the state of the ROS network, including the parameter 627 server state and core services 628 """ 629 # this may have already been done, but do just in case 630 self.config.assign_machines() 631 632 if self.remote_runner: 633 # hook in our listener aggregator 634 self.remote_runner.add_process_listener(self.listeners) 635 636 # start up the core: master + core nodes defined in core.xml 637 launched = self._launch_master() 638 if launched: 639 self._launch_core_nodes() 640 641 # run exectuables marked as setup period. this will block 642 # until these executables exit. setup executable have to run 643 # *before* parameters are uploaded so that commands like 644 # rosparam delete can execute. 645 self._launch_setup_executables() 646 647 # no parameters for a child process 648 if not self.is_child: 649 self._load_parameters()

650

651 - def launch(self):

652 """ 653 Run the launch. Depending on usage, caller should call 654 spin_once or spin as appropriate after launch(). 655 @return ([str], [str]): tuple containing list of nodes that 656 successfully launches and list of nodes that failed to launch 657 @rtype: ([str], [str]) 658 @raise RLException: if launch fails (e.g. run_id parameter does 659 not match ID on parameter server) 660 """ 661 try: 662 self._setup() 663 succeeded, failed = self._launch_nodes() 664 return succeeded, failed 665 except KeyboardInterrupt: 666 self.stop() 667 raise

668

669 - def run_test(self, test):

670 """ 671 Run the test node. Blocks until completion or timeout. 672 @param test: test node to run 673 @type test: Test 674 @raise RLTestTimeoutException: if test fails to launch or test times out 675 """ 676 self.logger.info("... preparing to run test [%s] of type [%s/%s]", test.test_name, test.package, test.type) 677 proc_h, success = self.launch_node(test) 678 if not success: 679 raise RLException("test [%s] failed to launch"%test.test_name) 680 681 #poll until test terminates or alloted time exceed 682 timeout_t = time.time() + test.time_limit 683 pm = self.pm 684 while pm.mainthread_spin_once() and self.is_node_running(test): 685 #test fails on timeout 686 if time.time() > timeout_t: 687 raise RLTestTimeoutException( 688 "max time [%ss] allotted for test [%s] of type [%s/%s]" % 689 (test.time_limit, test.test_name, test.package, test.type)) 690 time.sleep(0.1)

Source Code for Module roslaunch.launch