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 Additional ROS client API methods.
37 """
38
39 import logging
40 import os
41 import socket
42 import struct
43 import sys
44 import time
45 import random
46 import yaml
47
48 import rosgraph
49 import rosgraph.names
50
51 import roslib
52
53 import rospy.core
54 from rospy.core import logwarn, loginfo, logerr, logdebug
55 import rospy.exceptions
56 import rospy.names
57 import rospy.rostime
58
59 import rospy.impl.init
60 import rospy.impl.rosout
61 import rospy.impl.simtime
62
63 TIMEOUT_READY = 15.0
64
65
66 from rosgraph_msgs.msg import Log
67 from roscpp.srv import GetLoggers, GetLoggersResponse, SetLoggerLevel, SetLoggerLevelResponse
68 from roscpp.msg import Logger
69 from rospy.impl.tcpros_service import Service
70 DEBUG = Log.DEBUG
71 INFO = Log.INFO
72 WARN = Log.WARN
73 ERROR = Log.ERROR
74 FATAL = Log.FATAL
75
77 """
78 Remove ROS remapping arguments from sys.argv arguments.
79 @return: copy of sys.argv with ROS remapping arguments removed
80 @rtype: [str]
81 """
82 if argv is None:
83 argv = sys.argv
84 return [a for a in argv if not rosgraph.names.REMAP in a]
85
87 """
88 Load node param mappings (aka private parameters) encoded in
89 command-line arguments, e.g. _foo:=bar. See also rosgraph.names.load_mappings.
90 @param argv: command-line arguments
91 @param argv: [str]
92 @return: param->value remappings.
93 @rtype: {str: val}
94 @raises: ROSInitException
95 """
96 try:
97 mappings = {}
98 for arg in argv:
99 if rosgraph.names.REMAP in arg:
100 src, dst = [x.strip() for x in arg.split(rosgraph.names.REMAP)]
101 if src and dst:
102 if len(src) > 1 and src[0] == '_' and src[1] != '_':
103 mappings[src[1:]] = yaml.load(dst)
104 return mappings
105 except Exception as e:
106 raise rospy.exceptions.ROSInitException("invalid command-line parameters: %s"%(str(e)))
107
109 """
110 Register function to be called on shutdown. This function will be
111 called before Node begins teardown.
112 @param h: Function with zero args to be called on shutdown.
113 @type h: fn()
114 """
115 rospy.core.add_client_shutdown_hook(h)
116
132
133 _logging_level_names = {
134 logging.DEBUG: 'DEBUG',
135 logging.INFO: 'INFO',
136 logging.WARNING: 'WARN',
137 logging.ERROR: 'ERROR',
138 logging.CRITICAL: 'FATAL',
139 }
140
142 """
143 ROS service handler to get the levels of all active loggers.
144 """
145 ret = GetLoggersResponse()
146 for n in logging.Logger.manager.loggerDict.keys():
147 level = logging.getLogger(n).getEffectiveLevel()
148 level = _logging_level_names[level]
149 ret.loggers.append(Logger(n, level))
150 return ret
151
152 _names_to_logging_levels = {
153 'DEBUG': logging.DEBUG,
154 'INFO': logging.INFO,
155 'WARN': logging.WARNING,
156 'ERROR': logging.ERROR,
157 'FATAL': logging.CRITICAL,
158 }
159
161 """
162 ROS service handler to set the logging level for a particular logger
163 """
164 level = request.level.upper()
165 if level in _names_to_logging_levels:
166 logger = logging.getLogger(request.logger)
167 logger.setLevel(_names_to_logging_levels[level])
168 else:
169 logging.getLogger('rospy').error("Bad logging level: %s"%level)
170 ret = SetLoggerLevelResponse()
171 return ret
172
174 """
175 Uploads private params to the parameter server. Private params are specified
176 via command-line remappings.
177
178 @raises: ROSInitException
179 """
180
181
182 params = load_command_line_node_params(argv)
183 for param_name, param_value in params.items():
184 logdebug("setting param %s to %s"%(param_name, param_value))
185 set_param(rosgraph.names.PRIV_NAME + param_name, param_value)
186
187 _init_node_args = None
188
189 -def init_node(name, argv=None, anonymous=False, log_level=None, disable_rostime=False, disable_rosout=False, disable_signals=False):
190 """
191 Register client node with the master under the specified name.
192 This MUST be called from the main Python thread unless
193 disable_signals is set to True. Duplicate calls to init_node are
194 only allowed if the arguments are identical as the side-effects of
195 this method are not reversible.
196
197 @param name: Node's name. This parameter must be a base name,
198 meaning that it cannot contain namespaces (i.e. '/')
199 @type name: str
200
201 @param argv: Command line arguments to this program, including
202 remapping arguments (default: sys.argv). If you provide argv
203 to init_node(), any previously created rospy data structure
204 (Publisher, Subscriber, Service) will have invalid
205 mappings. It is important that you call init_node() first if
206 you wish to provide your own argv.
207 @type argv: [str]
208
209 @param anonymous: if True, a name will be auto-generated for the
210 node using name as the base. This is useful when you wish to
211 have multiple instances of the same node and don't care about
212 their actual names (e.g. tools, guis). name will be used as
213 the stem of the auto-generated name. NOTE: you cannot remap
214 the name of an anonymous node.
215 @type anonymous: bool
216
217 @param log_level: log level for sending message to /rosout and log
218 file, which is INFO by default. For convenience, you may use
219 rospy.DEBUG, rospy.INFO, rospy.ERROR, rospy.WARN, rospy.FATAL
220 @type log_level: int
221
222 @param disable_signals: If True, rospy will not register its own
223 signal handlers. You must set this flag if (a) you are unable
224 to call init_node from the main thread and/or you are using
225 rospy in an environment where you need to control your own
226 signal handling (e.g. WX). If you set this to True, you should
227 call rospy.signal_shutdown(reason) to initiate clean shutdown.
228
229 NOTE: disable_signals is overridden to True if
230 roslib.is_interactive() is True.
231
232 @type disable_signals: bool
233
234 @param disable_rostime: for internal testing only: suppresses
235 automatic subscription to rostime
236 @type disable_rostime: bool
237
238 @param disable_rosout: for internal testing only: suppress
239 auto-publication of rosout
240 @type disable_rostime: bool
241
242 @raise ROSInitException: if initialization/registration fails
243 @raise ValueError: if parameters are invalid (e.g. name contains a namespace or is otherwise illegal)
244 """
245 if argv is None:
246 argv = sys.argv
247 else:
248
249
250 rospy.names.reload_mappings(argv)
251
252
253 if rosgraph.names.SEP in name:
254 raise ValueError("namespaces are not allowed in node names")
255 if not rosgraph.names.is_legal_base_name(name):
256 import warnings
257 warnings.warn("'%s' is not a legal ROS base name. This may cause problems with other ROS tools"%name, stacklevel=2)
258
259 global _init_node_args
260
261
262
263 if _init_node_args:
264 if _init_node_args != (name, argv, anonymous, log_level, disable_rostime, disable_signals):
265 raise rospy.exceptions.ROSException("rospy.init_node() has already been called with different arguments: "+str(_init_node_args))
266 else:
267 return
268
269
270
271 disable_signals = disable_signals or roslib.is_interactive()
272 _init_node_args = (name, argv, anonymous, log_level, disable_rostime, disable_signals)
273
274 if not disable_signals:
275
276 rospy.core.register_signals()
277 else:
278 logdebug("signal handlers for rospy disabled")
279
280
281 mappings = rospy.names.get_mappings()
282 if '__name' in mappings:
283
284 name = rosgraph.names.resolve_name(mappings['__name'], rospy.core.get_caller_id())
285 if anonymous:
286 logdebug("[%s] WARNING: due to __name setting, anonymous setting is being changed to false"%name)
287 anonymous = False
288
289 if anonymous:
290
291
292 name = "%s_%s_%s"%(name, os.getpid(), int(time.time()*1000))
293
294 resolved_node_name = rospy.names.resolve_name(name)
295 rospy.core.configure_logging(resolved_node_name)
296
297 rospy.names.initialize_mappings(resolved_node_name)
298
299 logger = logging.getLogger("rospy.client")
300 logger.info("init_node, name[%s], pid[%s]", resolved_node_name, os.getpid())
301
302
303 node = rospy.impl.init.start_node(os.environ, resolved_node_name)
304 rospy.core.set_node_uri(node.uri)
305 rospy.core.add_shutdown_hook(node.shutdown)
306
307 if rospy.core.is_shutdown():
308 logger.warn("aborting node initialization as shutdown has been triggered")
309 raise rospy.exceptions.ROSInitException("init_node interrupted before it could complete")
310
311
312 _init_node_params(argv, name)
313
314 rospy.core.set_initialized(True)
315
316 if not disable_rosout:
317 rospy.impl.rosout.init_rosout()
318 rospy.impl.rosout.load_rosout_handlers(log_level)
319
320 if not disable_rostime:
321 if not rospy.impl.simtime.init_simtime():
322 raise rospy.exceptions.ROSInitException("Failed to initialize time. Please check logs for additional details")
323 else:
324 rospy.rostime.set_rostime_initialized(True)
325
326 logdebug("init_node, name[%s], pid[%s]", resolved_node_name, os.getpid())
327
328 Service('~get_loggers', GetLoggers, _get_loggers)
329 Service('~set_logger_level', SetLoggerLevel, _set_logger_level)
330
331
332
333 _master_proxy = None
334
336 """
337 Get a remote handle to the ROS Master.
338 This method can be called independent of running a ROS node,
339 though the ROS_MASTER_URI must be declared in the environment.
340
341 @return: ROS Master remote object
342 @rtype: L{rospy.MasterProxy}
343 @raise Exception: if server cannot be located or system cannot be
344 initialized
345 """
346 global _master_proxy
347 if _master_proxy is not None:
348 return _master_proxy
349 _master_proxy = rospy.msproxy.MasterProxy(rosgraph.get_master_uri())
350 return _master_proxy
351
352
353
354
356 """
357 Retrieve list of topics that the master is reporting as being published.
358
359 @return: List of topic names and types: [[topic1, type1]...[topicN, typeN]]
360 @rtype: [[str, str]]
361 """
362 code, msg, val = get_master().getPublishedTopics(namespace)
363 if code != 1:
364 raise rospy.exceptions.ROSException("unable to get published topics: %s"%msg)
365 return val
366
371 if self.msg is None:
372 self.msg = msg
375 """
376 Receive one message from topic.
377
378 This will create a new subscription to the topic, receive one message, then unsubscribe.
379
380 @param topic: name of topic
381 @type topic: str
382 @param topic_type: topic type
383 @type topic_type: L{rospy.Message} class
384 @param timeout: timeout time in seconds
385 @type timeout: double
386 @return: Message
387 @rtype: L{rospy.Message}
388 @raise ROSException: if specified timeout is exceeded
389 @raise ROSInterruptException: if shutdown interrupts wait
390 """
391 wfm = _WFM()
392 s = None
393 try:
394 s = rospy.topics.Subscriber(topic, topic_type, wfm.cb)
395 if timeout is not None:
396 timeout_t = time.time() + timeout
397 while not rospy.core.is_shutdown() and wfm.msg is None:
398 rospy.rostime.wallsleep(0.01)
399 if time.time() >= timeout_t:
400 raise rospy.exceptions.ROSException("timeout exceeded while waiting for message on topic %s"%topic)
401
402 else:
403 while not rospy.core.is_shutdown() and wfm.msg is None:
404 rospy.rostime.wallsleep(0.01)
405 finally:
406 if s is not None:
407 s.unregister()
408 if rospy.core.is_shutdown():
409 raise rospy.exceptions.ROSInterruptException("rospy shutdown")
410 return wfm.msg
411
412
413
414
415
416 _param_server = None
424
425
427 _unspecified = _Unspecified()
428
430 """
431 Retrieve a parameter from the param server
432
433 NOTE: this method is not thread-safe.
434
435 @param default: (optional) default value to return if key is not set
436 @type default: any
437 @return: parameter value
438 @rtype: XmlRpcLegalValue
439 @raise ROSException: if parameter server reports an error
440 @raise KeyError: if value not set and default is not given
441 """
442 try:
443 _init_param_server()
444 return _param_server[param_name]
445 except KeyError:
446 if default != _unspecified:
447 return default
448 else:
449 raise
450
452 """
453 Retrieve list of parameter names.
454
455 NOTE: this method is not thread-safe.
456
457 @return: parameter names
458 @rtype: [str]
459 @raise ROSException: if parameter server reports an error
460 """
461 _init_param_server()
462 code, msg, val = _param_server.getParamNames()
463 if code != 1:
464 raise rospy.exceptions.ROSException("Unable to retrieve parameter names: %s"%msg)
465 else:
466 return val
467
469 """
470 Set a parameter on the param server
471
472 NOTE: this method is not thread-safe.
473
474 @param param_name: parameter name
475 @type param_name: str
476 @param param_value: parameter value
477 @type param_value: XmlRpcLegalValue
478 @raise ROSException: if parameter server reports an error
479 """
480
481 if not rosgraph.names.is_legal_name(param_name):
482 import warnings
483 warnings.warn("'%s' is not a legal ROS graph resource name. This may cause problems with other ROS tools"%param_name, stacklevel=2)
484
485 _init_param_server()
486 _param_server[param_name] = param_value
487
489 """
490 Search for a parameter on the param server
491
492 NOTE: this method is not thread-safe.
493
494 @param param_name: parameter name
495 @type param_name: str
496 @return: key of matching parameter or None if no matching parameter.
497 @rtype: str
498 @raise ROSException: if parameter server reports an error
499 """
500 _init_param_server()
501 return _param_server.search_param(param_name)
502
504 """
505 Delete a parameter on the param server
506
507 NOTE: this method is not thread-safe.
508
509 @param param_name: parameter name
510 @type param_name: str
511 @raise KeyError: if parameter is not set
512 @raise ROSException: if parameter server reports an error
513 """
514 _init_param_server()
515 del _param_server[param_name]
516
518 """
519 Test if parameter exists on the param server
520
521 NOTE: this method is not thread-safe.
522
523 @param param_name: parameter name
524 @type param_name: str
525 @raise ROSException: if parameter server reports an error
526 """
527 _init_param_server()
528 return param_name in _param_server
529