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