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, xmlrpc_port=0, tcpros_port=0):
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 @param xmlrpc_port: If provided, it will use this port number for the client
243 XMLRPC node.
244 @type xmlrpc_port: int
245
246 @param tcpros_port: If provided, the TCPROS server will listen for
247 connections on this port
248 @type tcpros_port: int
249
250 @raise ROSInitException: if initialization/registration fails
251 @raise ValueError: if parameters are invalid (e.g. name contains a namespace or is otherwise illegal)
252 """
253 if argv is None:
254 argv = sys.argv
255 else:
256
257
258 rospy.names.reload_mappings(argv)
259
260
261 if rosgraph.names.SEP in name:
262 raise ValueError("namespaces are not allowed in node names")
263 if not rosgraph.names.is_legal_base_name(name):
264 import warnings
265 warnings.warn("'%s' is not a legal ROS base name. This may cause problems with other ROS tools"%name, stacklevel=2)
266
267 global _init_node_args
268
269
270
271 if _init_node_args:
272 if _init_node_args != (name, argv, anonymous, log_level, disable_rostime, disable_signals):
273 raise rospy.exceptions.ROSException("rospy.init_node() has already been called with different arguments: "+str(_init_node_args))
274 else:
275 return
276
277
278
279 disable_signals = disable_signals or roslib.is_interactive()
280 _init_node_args = (name, argv, anonymous, log_level, disable_rostime, disable_signals)
281
282 if not disable_signals:
283
284 rospy.core.register_signals()
285 else:
286 logdebug("signal handlers for rospy disabled")
287
288
289 mappings = rospy.names.get_mappings()
290 if '__name' in mappings:
291
292 name = rosgraph.names.resolve_name(mappings['__name'], rospy.core.get_caller_id())
293 if anonymous:
294 logdebug("[%s] WARNING: due to __name setting, anonymous setting is being changed to false"%name)
295 anonymous = False
296
297 if anonymous:
298
299
300 name = "%s_%s_%s"%(name, os.getpid(), int(time.time()*1000))
301
302 resolved_node_name = rospy.names.resolve_name(name)
303 rospy.core.configure_logging(resolved_node_name)
304
305 rospy.names.initialize_mappings(resolved_node_name)
306
307 logger = logging.getLogger("rospy.client")
308 logger.info("init_node, name[%s], pid[%s]", resolved_node_name, os.getpid())
309
310
311 node = rospy.impl.init.start_node(os.environ, resolved_node_name, port=xmlrpc_port, tcpros_port=tcpros_port)
312 rospy.core.set_node_uri(node.uri)
313 rospy.core.add_shutdown_hook(node.shutdown)
314
315 if rospy.core.is_shutdown():
316 logger.warn("aborting node initialization as shutdown has been triggered")
317 raise rospy.exceptions.ROSInitException("init_node interrupted before it could complete")
318
319
320 _init_node_params(argv, name)
321
322 rospy.core.set_initialized(True)
323
324 if not disable_rosout:
325 rospy.impl.rosout.init_rosout()
326 rospy.impl.rosout.load_rosout_handlers(log_level)
327
328 if not disable_rostime:
329 if not rospy.impl.simtime.init_simtime():
330 raise rospy.exceptions.ROSInitException("Failed to initialize time. Please check logs for additional details")
331 else:
332 rospy.rostime.set_rostime_initialized(True)
333
334 logdebug("init_node, name[%s], pid[%s]", resolved_node_name, os.getpid())
335
336 Service('~get_loggers', GetLoggers, _get_loggers)
337 Service('~set_logger_level', SetLoggerLevel, _set_logger_level)
338
339
340
341 _master_proxy = None
342
344 """
345 Get a remote handle to the ROS Master.
346 This method can be called independent of running a ROS node,
347 though the ROS_MASTER_URI must be declared in the environment.
348
349 @return: ROS Master remote object
350 @rtype: L{rospy.MasterProxy}
351 @raise Exception: if server cannot be located or system cannot be
352 initialized
353 """
354 global _master_proxy
355 if _master_proxy is not None:
356 return _master_proxy
357 _master_proxy = rospy.msproxy.MasterProxy(rosgraph.get_master_uri())
358 return _master_proxy
359
360
361
362
364 """
365 Retrieve list of topics that the master is reporting as being published.
366
367 @return: List of topic names and types: [[topic1, type1]...[topicN, typeN]]
368 @rtype: [[str, str]]
369 """
370 code, msg, val = get_master().getPublishedTopics(namespace)
371 if code != 1:
372 raise rospy.exceptions.ROSException("unable to get published topics: %s"%msg)
373 return val
374
379 if self.msg is None:
380 self.msg = msg
383 """
384 Receive one message from topic.
385
386 This will create a new subscription to the topic, receive one message, then unsubscribe.
387
388 @param topic: name of topic
389 @type topic: str
390 @param topic_type: topic type
391 @type topic_type: L{rospy.Message} class
392 @param timeout: timeout time in seconds
393 @type timeout: double
394 @return: Message
395 @rtype: L{rospy.Message}
396 @raise ROSException: if specified timeout is exceeded
397 @raise ROSInterruptException: if shutdown interrupts wait
398 """
399 wfm = _WFM()
400 s = None
401 try:
402 s = rospy.topics.Subscriber(topic, topic_type, wfm.cb)
403 if timeout is not None:
404 timeout_t = time.time() + timeout
405 while not rospy.core.is_shutdown() and wfm.msg is None:
406 rospy.rostime.wallsleep(0.01)
407 if time.time() >= timeout_t:
408 raise rospy.exceptions.ROSException("timeout exceeded while waiting for message on topic %s"%topic)
409
410 else:
411 while not rospy.core.is_shutdown() and wfm.msg is None:
412 rospy.rostime.wallsleep(0.01)
413 finally:
414 if s is not None:
415 s.unregister()
416 if rospy.core.is_shutdown():
417 raise rospy.exceptions.ROSInterruptException("rospy shutdown")
418 return wfm.msg
419
420
421
422
423
424 _param_server = None
432
433
435 _unspecified = _Unspecified()
436
438 """
439 Retrieve a parameter from the param server
440
441 NOTE: this method is not thread-safe.
442
443 @param default: (optional) default value to return if key is not set
444 @type default: any
445 @return: parameter value
446 @rtype: XmlRpcLegalValue
447 @raise ROSException: if parameter server reports an error
448 @raise KeyError: if value not set and default is not given
449 """
450 try:
451 _init_param_server()
452 return _param_server[param_name]
453 except KeyError:
454 if default != _unspecified:
455 return default
456 else:
457 raise
458
460 """
461 Retrieve list of parameter names.
462
463 NOTE: this method is not thread-safe.
464
465 @return: parameter names
466 @rtype: [str]
467 @raise ROSException: if parameter server reports an error
468 """
469 _init_param_server()
470 code, msg, val = _param_server.getParamNames()
471 if code != 1:
472 raise rospy.exceptions.ROSException("Unable to retrieve parameter names: %s"%msg)
473 else:
474 return val
475
477 """
478 Set a parameter on the param server
479
480 NOTE: this method is not thread-safe.
481
482 @param param_name: parameter name
483 @type param_name: str
484 @param param_value: parameter value
485 @type param_value: XmlRpcLegalValue
486 @raise ROSException: if parameter server reports an error
487 """
488
489 if not rosgraph.names.is_legal_name(param_name):
490 import warnings
491 warnings.warn("'%s' is not a legal ROS graph resource name. This may cause problems with other ROS tools"%param_name, stacklevel=2)
492
493 _init_param_server()
494 _param_server[param_name] = param_value
495
497 """
498 Search for a parameter on the param server
499
500 NOTE: this method is not thread-safe.
501
502 @param param_name: parameter name
503 @type param_name: str
504 @return: key of matching parameter or None if no matching parameter.
505 @rtype: str
506 @raise ROSException: if parameter server reports an error
507 """
508 _init_param_server()
509 return _param_server.search_param(param_name)
510
512 """
513 Delete a parameter on the param server
514
515 NOTE: this method is not thread-safe.
516
517 @param param_name: parameter name
518 @type param_name: str
519 @raise KeyError: if parameter is not set
520 @raise ROSException: if parameter server reports an error
521 """
522 _init_param_server()
523 del _param_server[param_name]
524
526 """
527 Test if parameter exists on the param server
528
529 NOTE: this method is not thread-safe.
530
531 @param param_name: parameter name
532 @type param_name: str
533 @raise ROSException: if parameter server reports an error
534 """
535 _init_param_server()
536 return param_name in _param_server
537