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):

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 """ 268 if run_id is None: 269 raise RLException("run_id is None") 270 self.run_id = run_id 271 272 # In the future we should can separate the notion of a core 273 # config vs. the config being launched. In that way, we can 274 # start to migrate to a model where a config is a parameter to 275 # a launch() method, rather than a central thing to the 276 # runner. 277 self.config = config 278 self.server_uri = server_uri 279 self.is_child = is_child 280 self.is_core = is_core 281 self.is_rostest = is_rostest 282 self.num_workers = num_workers 283 self.timeout = timeout 284 self.logger = logging.getLogger('roslaunch') 285 self.pm = pmon or start_process_monitor() 286 287 # wire in ProcessMonitor events to our listeners 288 # aggregator. We similarly wire in the remote events when we 289 # create the remote runner. 290 self.listeners = _ROSLaunchListeners() 291 if self.pm is None: 292 raise RLException("unable to initialize roslaunch process monitor") 293 if self.pm.is_shutdown: 294 raise RLException("bad roslaunch process monitor initialization: process monitor is already dead") 295 296 self.pm.add_process_listener(self.listeners) 297 298 self.remote_runner = remote_runner

299

300 - def add_process_listener(self, l):

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

308

309 - def _load_parameters(self):

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

348

349 - def _launch_nodes(self):

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

381

382 - def _launch_master(self):

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

433

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

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

456 #self.run_id = val 457 #printlog_bold("/run_id is %s"%self.run_id) 458

459 - def _launch_executable(self, e):

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

478 479 #TODO: _launch_run_executables, _launch_teardown_executables 480 #TODO: define and implement behavior for remote launch

481 - def _launch_setup_executables(self):

482 """ 483 @raise RLException: if exectuable fails. Failure includes non-zero exit code. 484 """ 485 exes = [e for e in self.config.executables if e.phase == PHASE_SETUP] 486 for e in exes: 487 self._launch_executable(e)

488

489 - def _launch_core_nodes(self):

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

516

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

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

566

567 - def is_node_running(self, node):

568 """ 569 Check for running node process. 570 @param node Node: node object to check 571 @return bool: True if process associated with node is running (launched && !dead) 572 """ 573 #process_name is not set until node is launched. 574 return node.process_name and self.pm.has_process(node.process_name)

575

576 - def spin_once(self):

577 """ 578 Same as spin() but only does one cycle. must be run from the main thread. 579 """ 580 if not self.pm: 581 return False 582 return self.pm.mainthread_spin_once()

583

584 - def spin(self):

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

603

604 - def stop(self):

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

618

619 - def _setup(self):

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

645

646 - def launch(self):

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

663

664 - def run_test(self, test):

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

Source Code for Module roslaunch.launch