1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33 from multimaster_msgs_fkie.msg import Capability
34 from multimaster_msgs_fkie.srv import ListDescription, ListNodes, Task, ListDescriptionResponse, ListNodesResponse
35 from rosgraph.rosenv import ROS_NAMESPACE
36 from roslaunch import ROSLaunchConfig, XmlLoader
37 import os
38 import rosgraph.masterapi
39 import rosgraph.names
40 import roslib.names
41 import roslib.network
42 import rospy
43 import shlex
44 import std_srvs.srv
45 import subprocess
46 import sys
47 import threading
48
49 from screen_handler import ScreenHandler
53 ''' The exception throwing while searching for the given launch file. '''
54 pass
55
58 ''' The exception throwing while run a node containing in the loaded configuration. '''
59 pass
60
63
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
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
92
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
103 self._pending_starts = set()
104 self._pending_starts_last_printed = set()
105
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
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 = []
134 self.sensors = {}
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
142 sys.argv = list(argv)
143
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
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
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
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
195 if self.load_params_at_start:
196 self.loadParams()
197
198
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
205
206
207
208
209
210
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
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
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
248
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
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
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)
272 if not cap_ns:
273 cap_ns = roslib.names.SEP
274
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
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
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
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
346 if package:
347 paths = roslib.packages.find_resource(package, launch_file)
348 if len(paths) > 0:
349
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
356 '''
357 Callback for the ROS service to get the list with available nodes.
358 '''
359 return ListNodesResponse(self.nodes)
360
362 '''
363 Callback for the ROS service to start a node.
364 '''
365 self.runNode(req.node)
366 return []
367
369 self.load(2.)
370 return []
371
372
373
374
375
376
377
378
379
380
381
382
384 '''
385 Returns the current description.
386 '''
387 return self.description_response
388
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
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
420 rospy.loginfo("%s is in exclude list, skip autostart", n.name)
421 return
422
423
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
430 for remap in n.remap_args:
431 args.append('%s:=%s' % (remap[0], remap[1]))
432
433
434
435
436
437
438
439
440
441
442 cmd = self._get_node(n.package, n.type)
443
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
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
467 popen_cmd = shlex.split(str(' '.join(cmd_args)))
468 rospy.loginfo("run node '%s as': %s", node, str(' '.join(popen_cmd)))
469
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
477 for k, v in n.env_args:
478 new_env[k] = v
479
480 if n.namespace:
481 new_env['ROS_NAMESPACE'] = n.namespace
482
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
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
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
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
512 thread = threading.Thread(target=ps.wait)
513 thread.setDaemon(True)
514 thread.start()
515
516 try:
517 self._pending_starts.remove(node)
518 except:
519 pass
520
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
527 cmd = None
528 try:
529 cmd = roslib.packages.find_node(pkg, filename)
530 except roslib.packages.ROSPkgException as e:
531
532 raise StartException(str(e))
533 except Exception as e:
534 raise StartException(str(e))
535
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
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
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
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
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
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
615 """
616 Load parameters onto the parameter server
617 """
618 import xmlrpclib
619 param_server = xmlrpclib.ServerProxy(masteruri)
620 p = None
621 try:
622
623 param_server_multi = xmlrpclib.MultiCall(param_server)
624
625
626
627 for p in clear_params:
628 param_server_multi.deleteParam(rospy.get_name(), p)
629 r = param_server_multi()
630
631
632
633
634
635 param_server_multi = xmlrpclib.MultiCall(param_server)
636 for p in params.itervalues():
637
638
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
646