default_cfg_fkie.default

63

64 - def __init__(self):

65 self.nodes = [] 66 '''@ivar: the list with names of nodes with name spaces.''' 67 self.sensors = {} 68 '''@ivar: Sensor description: C{dict(node name : [(sensor type, sensor name, sensor description), ...])}''' 69 self.robot_descr = ('', '', '') 70 '''@ivar: robot description as tupel of (type, name, text) ''' 71 self.package = '' 72 self.file = '' 73 self.__lock = threading.RLock() 74 # Load parameter 75 self.launch_file = rospy.get_param('~launch_file', '') 76 rospy.loginfo("launch_file: %s" % self.launch_file) 77 self.package = rospy.get_param('~package', '') 78 rospy.loginfo("package: %s" % self.package) 79 self.do_autostart = rospy.get_param('~autostart', False) 80 rospy.loginfo("do_autostart: %s" % self.do_autostart) 81 self.load_params_at_start = rospy.get_param('~load_params_at_start', True) 82 self.parameter_loaded = False 83 rospy.loginfo("load_params_at_start: %s" % self.load_params_at_start) 84 self.argv = rospy.get_param('~argv', []) 85 rospy.loginfo("argv: %s" % self.argv) 86 if not isinstance(self.argv, list): 87 self.argv = ["%s" % self.argv] 88 sys.argv.extend(self.argv) 89 if self.do_autostart: 90 rospy.set_param('~autostart', False) 91 # initialize the ROS services 92 # rospy.Service('~load', LoadLaunch, self.rosservice_load_launch) 93 self._reload_service = rospy.Service('~reload', std_srvs.srv.Empty, self.rosservice_reload) 94 rospy.Service('~description', ListDescription, self.rosservice_description) 95 self.runService = None 96 '''@ivar: The service will be created on each load of a launch file to 97 inform the caller about a new configuration. ''' 98 self.listService = None 99 '''@ivar: The service will be created on each load of a launch file to 100 inform the caller about a new configuration. ''' 101 self.description_response = ListDescriptionResponse() 102 # variables to print the pending autostart nodes 103 self._pending_starts = set() 104 self._pending_starts_last_printed = set()

105

106 - def _filter_args(self, argv):

107 afilter = ['__ns:=', '__name:=', '_package:=', '_launch_file:='] 108 result = [] 109 for a in argv: 110 in_filter = False 111 for f in afilter: 112 if a.startswith(f): 113 in_filter = True 114 break 115 if ':=' not in a or in_filter: 116 continue 117 result.append(a) 118 return result

119

120 - def load(self, delay_service_creation=0.):

121 ''' 122 Load the launch file configuration 123 ''' 124 with self.__lock: 125 self._pending_starts.clear() 126 # shutdown the services to inform the caller about a new configuration. 127 if self.runService is not None: 128 self.runService.shutdown('reload config') 129 self.runService = None 130 if self.listService is not None: 131 self.listService.shutdown('reload config') 132 self.listService = None 133 self.nodes = [] # the name of nodes with namespace 134 self.sensors = {} # sensor descriptions 135 launch_path = self.getPath(self.launch_file, self.package) 136 rospy.loginfo("loading launch file: %s", launch_path) 137 self.masteruri = self._masteruri_from_ros() 138 self.roscfg = ROSLaunchConfig() 139 loader = XmlLoader() 140 argv = self._filter_args(sys.argv) 141 # remove namespace from sys.argv to avoid load the launchfile info local namespace 142 sys.argv = list(argv) 143 # set the global environment to empty namespace 144 os.environ[ROS_NAMESPACE] = rospy.names.SEP 145 rospy.set_param('~argv_used', list(set(argv))) 146 loader.load(launch_path, self.roscfg, verbose=False, argv=argv) 147 # create the list with node names 148 for item in self.roscfg.nodes: 149 if item.machine_name and not item.machine_name == 'localhost': 150 machine = self.roscfg.machines[item.machine_name] 151 if roslib.network.is_local_address(machine.address): 152 self.nodes.append(roslib.names.ns_join(item.namespace, item.name)) 153 else: 154 self.nodes.append(roslib.names.ns_join(item.namespace, item.name)) 155 # get the robot description 156 self.description_response = dr = ListDescriptionResponse() 157 dr.robot_name = '' 158 dr.robot_type = '' 159 dr.robot_descr = '' 160 for param, p in self.roscfg.params.items(): 161 if param.endswith('robots'): 162 if isinstance(p.value, list): 163 if len(p.value) > 0 and len(p.value[0]) != 5: 164 print "WRONG format, expected: ['host(ROS master Name)', 'type', 'name', 'images', 'description'] -> ignore", param 165 else: 166 for entry in p.value: 167 try: 168 print entry[0], rospy.get_param('/mastername', '') 169 if not entry[0] or entry[0] == rospy.get_param('/mastername', ''): 170 dr.robot_name = self._decode(entry[2]) 171 dr.robot_type = entry[1] 172 dr.robot_images = entry[3].split(',') 173 dr.robot_descr = self._decode(entry[4]) 174 break 175 except: 176 pass 177 # get the sensor description 178 tmp_cap_dict = self.getCapabilitiesDesrc() 179 for machine, ns_dict in tmp_cap_dict.items(): 180 if machine in self.roscfg.machines: 181 machine = self.roscfg.machines[machine].address 182 if not machine or roslib.network.is_local_address(machine): 183 for ns, group_dict in ns_dict.items(): 184 for group, descr_dict in group_dict.items(): 185 if descr_dict['nodes']: 186 cap = Capability() 187 cap.namespace = ns 188 cap.name = group 189 cap.type = descr_dict['type'] 190 cap.images = list(descr_dict['images']) 191 cap.description = descr_dict['description'] 192 cap.nodes = list(descr_dict['nodes']) 193 dr.capabilities.append(cap) 194 # load parameters into the ROS parameter server 195 if self.load_params_at_start: 196 self.loadParams() 197 # initialize the ROS services 198 # HACK to let the node_manager to update the view 199 if delay_service_creation > 0.: 200 t = threading.Timer(delay_service_creation, self._timed_service_creation) 201 t.start() 202 else: 203 self._timed_service_creation() 204 # self.timer = rospy.Timer(rospy.Duration(2), self.timed_service_creation, True) 205 # if self.nodes: 206 # self.runService = rospy.Service('~run', Task, self.rosservice_start_node) 207 # self.listServic = rospy.Service('~list_nodes', ListNodes, self.rosservice_list_nodes) 208 # except: 209 # import traceback 210 # print traceback.format_exc() 211 if self.do_autostart: 212 if not self.parameter_loaded: 213 self.loadParams() 214 for n in self.nodes: 215 try: 216 self.runNode(n, self.do_autostart) 217 except Exception as e: 218 rospy.logwarn("Error while start %s: %s", n, e) 219 self.do_autostart = False

220

221 - def _decode(self, val):

222 ''' 223 Replaces the '\\n' by LF (Line Feed) and decode the string entry from system default 224 coding to unicode. 225 @param val: the string coding as system default 226 @type val: str 227 @return: the decoded string 228 @rtype: C{unicode} or original on error 229 ''' 230 result = val.replace("\\n ", "\n") 231 try: 232 result = result.decode(sys.getfilesystemencoding()) 233 except: 234 pass 235 return result

236

237 - def getCapabilitiesDesrc(self):

238 ''' 239 Parses the launch file for C{capabilities} and C{capability_group} parameter 240 and creates dictionary for grouping the nodes. 241 @return: the capabilities description stored in this configuration 242 @rtype: C{dict(machine : dict(namespace: dict(group:dict('type' : str, 'description' : str, 'nodes' : [str]))))} 243 ''' 244 result = dict() 245 capabilies_descr = dict() 246 if self.roscfg is not None: 247 # get the capabilities description 248 # use two separate loops, to create the description list first 249 for param, p in self.roscfg.params.items(): 250 if param.endswith('capabilities'): 251 if isinstance(p.value, list): 252 if len(p.value) > 0 and len(p.value[0]) != 4: 253 print "WRONG format, expected: ['name', 'type', 'images', 'description'] -> ignore", param 254 else: 255 for entry in p.value: 256 capabilies_descr[entry[0]] = {'type': ''.join([entry[1]]), 'images': entry[2].split(','), 'description': self._decode(entry[3])} 257 # get the capability nodes 258 for item in self.roscfg.nodes: 259 node_fullname = roslib.names.ns_join(item.namespace, item.name) 260 machine_name = item.machine_name if item.machine_name is not None and not item.machine_name == 'localhost' else '' 261 added = False 262 cap_param = roslib.names.ns_join(node_fullname, 'capability_group') 263 cap_ns = node_fullname 264 # find the capability group parameter in namespace 265 while cap_param not in self.roscfg.params and cap_param.count(roslib.names.SEP) > 1: 266 cap_ns = roslib.names.namespace(cap_ns).rstrip(roslib.names.SEP) 267 if not cap_ns: 268 cap_ns = roslib.names.SEP 269 cap_param = roslib.names.ns_join(cap_ns, 'capability_group') 270 if cap_ns == node_fullname: 271 cap_ns = item.namespace.rstrip(roslib.names.SEP) # if the parameter group parameter found, assign node to the group 272 if not cap_ns: 273 cap_ns = roslib.names.SEP 274 # if the 'capability_group' parameter found, assign node to the group 275 if cap_param in self.roscfg.params and self.roscfg.params[cap_param].value: 276 p = self.roscfg.params[cap_param] 277 if machine_name not in result: 278 result[machine_name] = dict() 279 for (ns, groups) in result[machine_name].items(): 280 if ns == cap_ns and p.value in groups: 281 groups[p.value]['nodes'].append(node_fullname) 282 added = True 283 break 284 if not added: 285 ns = cap_ns 286 # add new group in the namespace of the node 287 if ns not in result[machine_name]: 288 result[machine_name][ns] = dict() 289 if p.value not in result[machine_name][ns]: 290 try: 291 result[machine_name][ns][p.value] = {'type': capabilies_descr[p.value]['type'], 292 'images': capabilies_descr[p.value]['images'], 293 'description': capabilies_descr[p.value]['description'], 294 'nodes': []} 295 except: 296 result[machine_name][ns][p.value] = {'type': '', 297 'images': [], 298 'description': '', 299 'nodes': []} 300 result[machine_name][ns][p.value]['nodes'].append(node_fullname) 301 return result

302

303 - def _masteruri_from_ros(self):

304 ''' 305 Returns the master URI depending on ROS distribution API. 306 @return: ROS master URI 307 @rtype: C{str} 308 ''' 309 try: 310 import rospkg.distro 311 distro = rospkg.distro.current_distro_codename() 312 if distro in ['electric', 'diamondback', 'cturtle']: 313 return roslib.rosenv.get_master_uri() 314 else: 315 return rosgraph.rosenv.get_master_uri() 316 except: 317 return roslib.rosenv.get_master_uri()

318

319 - def _timed_service_creation(self):

320 with self.__lock: 321 try: 322 if self.runService is None: 323 self.runService = rospy.Service('~run', Task, self.rosservice_start_node) 324 if self.listService is None: 325 self.listService = rospy.Service('~list_nodes', ListNodes, self.rosservice_list_nodes) 326 except: 327 import traceback 328 print traceback.format_exc()

329

330 - def getPath(self, path, package=''):

331 ''' 332 Searches for a launch file. If package is given, try first to find the launch 333 file in the given package. If more then one launch file with the same name 334 found in the package, the first one will be tacked. 335 @param path: the file name of the launch file 336 @type path: C{str} 337 @param package: the package containing the launch file or an empty string, 338 if the C{file} is an absolute path 339 @type package: C{str} 340 @return: the absolute path of the launch file 341 @rtype: C{str} 342 @raise LoadException: if the given file is not found 343 ''' 344 launch_file = path 345 # if package is set, try to find the launch file in the given package 346 if package: 347 paths = roslib.packages.find_resource(package, launch_file) 348 if len(paths) > 0: 349 # if more then one launch file is found, take the first one 350 launch_file = paths[0] 351 if os.path.isfile(launch_file) and os.path.exists(launch_file): 352 return launch_file 353 raise LoadException('File %s in package [%s] not found!' % (path, package))

354

355 - def rosservice_list_nodes(self, req):

356 ''' 357 Callback for the ROS service to get the list with available nodes. 358 ''' 359 return ListNodesResponse(self.nodes)

360

361 - def rosservice_start_node(self, req):

362 ''' 363 Callback for the ROS service to start a node. 364 ''' 365 self.runNode(req.node) 366 return []

367

368 - def rosservice_reload(self, req):

369 self.load(2.) 370 return []

371 372 # def rosservice_load_launch(self, req): 373 # ''' 374 # Load the launch file 375 # ''' 376 # try: 377 # self.__lock.acquire() 378 # self.load(req.package, req.file, req.argv) 379 # finally: 380 # self.__lock.release() 381 # return [] 382

383 - def rosservice_description(self, req):

384 ''' 385 Returns the current description. 386 ''' 387 return self.description_response

388

389 - def loadParams(self):

390 ''' 391 Loads all parameter into ROS parameter server. 392 ''' 393 params = dict() 394 for param, value in self.roscfg.params.items(): 395 params[param] = value 396 # rospy.loginfo("register PARAMS:\n%s", '\n'.join(params)) 397 self._load_parameters(self.masteruri, params, self.roscfg.clear_params) 398 self.parameter_loaded = True

399

400 - def runNode(self, node, autostart=False):

401 ''' 402 Start the node with given name from the currently loaded configuration. 403 @param node: the name of the node 404 @type node: C{str} 405 @raise StartException: if an error occurred while start. 406 ''' 407 if not self.parameter_loaded: 408 self.loadParams() 409 n = None 410 for item in self.roscfg.nodes: 411 itemname = rospy.names.ns_join(item.namespace, item.name) 412 if itemname == node: 413 n = item 414 break 415 if n is None: 416 raise StartException("Node '%s' not found!" % node) 417 418 if autostart and self._get_start_exclude(rospy.names.ns_join(n.namespace, n.name)): 419 # skip autostart 420 rospy.loginfo("%s is in exclude list, skip autostart", n.name) 421 return 422 423 # env = n.env_args 424 prefix = n.launch_prefix if n.launch_prefix is not None else '' 425 args = ['__ns:=%s' % n.namespace, '__name:=%s' % n.name] 426 if not (n.cwd is None): 427 args.append('__cwd:=%s' % n.cwd) 428 429 # add remaps 430 for remap in n.remap_args: 431 args.append('%s:=%s' % (remap[0], remap[1])) 432 433 # masteruri = self.masteruri 434 435 # if n.machine_name and not n.machine_name == 'localhost': 436 # machine = self.roscfg.machines[n.machine_name] 437 # TODO: env-loader support? 438 # if machine.env_args: 439 # env[len(env):] = machine.env_args 440 441 # nm.screen().testScreen() 442 cmd = self._get_node(n.package, n.type) 443 # determine the current working path, Default: the package of the node 444 cwd = self.get_ros_home() 445 if not (n.cwd is None): 446 if n.cwd == 'ROS_HOME': 447 cwd = self.get_ros_home() 448 elif n.cwd == 'node': 449 cwd = os.path.dirname(cmd[0]) 450 respawn = [''] 451 if n.respawn: 452 respawn = self._get_node('node_manager_fkie', 'respawn') 453 # set the respawn environment variables 454 respawn_params = self._get_respawn_params(rospy.names.ns_join(n.namespace, n.name)) 455 if respawn_params['max'] > 0: 456 n.env_args.append(('RESPAWN_MAX', '%d' % respawn_params['max'])) 457 if respawn_params['min_runtime'] > 0: 458 n.env_args.append(('RESPAWN_MIN_RUNTIME', '%d' % respawn_params['min_runtime'])) 459 if respawn_params['delay'] > 0: 460 n.env_args.append(('RESPAWN_DELAY', '%d' % respawn_params['delay'])) 461 node_cmd = [respawn[0], prefix, cmd[0]] 462 cmd_args = [ScreenHandler.getSceenCmd(node)] 463 cmd_args[len(cmd_args):] = node_cmd 464 cmd_args.append(n.args) 465 cmd_args[len(cmd_args):] = args 466 # print 'runNode: ', cmd_args 467 popen_cmd = shlex.split(str(' '.join(cmd_args))) 468 rospy.loginfo("run node '%s as': %s", node, str(' '.join(popen_cmd))) 469 # remove the 'BASH_ENV' and 'ENV' from environment 470 new_env = dict(os.environ) 471 try: 472 for k in ['BASH_ENV', 'ENV']: 473 del new_env[k] 474 except: 475 pass 476 # add node environment parameter 477 for k, v in n.env_args: 478 new_env[k] = v 479 # the ROS_NAMESPACE environment is used in cpp plugins in rqt 480 if n.namespace: 481 new_env['ROS_NAMESPACE'] = n.namespace 482 # set delayed autostart parameter 483 self._run_node(popen_cmd, cwd, new_env, rospy.names.ns_join(n.namespace, n.name), autostart) 484 if len(cmd) > 1: 485 raise StartException('Multiple executables are found! The first one was started! Exceutables:\n%s' % str(cmd))

486

487 - def _run_node(self, cmd, cwd, env, node, autostart=False):

488 self._pending_starts.add(node) 489 start_now = True 490 start_delay = self._get_start_delay(node) 491 start_required = self._get_start_required(node) 492 if autostart and start_required: 493 start_now = False 494 # get published topics from ROS master 495 master = rosgraph.masterapi.Master(self.masteruri) 496 for topic, datatype in master.getPublishedTopics(''): 497 if start_required == topic: 498 start_now = True 499 break 500 if not start_now: 501 # Start the timer for waiting for the topic 502 start_timer = threading.Timer(3., self._run_node, args=(cmd, cwd, env, node, autostart)) 503 start_timer.start() 504 if start_now and autostart and start_delay > 0: 505 start_now = False 506 # start timer for delayed start 507 start_timer = threading.Timer(start_delay, self._run_node, args=(cmd, cwd, env, node, False)) 508 start_timer.start() 509 if start_now: 510 ps = subprocess.Popen(cmd, cwd=cwd, env=env) 511 # wait for process to avoid 'defunct' processes 512 thread = threading.Thread(target=ps.wait) 513 thread.setDaemon(True) 514 thread.start() 515 # remove from pending autostarts 516 try: 517 self._pending_starts.remove(node) 518 except: 519 pass 520 # print the current pending autostarts 521 if self._pending_starts_last_printed != self._pending_starts: 522 self._pending_starts_last_printed.clear() 523 self._pending_starts_last_printed.update(self._pending_starts) 524 rospy.loginfo("Pending autostarts %d: %s", len(self._pending_starts), self._pending_starts)

525

526 - def _get_node(self, pkg, filename):

527 cmd = None 528 try: 529 cmd = roslib.packages.find_node(pkg, filename) 530 except roslib.packages.ROSPkgException as e: 531 # multiple nodes, invalid package 532 raise StartException(str(e)) 533 except Exception as e: 534 raise StartException(str(e)) 535 # handle different result types str or array of string 536 import types 537 if isinstance(cmd, types.StringTypes): 538 cmd = [cmd] 539 if cmd is None or len(cmd) == 0: 540 raise StartException('%s in package [%s] not found!' % (filename, pkg)) 541 return cmd

542

543 - def _get_start_exclude(self, node):

544 param_name = rospy.names.ns_join(node, 'default_cfg/autostart/exclude') 545 try: 546 return bool(self.roscfg.params[param_name].value) 547 except: 548 pass 549 return False

550

551 - def _get_start_delay(self, node):

552 param_name = rospy.names.ns_join(node, 'default_cfg/autostart/delay') 553 try: 554 return float(self.roscfg.params[param_name].value) 555 except: 556 pass 557 return 0.

558

559 - def _get_start_required(self, node):

560 param_name = rospy.names.ns_join(node, 'default_cfg/autostart/required/publisher') 561 topic = '' 562 try: 563 topic = self.roscfg.params[param_name].value 564 if rosgraph.names.is_private(topic): 565 rospy.logwarn('Private for autostart required topic `%s` is ignored!' % topic) 566 topic = '' 567 elif not rosgraph.names.is_global(topic): 568 topic = rospy.names.ns_join(rosgraph.names.namespace(node), topic) 569 except: 570 pass 571 return topic

572

573 - def _get_respawn_params(self, node):

574 result = {'max': 0, 'min_runtime': 0, 'delay': 0} 575 respawn_max = rospy.names.ns_join(node, 'respawn/max') 576 respawn_min_runtime = rospy.names.ns_join(node, 'respawn/min_runtime') 577 respawn_delay = rospy.names.ns_join(node, 'respawn/delay') 578 try: 579 result['max'] = int(self.roscfg.params[respawn_max].value) 580 except: 581 pass 582 try: 583 result['min_runtime'] = int(self.roscfg.params[respawn_min_runtime].value) 584 except: 585 pass 586 try: 587 result['delay'] = int(self.roscfg.params[respawn_delay].value) 588 except: 589 pass 590 return result

591

592 - def get_ros_home(self):

593 ''' 594 Returns the ROS HOME path depending on ROS distribution API. 595 @return: ROS HOME path 596 @rtype: C{str} 597 ''' 598 try: 599 import rospkg.distro 600 distro = rospkg.distro.current_distro_codename() 601 if distro in ['electric', 'diamondback', 'cturtle']: 602 import roslib.rosenv 603 return roslib.rosenv.get_ros_home() 604 else: 605 import rospkg 606 return rospkg.get_ros_home() 607 except: 608 import traceback 609 print traceback.format_exc() 610 import roslib.rosenv 611 return roslib.rosenv.get_ros_home()

612 613 @classmethod

614 - def _load_parameters(cls, masteruri, params, clear_params):

615 """ 616 Load parameters onto the parameter server 617 """ 618 import xmlrpclib 619 param_server = xmlrpclib.ServerProxy(masteruri) 620 p = None 621 try: 622 # multi-call style xmlrpc 623 param_server_multi = xmlrpclib.MultiCall(param_server) 624 625 # clear specified parameter namespaces 626 # #2468 unify clear params to prevent error 627 for p in clear_params: 628 param_server_multi.deleteParam(rospy.get_name(), p) 629 r = param_server_multi() 630 # for code, msg, _ in r: 631 # if code != 1: 632 # raise StartException("Failed to clear parameter: %s"%(msg)) 633 634 # multi-call objects are not reusable 635 param_server_multi = xmlrpclib.MultiCall(param_server) 636 for p in params.itervalues(): 637 # suppressing this as it causes too much spam 638 # printlog("setting parameter [%s]"%p.key) 639 param_server_multi.setParam(rospy.get_name(), p.key, p.value) 640 r = param_server_multi() 641 for code, msg, _ in r: 642 if code != 1: 643 raise StartException("Failed to set parameter: %s" % (msg)) 644 except Exception: 645 raise # re-raise as this is fatal

Source Code for Module default_cfg_fkie.default_cfg