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
34
35 """
36 Defines the L{ROSLaunchConfig} object, which holds and the state of
37 the roslaunch file.
38 """
39
40 import os
41 import logging
42 import types
43
44 import rospkg
45 import rospkg.distro
46 import rosgraph.names
47 import rosgraph.network
48
49 from .core import Master, local_machine, is_machine_local, RLException
50 import roslaunch.loader
51 import roslaunch.xmlloader
52
53 try:
54 from rosmaster import DEFAULT_MASTER_PORT
55 except:
56 DEFAULT_MASTER_PORT = 11311
57
59 """
60 utility to determine namespaces of a name
61 @raises ValueError
62 @raises TypeError
63 """
64 if name is None:
65 raise ValueError('name')
66 try:
67 if not isinstance(name, basestring):
68 raise TypeError('name')
69 except NameError:
70 if not isinstance(name, str):
71 raise TypeError('name')
72 if not name:
73 return ['/']
74
75 splits = [x for x in name.split('/') if x]
76 return ['/'] + ['/'+'/'.join(splits[:i]) for i in range(1, len(splits))]
77
79
80 filename = os.path.join(rospkg.get_etc_ros_dir(), 'roscore.xml')
81 if os.path.isfile(filename):
82 return filename
83 r = rospkg.RosPack()
84 return os.path.join(r.get_path('roslaunch'), 'resources', 'roscore.xml')
85
87 """
88 Load roscore configuration into the ROSLaunchConfig using the specified XmlLoader
89 @param config ROSLaunchConfig
90 @param loader XmlLoader
91 """
92 f_roscore = get_roscore_filename()
93 logging.getLogger('roslaunch').info('loading roscore config file %s'%f_roscore)
94 loader.load(f_roscore, config, core=True, verbose=verbose)
95
97 """
98 @raise RLException
99 """
100 if env is None:
101 env = os.environ
102
103 distro_name = rospkg.distro.current_distro_codename()
104
105 if distro_name in ['electric', 'diamondback', 'cturtle']:
106 raise RLException("This version of roslaunch is not compatible with pre-Fuerte ROS distributions")
107 return '/opt/ros/%s/env.sh'%(distro_name)
108
110 """
111 Generate summary label for node based on its package, type, and name
112 """
113 if node.name:
114 return "%s (%s/%s)"%(node.name, node.package, node.type)
115 else:
116 return "%s/%s"%(node.package, node.type)
117
119 """
120 ROSLaunchConfig is the container for the loaded roslaunch file state. It also
121 is responsible for validating then executing the desired state.
122 """
123
125 """
126 Initialize an empty config object. Master defaults to the environment's master.
127 """
128 self.master = Master()
129 self.nodes_core = []
130 self.nodes = []
131
132 self.roslaunch_files = []
133
134
135 self.resolved_node_names = []
136
137 self.tests = []
138 self.machines = {}
139 self.params = {}
140 self.clear_params = []
141 self.executables = []
142
143
144 self.config_errors = []
145
146 m = local_machine()
147 self.machines[m.name] = m
148 self._assign_machines_complete = False
149 self._remote_nodes_present = None
150
151 self.logger = logging.getLogger('roslaunch')
152
154 """
155 Add metadata about file used to create config
156 """
157 self.roslaunch_files.append(f)
158
160 """
161 Report human-readable error message related to configuration error
162 @param msg: error message
163 @type msg: str
164 """
165 self.config_errors.append(msg)
166
168 """
169 Set the master configuration
170 @param m: Master
171 @type m: L{Master}
172 """
173 self.master = m
174
176 """
177 @return: True if roslaunch will launch nodes on a remote machine
178 @rtype: bool
179 @raises: RLException
180 """
181 if not self._assign_machines_complete:
182 raise RLException("ERROR: has_remote_nodes() cannot be called until prelaunch check is complete")
183 return self._remote_nodes_present
184
186 """
187 Assign nodes to machines and determine whether or not there are any remote machines
188 """
189
190 if self._assign_machines_complete:
191 return
192
193 machine_unify_dict = {}
194
195 self._assign_machines_complete = True
196
197 local_machine = self.machines['']
198 for n in self.nodes_core:
199 n.machine = local_machine
200
201
202 for n in self.nodes + self.tests:
203 m = self._select_machine(n)
204
205
206
207
208 config_key = m.config_key()
209 if config_key in machine_unify_dict:
210 new_m = machine_unify_dict[config_key]
211 if m != new_m:
212 self.logger.info("... changing machine assignment from [%s] to [%s] as they are equivalent", m.name, new_m.name)
213 m = new_m
214 else:
215 machine_unify_dict[config_key] = m
216 n.machine = m
217 self.logger.info("... selected machine [%s] for node of type [%s/%s]", m.name, n.package, n.type)
218
219
220
221 self._remote_nodes_present = False
222 if [m for m in machine_unify_dict.values() if not is_machine_local(m)]:
223 self._remote_nodes_present = True
224
226 """
227 Get a human-readable string summary of the launch
228 @param local bool: if True, only print local nodes
229 @return: summary
230 @rtype: str
231 """
232 summary = '\nSUMMARY\n========'
233 if self.clear_params:
234 summary += '\n\nCLEAR PARAMETERS\n' + '\n'.join(sorted([' * %s'%p for p in self.clear_params]))
235 if self.params:
236 def strip_string(value):
237
238 try:
239 value = str(value)
240 except UnicodeEncodeError:
241 return '<...>'
242 max_length = 20
243 if len(value) > max_length:
244 value = value[:max_length - 3] + '...'
245
246 for i, char in enumerate(value):
247 o = ord(char)
248 if o < 32 or o > 126:
249
250 value = value.rstrip()
251 if i >= len(value):
252 break
253 return '<...>'
254 return value
255 summary += '\n\nPARAMETERS\n' + '\n'.join(sorted([' * %s: %s' % (k, strip_string(v.value)) for k, v in self.params.items()]))
256 if not local:
257 summary += '\n\nMACHINES\n' + '\n'.join(sorted([' * %s'%k for k in self.machines if k]))
258 summary += '\n\nNODES\n'
259 namespaces = {}
260 if local:
261 nodes = [n for n in self.nodes if is_machine_local(n.machine)]
262 else:
263 nodes = self.nodes
264 for n in nodes:
265 ns = n.namespace
266 if ns not in namespaces:
267 namespaces[ns] = [n]
268 else:
269 namespaces[ns].append(n)
270 for k,v in namespaces.items():
271 summary += ' %s\n'%k + '\n'.join(sorted([' %s'%_summary_name(n) for n in v]))
272 summary += '\n'
273 return summary
274
276 """
277 Declare an exectuable to be run during the launch
278 @param exe: Executable
279 @type exe: L{Executable}
280 @raises ValueError
281 """
282 if not exe:
283 raise ValueError("exe is None")
284 self.executables.append(exe)
285
287 """
288 Declare a parameter to be cleared before new parameters are set
289 @param param: parameter to clear
290 @type param: str
291 """
292 self.clear_params.append(param)
293
294 - def add_param(self, p, filename=None, verbose=True):
295 """
296 Declare a parameter to be set on the param server before launching nodes
297 @param p: parameter instance
298 @type p: L{Param}
299 """
300 key = p.key
301
302
303 if key in self.params and self.params[key] != p:
304 if filename:
305 self.logger.debug("[%s] overriding parameter [%s]"%(filename, p.key))
306 else:
307 self.logger.debug("overriding parameter [%s]"%p.key)
308
309 for parent_key in [pk for pk in namespaces_of(key) if pk in self.params]:
310 self.add_config_error("parameter [%s] conflicts with parent parameter [%s]"%(key, parent_key))
311
312 self.params[key] = p
313 if verbose:
314 print("Added parameter [%s]" % key)
315 t = type(p.value)
316 if t in [bool, int, float]:
317 self.logger.debug("add_param[%s]: type [%s] value [%s]"%(p.key, t, p.value))
318 else:
319 self.logger.debug("add_param[%s]: type [%s]"%(p.key, t))
320
322 """
323 Declare a machine and associated parameters so that it can be used for
324 running nodes.
325 @param m: machine instance
326 @type m: L{Machine}
327 @return: True if new machine added, False if machine already specified.
328 @rtype: bool
329 @raises RLException: if cannot add machine as specified
330 """
331 name = m.name
332
333
334 if not m.env_loader:
335 m.env_loader = calculate_env_loader()
336 if m.address == 'localhost':
337 address = rosgraph.network.get_local_address()
338 self.logger.info("addMachine[%s]: remapping localhost address to %s"%(name, address))
339 if name in self.machines:
340 if m != self.machines[name]:
341 raise RLException("Machine [%s] already added and does not match duplicate entry"%name)
342 return False
343 else:
344 self.machines[name] = m
345 if verbose:
346 print("Added machine [%s]" % name)
347 return True
348
349 - def add_test(self, test, verbose=True):
350 """
351 Add test declaration. Used by rostest
352 @param test: test node instance to add to launch
353 @type test: L{Test}
354 """
355 self.tests.append(test)
356
357 - def add_node(self, node, core=False, verbose=True):
358 """
359 Add node declaration
360 @param node: node instance to add to launch
361 @type node: L{Node}
362 @param core: if True, node is a ROS core node
363 @type core: bool
364 @raises RLException: if ROS core node is missing required name
365 """
366 if node.name:
367
368 resolved_name = rosgraph.names.ns_join(node.namespace, node.name)
369 matches = [n for n in self.resolved_node_names if n == resolved_name]
370 if matches:
371 raise RLException("roslaunch file contains multiple nodes named [%s].\nPlease check all <node> 'name' attributes to make sure they are unique.\nAlso check that $(anon id) use different ids."%resolved_name)
372 else:
373 self.resolved_node_names.append(resolved_name)
374
375 if not core:
376 self.nodes.append(node)
377 if verbose:
378 print("Added node of type [%s/%s] in namespace [%s]" % (node.package, node.type, node.namespace))
379 self.logger.info("Added node of type [%s/%s] in namespace [%s]", node.package, node.type, node.namespace)
380 else:
381 if not node.name:
382 raise RLException("ROS core nodes must have a name. [%s/%s]"%(node.package, node.type))
383 self.nodes_core.append(node)
384 if verbose:
385 print("Added core node of type [%s/%s] in namespace [%s]" % (node.package, node.type, node.namespace))
386 self.logger.info("Added core node of type [%s/%s] in namespace [%s]", node.package, node.type, node.namespace)
387
389 """
390 Select a machine for a node to run on. For nodes that are
391 already assigned to a machine, this will map the string name to
392 a L{Machine} instance. If the node isn't already tagged with a
393 particular machine, one will be selected for it.
394 @param node: node to assign machine for
395 @type node: L{Node}
396 @return: machine to run on
397 @rtype: L{Machine}
398 @raises RLException: If machine state is improperly configured
399 """
400 machine = node.machine_name
401
402 if machine:
403 if not machine in self.machines:
404 raise RLException("ERROR: unknown machine [%s]"%machine)
405 return self.machines[machine]
406 else:
407
408 return self.machines['']
409
410 -def load_config_default(roslaunch_files, port, roslaunch_strs=None, loader=None, verbose=False, assign_machines=True):
411 """
412 Base routine for creating a ROSLaunchConfig from a set of
413 roslaunch_files and or launch XML strings and initializing it. This
414 config will have a core definition and also set the master to run
415 on port.
416 @param roslaunch_files: list of launch files to load
417 @type roslaunch_files: [str]
418 @param port: roscore/master port override. Set to 0 or None to use default.
419 @type port: int
420 @param roslaunch_strs: (optional) roslaunch XML strings to load
421 @type roslaunch_strs: [str]
422 @param verbose: (optional) print info to screen about model as it is loaded.
423 @type verbose: bool
424 @param assign_machines: (optional) assign nodes to machines (default: True)
425 @type assign_machines: bool
426 @return: initialized rosconfig instance
427 @rytpe: L{ROSLaunchConfig} initialized rosconfig instance
428 @raises: RLException
429 """
430 logger = logging.getLogger('roslaunch.config')
431
432
433
434
435
436 config = ROSLaunchConfig()
437 if port:
438 config.master.uri = rosgraph.network.create_local_xmlrpc_uri(port)
439
440 loader = loader or roslaunch.xmlloader.XmlLoader()
441
442
443
444
445 load_roscore(loader, config, verbose=verbose)
446
447
448 for f in roslaunch_files:
449 try:
450 logger.info('loading config file %s'%f)
451 loader.load(f, config, verbose=verbose)
452 except roslaunch.xmlloader.XmlParseException as e:
453 raise RLException(e)
454 except roslaunch.loader.LoadException as e:
455 raise RLException(e)
456
457
458
459 if roslaunch_strs:
460 for launch_str in roslaunch_strs:
461 try:
462 logger.info('loading config file from string')
463 loader.load_string(launch_str, config)
464 except roslaunch.xmlloader.XmlParseException as e:
465 raise RLException('Launch string: %s\nException: %s'%(launch_str, e))
466 except roslaunch.loader.LoadException as e:
467 raise RLException('Launch string: %s\nException: %s'%(launch_str, e))
468
469
470 if assign_machines:
471 config.assign_machines()
472 return config
473