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
40
41 import logging
42 import os
43 import socket
44 import struct
45 import sys
46 import time
47 import random
48
49 import roslib
50 import roslib.names
51 import roslib.network
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 DEBUG = Log.DEBUG
68 INFO = Log.INFO
69 WARN = Log.WARN
70 ERROR = Log.ERROR
71 FATAL = Log.FATAL
72
73 _rospy_to_logging_levels = {
74 DEBUG: logging.DEBUG,
75 INFO: logging.INFO,
76 WARN: logging.WARNING,
77 ERROR: logging.ERROR,
78 FATAL: logging.CRITICAL,
79 }
80
81
83 """
84 Register function to be called on shutdown. This function will be
85 called before Node begins teardown.
86 @param h: Function with zero args to be called on shutdown.
87 @type h: fn()
88 """
89 rospy.core.add_client_shutdown_hook(h)
90
106
108 """
109 Helper function for using sys.argv is ROS client libraries.
110 @return: copy of sys.argv with ROS remapping arguments removed
111 @rtype: [str]
112 """
113 import roslib.scriptutil
114 return roslib.scriptutil.myargv(argv=argv)
115
117 """
118 Uploads private params to the parameter server. Private params are specified
119 via command-line remappings.
120 """
121
122
123 import roslib.params
124 params = roslib.params.load_command_line_node_params(argv)
125 for param_name, param_value in params.items():
126 logdebug("setting param %s to %s"%(param_name, param_value))
127 set_param(roslib.names.PRIV_NAME + param_name, param_value)
128
129 _init_node_args = None
130
131 -def init_node(name, argv=None, anonymous=False, log_level=INFO, disable_rostime=False, disable_rosout=False, disable_signals=False):
132 """
133 Register client node with the master under the specified name.
134 This MUST be called from the main Python thread unless
135 disable_signals is set to True. Duplicate calls to init_node are
136 only allowed if the arguments are identical as the side-effects of
137 this method are not reversible.
138
139 @param name: Node's name. This parameter must be a base name,
140 meaning that it cannot contain namespaces (i.e. '/')
141 @type name: str
142
143 @param argv: Command line arguments to this program, including
144 remapping arguments (default: sys.argv). If you provide argv
145 to init_node(), any previously created rospy data structure
146 (Publisher, Subscriber, Service) will have invalid
147 mappings. It is important that you call init_node() first if
148 you wish to provide your own argv.
149 @type argv: [str]
150
151 @param anonymous: if True, a name will be auto-generated for the
152 node using name as the base. This is useful when you wish to
153 have multiple instances of the same node and don't care about
154 their actual names (e.g. tools, guis). name will be used as
155 the stem of the auto-generated name. NOTE: you cannot remap
156 the name of an anonymous node.
157 @type anonymous: bool
158
159 @param log_level: log level for sending message to /rosout and log
160 file, which is INFO by default. For convenience, you may use
161 rospy.DEBUG, rospy.INFO, rospy.ERROR, rospy.WARN, rospy.FATAL
162 @type log_level: int
163
164 @param disable_signals: If True, rospy will not register its own
165 signal handlers. You must set this flag if (a) you are unable
166 to call init_node from the main thread and/or you are using
167 rospy in an environment where you need to control your own
168 signal handling (e.g. WX). If you set this to True, you should
169 call rospy.signal_shutdown(reason) to initiate clean shutdown.
170
171 NOTE: disable_signals is overridden to True if
172 roslib.is_interactive() is True.
173
174 @type disable_signals: bool
175
176 @param disable_rostime: for internal testing only: suppresses
177 automatic subscription to rostime
178 @type disable_rostime: bool
179
180 @param disable_rosout: for internal testing only: suppress
181 auto-publication of rosout
182 @type disable_rostime: bool
183
184 @raise ROSInitException: if initialization/registration fails
185 @raise ValueError: if parameters are invalid (e.g. name contains a namespace or is otherwise illegal)
186 """
187 if argv is None:
188 argv = sys.argv
189 else:
190
191
192 rospy.names.reload_mappings(argv)
193
194
195 if roslib.names.SEP in name:
196 raise ValueError("namespaces are not allowed in node names")
197 if not roslib.names.is_legal_base_name(name):
198 import warnings
199 warnings.warn("'%s' is not a legal ROS base name. This may cause problems with other ROS tools"%name, stacklevel=2)
200
201 global _init_node_args
202
203
204
205 if _init_node_args:
206 if _init_node_args != (name, argv, anonymous, log_level, disable_rostime, disable_signals):
207 raise rospy.exceptions.ROSException("rospy.init_node() has already been called with different arguments: "+str(_init_node_args))
208 else:
209 return
210
211
212
213 disable_signals = disable_signals or roslib.is_interactive()
214 _init_node_args = (name, argv, anonymous, log_level, disable_rostime, disable_signals)
215
216 if not disable_signals:
217
218 rospy.core.register_signals()
219 else:
220 logdebug("signal handlers for rospy disabled")
221
222
223 mappings = rospy.names.get_mappings()
224 if '__name' in mappings:
225
226 name = roslib.names.resolve_name(mappings['__name'], rospy.core.get_caller_id())
227 if anonymous:
228 logdebug("[%s] WARNING: due to __name setting, anonymous setting is being changed to false"%name)
229 anonymous = False
230
231 if anonymous:
232
233
234 name = "%s_%s_%s"%(name, os.getpid(), int(time.time()*1000))
235
236 resolved_node_name = rospy.names.resolve_name(name)
237 rospy.core.configure_logging(resolved_node_name, level=_rospy_to_logging_levels[log_level])
238
239 rospy.names.initialize_mappings(resolved_node_name)
240
241 logger = logging.getLogger("rospy.client")
242 logger.info("init_node, name[%s], pid[%s]", resolved_node_name, os.getpid())
243
244
245 node = rospy.impl.init.start_node(os.environ, resolved_node_name)
246 rospy.core.set_node_uri(node.uri)
247 rospy.core.add_shutdown_hook(node.shutdown)
248
249 if rospy.core.is_shutdown():
250 logger.warn("aborting node initialization as shutdown has been triggered")
251 raise rospy.exceptions.ROSInitException("init_node interrupted before it could complete")
252
253
254 _init_node_params(argv, name)
255
256 rospy.core.set_initialized(True)
257
258 rospy.impl.rosout.load_rosout_handlers(log_level)
259 if not disable_rosout:
260 rospy.impl.rosout.init_rosout()
261 logdebug("init_node, name[%s], pid[%s]", resolved_node_name, os.getpid())
262 if not disable_rostime:
263 if not rospy.impl.simtime.init_simtime():
264 raise rospy.exceptions.ROSInitException("Failed to initialize time. Please check logs for additional details")
265 else:
266 rospy.rostime.set_rostime_initialized(True)
267
268
269 _master_proxy = None
270
272 """
273 Get a remote handle to the ROS Master.
274 This method can be called independent of running a ROS node,
275 though the ROS_MASTER_URI must be declared in the environment.
276
277 @return: ROS Master remote object
278 @rtype: L{rospy.MasterProxy}
279 @raise Exception: if server cannot be located or system cannot be
280 initialized
281 """
282 global _master_proxy
283 if _master_proxy is not None:
284 return _master_proxy
285 import roslib.rosenv
286 _master_proxy = rospy.msproxy.MasterProxy(roslib.rosenv.get_master_uri())
287 return _master_proxy
288
289
290
291
293 """
294 Retrieve list of topics that the master is reporting as being published.
295
296 @return: List of topic names and types: [[topic1, type1]...[topicN, typeN]]
297 @rtype: [[str, str]]
298 """
299 code, msg, val = get_master().getPublishedTopics(namespace)
300 if code != 1:
301 raise rospy.exceptions.ROSException("unable to get published topics: %s"%msg)
302 return val
303
308 if self.msg is None:
309 self.msg = msg
312 """
313 Receive one message from topic.
314
315 This will create a new subscription to the topic, receive one message, then unsubscribe.
316
317 @param topic: name of topic
318 @type topic: str
319 @param topic_type: topic type
320 @type topic_type: L{rospy.Message} class
321 @param timeout: timeout time in seconds
322 @type timeout: double
323 @return: Message
324 @rtype: L{rospy.Message}
325 @raise ROSException: if specified timeout is exceeded
326 @raise ROSInterruptException: if shutdown interrupts wait
327 """
328 wfm = _WFM()
329 s = None
330 try:
331 s = rospy.topics.Subscriber(topic, topic_type, wfm.cb)
332 if timeout is not None:
333 timeout_t = time.time() + timeout
334 while not rospy.core.is_shutdown() and wfm.msg is None:
335 rospy.rostime.wallsleep(0.01)
336 if time.time() >= timeout_t:
337 raise rospy.exceptions.ROSException("timeout exceeded while waiting for message on topic %s"%topic)
338
339 else:
340 while not rospy.core.is_shutdown() and wfm.msg is None:
341 rospy.rostime.wallsleep(0.01)
342 finally:
343 if s is not None:
344 s.unregister()
345 if rospy.core.is_shutdown():
346 raise rospy.exceptions.ROSInterruptException("rospy shutdown")
347 return wfm.msg
348
349
350
351
352
353 _param_server = None
361
362
364 _unspecified = _Unspecified()
365
367 """
368 Retrieve a parameter from the param server
369 @param default: (optional) default value to return if key is not set
370 @type default: any
371 @return: parameter value
372 @rtype: XmlRpcLegalValue
373 @raise ROSException: if parameter server reports an error
374 @raise KeyError: if value not set and default is not given
375 """
376 try:
377 _init_param_server()
378 return _param_server[param_name]
379 except KeyError:
380 if default != _unspecified:
381 return default
382 else:
383 raise
384
386 """
387 Retrieve list of parameter names.
388 @return: parameter names
389 @rtype: [str]
390 @raise ROSException: if parameter server reports an error
391 """
392 _init_param_server()
393 code, msg, val = _param_server.getParamNames()
394 if code != 1:
395 raise rospy.exceptions.ROSException("Unable to retrieve parameter names: %s"%msg)
396 else:
397 return val
398
400 """
401 Set a parameter on the param server
402 @param param_name: parameter name
403 @type param_name: str
404 @param param_value: parameter value
405 @type param_value: XmlRpcLegalValue
406 @raise ROSException: if parameter server reports an error
407 """
408
409 if not roslib.names.is_legal_name(param_name):
410 import warnings
411 warnings.warn("'%s' is not a legal ROS graph resource name. This may cause problems with other ROS tools"%param_name, stacklevel=2)
412
413 _init_param_server()
414 _param_server[param_name] = param_value
415
417 """
418 Search for a parameter on the param server
419 @param param_name: parameter name
420 @type param_name: str
421 @return: key of matching parameter or None if no matching parameter.
422 @rtype: str
423 @raise ROSException: if parameter server reports an error
424 """
425 _init_param_server()
426 return _param_server.search_param(param_name)
427
429 """
430 Delete a parameter on the param server
431 @param param_name: parameter name
432 @type param_name: str
433 @raise KeyError: if parameter is not set
434 @raise ROSException: if parameter server reports an error
435 """
436 _init_param_server()
437 del _param_server[param_name]
438
440 """
441 Test if parameter exists on the param server
442 @param param_name: parameter name
443 @type param_name: str
444 @raise ROSException: if parameter server reports an error
445 """
446 _init_param_server()
447 return param_name in _param_server
448