rospy.client

1 # Software License Agreement (BSD License) 2 # 3 # Copyright (c) 2008, Willow Garage, Inc. 4 # All rights reserved. 5 # 6 # Redistribution and use in source and binary forms, with or without 7 # modification, are permitted provided that the following conditions 8 # are met: 9 # 10 # * Redistributions of source code must retain the above copyright 11 # notice, this list of conditions and the following disclaimer. 12 # * Redistributions in binary form must reproduce the above 13 # copyright notice, this list of conditions and the following 14 # disclaimer in the documentation and/or other materials provided 15 # with the distribution. 16 # * Neither the name of Willow Garage, Inc. nor the names of its 17 # contributors may be used to endorse or promote products derived 18 # from this software without specific prior written permission. 19 # 20 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 # POSSIBILITY OF SUCH DAMAGE. 32 # 33 # Revision $Id: client.py 12719 2010-12-31 21:17:31Z kwc $ 34 35 """ 36 Additional ROS client API methods. 37 """ 38 39 from __future__ import with_statement 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 #seconds 64 65 # log level constants 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

82 -def on_shutdown(h):

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

91 -def spin():

92 """ 93 Blocks until ROS node is shutdown. Yields activity to other threads. 94 @raise ROSInitException: if node is not in a properly initialized state 95 """ 96 97 if not rospy.core.is_initialized(): 98 raise rospy.exceptions.ROSInitException("client code must call rospy.init_node() first") 99 logdebug("node[%s, %s] entering spin(), pid[%s]", rospy.core.get_caller_id(), rospy.core.get_node_uri(), os.getpid()) 100 try: 101 while not rospy.core.is_shutdown(): 102 time.sleep(0.5) 103 except KeyboardInterrupt: 104 logdebug("keyboard interrupt, shutting down") 105 rospy.core.signal_shutdown('keyboard interrupt')

106

107 -def myargv(argv=sys.argv):

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

116 -def _init_node_params(argv, node_name):

117 """ 118 Uploads private params to the parameter server. Private params are specified 119 via command-line remappings. 120 """ 121 122 # #1027: load in param name mappings 123 import roslib.params 124 params = roslib.params.load_command_line_node_params(argv) 125 for param_name, param_value in params.iteritems(): 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 rostests only, suppresses 177 automatic subscription to rostime 178 @type disable_rostime: bool 179 180 @param disable_rosout: suppress auto-publication of rosout 181 @type disable_rostime: bool 182 183 @raise ROSInitException: if initialization/registration fails 184 @raise ValueError: if parameters are invalid (e.g. name contains a namespace or is otherwise illegal) 185 """ 186 if argv is None: 187 argv = sys.argv 188 else: 189 # reload the mapping table. Any previously created rospy data 190 # structure does *not* reinitialize based on the new mappings. 191 rospy.names.reload_mappings(argv) 192 193 # this test can be eliminated once we change from warning to error in the next check 194 if roslib.names.SEP in name: 195 raise ValueError("namespaces are not allowed in node names") 196 if not roslib.names.is_legal_base_name(name): 197 import warnings 198 warnings.warn("'%s' is not a legal ROS base name. This may cause problems with other ROS tools"%name, stacklevel=2) 199 200 global _init_node_args 201 202 # #972: allow duplicate init_node args if calls are identical 203 # NOTE: we don't bother checking for node name aliases (e.g. 'foo' == '/foo'). 204 if _init_node_args: 205 if _init_node_args != (name, argv, anonymous, log_level, disable_rostime, disable_signals): 206 raise rospy.exceptions.ROSException("rospy.init_node() has already been called with different arguments: "+str(_init_node_args)) 207 else: 208 return #already initialized 209 210 # for scripting environments, we don't want to use the ROS signal 211 # handlers 212 disable_signals = disable_signals or roslib.is_interactive() 213 _init_node_args = (name, argv, anonymous, log_level, disable_rostime, disable_signals) 214 215 if not disable_signals: 216 # NOTE: register_signals must be called from main thread 217 rospy.core.register_signals() # add handlers for SIGINT/etc... 218 else: 219 logdebug("signal handlers for rospy disabled") 220 221 # check for name override 222 mappings = rospy.names.get_mappings() 223 if '__name' in mappings: 224 # use roslib version of resolve_name to avoid remapping 225 name = roslib.names.resolve_name(mappings['__name'], rospy.core.get_caller_id()) 226 if anonymous: 227 logdebug("[%s] WARNING: due to __name setting, anonymous setting is being changed to false"%name) 228 anonymous = False 229 230 if anonymous: 231 # not as good as a uuid/guid, but more readable. can't include 232 # hostname as that is not guaranteed to be a legal ROS name 233 name = "%s_%s_%s"%(name, os.getpid(), int(time.time()*1000)) 234 235 resolved_node_name = rospy.names.resolve_name(name) 236 rospy.core.configure_logging(resolved_node_name, level=_rospy_to_logging_levels[log_level]) 237 # #1810 238 rospy.names.initialize_mappings(resolved_node_name) 239 240 logger = logging.getLogger("rospy.client") 241 logger.info("init_node, name[%s], pid[%s]", resolved_node_name, os.getpid()) 242 243 # node initialization blocks until registration with master 244 node = rospy.impl.init.start_node(os.environ, resolved_node_name) 245 rospy.core.set_node_uri(node.uri) 246 rospy.core.add_shutdown_hook(node.shutdown) 247 248 if rospy.core.is_shutdown(): 249 logger.warn("aborting node initialization as shutdown has been triggered") 250 raise rospy.exceptions.ROSInitException("init_node interrupted before it could complete") 251 252 # upload private params (set via command-line) to parameter server 253 _init_node_params(argv, name) 254 255 rospy.core.set_initialized(True) 256 257 rospy.impl.rosout.load_rosout_handlers(log_level) 258 if not disable_rosout: 259 rospy.impl.rosout.init_rosout() 260 logdebug("init_node, name[%s], pid[%s]", resolved_node_name, os.getpid()) 261 if not disable_rostime: 262 if not rospy.impl.simtime.init_simtime(): 263 raise rospy.exceptions.ROSInitException("Failed to initialize time. Please check logs for additional details") 264 else: 265 rospy.rostime.set_rostime_initialized(True)

266 267 #_master_proxy is a MasterProxy wrapper 268 _master_proxy = None 269

270 -def get_master(env=os.environ):

271 """ 272 Get a remote handle to the ROS Master. 273 This method can be called independent of running a ROS node, 274 though the ROS_MASTER_URI must be declared in the environment. 275 276 @return: ROS Master remote object 277 @rtype: L{rospy.MasterProxy} 278 @raise Exception: if server cannot be located or system cannot be 279 initialized 280 """ 281 global _master_proxy 282 if _master_proxy is not None: 283 return _master_proxy 284 import roslib.rosenv 285 _master_proxy = rospy.msproxy.MasterProxy(roslib.rosenv.get_master_uri()) 286 return _master_proxy

287 288 ######################################################### 289 # Topic helpers 290

291 -def get_published_topics(namespace='/'):

292 """ 293 Retrieve list of topics that the master is reporting as being published. 294 295 @return: List of topic names and types: [[topic1, type1]...[topicN, typeN]] 296 @rtype: [[str, str]] 297 """ 298 code, msg, val = get_master().getPublishedTopics(namespace) 299 if code != 1: 300 raise rospy.exceptions.ROSException("unable to get published topics: %s"%msg) 301 return val

302

303 -class _WFM(object):

304 - def __init__(self):

305 self.msg = None

306 - def cb(self, msg):

307 if self.msg is None: 308 self.msg = msg

309

310 -def wait_for_message(topic, topic_type, timeout=None):

311 """ 312 Receive one message from topic. 313 314 This will create a new subscription to the topic, receive one message, then unsubscribe. 315 316 @param topic: name of topic 317 @type topic: str 318 @param topic_type: topic type 319 @type topic_type: L{rospy.Message} class 320 @param timeout: timeout time in seconds 321 @type timeout: double 322 @return: Message 323 @rtype: L{rospy.Message} 324 @raise ROSException: if specified timeout is exceeded 325 @raise ROSInterruptException: if shutdown interrupts wait 326 """ 327 wfm = _WFM() 328 s = None 329 try: 330 s = rospy.topics.Subscriber(topic, topic_type, wfm.cb) 331 if timeout is not None: 332 timeout_t = time.time() + timeout 333 while not rospy.core.is_shutdown() and wfm.msg is None: 334 time.sleep(0.01) 335 if time.time() >= timeout_t: 336 raise rospy.exceptions.ROSException("timeout exceeded while waiting for message on topic %s"%topic) 337 338 else: 339 while not rospy.core.is_shutdown() and wfm.msg is None: 340 time.sleep(0.01) 341 finally: 342 if s is not None: 343 s.unregister() 344 if rospy.core.is_shutdown(): 345 raise rospy.exceptions.ROSInterruptException("rospy shutdown") 346 return wfm.msg

347 348 349 ######################################################### 350 # Param Server Access 351 352 _param_server = None

353 -def _init_param_server():

354 """ 355 Initialize parameter server singleton 356 """ 357 global _param_server 358 if _param_server is None: 359 _param_server = get_master() #in the future param server will be a service

360 361 # class and singleton to distinguish whether or not user has passed us a default value

362 -class _Unspecified(object): pass

363 _unspecified = _Unspecified() 364

365 -def get_param(param_name, default=_unspecified):

366 """ 367 Retrieve a parameter from the param server 368 @param default: (optional) default value to return if key is not set 369 @type default: any 370 @return: parameter value 371 @rtype: XmlRpcLegalValue 372 @raise ROSException: if parameter server reports an error 373 @raise KeyError: if value not set and default is not given 374 """ 375 try: 376 _init_param_server() 377 return _param_server[param_name] #MasterProxy does all the magic for us 378 except KeyError: 379 if default != _unspecified: 380 return default 381 else: 382 raise

383

384 -def get_param_names():

385 """ 386 Retrieve list of parameter names. 387 @return: parameter names 388 @rtype: [str] 389 @raise ROSException: if parameter server reports an error 390 """ 391 _init_param_server() 392 code, msg, val = _param_server.getParamNames() #MasterProxy does all the magic for us 393 if code != 1: 394 raise rospy.exceptions.ROSException("Unable to retrieve parameter names: %s"%msg) 395 else: 396 return val

397

398 -def set_param(param_name, param_value):

399 """ 400 Set a parameter on the param server 401 @param param_name: parameter name 402 @type param_name: str 403 @param param_value: parameter value 404 @type param_value: XmlRpcLegalValue 405 @raise ROSException: if parameter server reports an error 406 """ 407 # #2202 408 if not roslib.names.is_legal_name(param_name): 409 import warnings 410 warnings.warn("'%s' is not a legal ROS graph resource name. This may cause problems with other ROS tools"%param_name, stacklevel=2) 411 412 _init_param_server() 413 _param_server[param_name] = param_value #MasterProxy does all the magic for us

414

415 -def search_param(param_name):

416 """ 417 Search for a parameter on the param server 418 @param param_name: parameter name 419 @type param_name: str 420 @return: key of matching parameter or None if no matching parameter. 421 @rtype: str 422 @raise ROSException: if parameter server reports an error 423 """ 424 _init_param_server() 425 return _param_server.search_param(param_name)

426

427 -def delete_param(param_name):

428 """ 429 Delete a parameter on the param server 430 @param param_name: parameter name 431 @type param_name: str 432 @raise KeyError: if parameter is not set 433 @raise ROSException: if parameter server reports an error 434 """ 435 _init_param_server() 436 del _param_server[param_name] #MasterProxy does all the magic for us

437

438 -def has_param(param_name):

439 """ 440 Test if parameter exists on the param server 441 @param param_name: parameter name 442 @type param_name: str 443 @raise ROSException: if parameter server reports an error 444 """ 445 _init_param_server() 446 return param_name in _param_server #MasterProxy does all the magic for us

447 448 ################################################################################ 449 # Time helpers 450 451 # these cannot go into rostime due to circular deps 452 453

454 -class Rate(object):

455 """ 456 Convenience class for sleeping in a loop at a specified rate 457 """ 458

459 - def __init__(self, hz):

460 """ 461 Constructor. 462 @param hz: hz rate to determine sleeping 463 @type hz: int 464 """ 465 # #1403 466 self.last_time = rospy.rostime.get_rostime() 467 self.sleep_dur = rospy.rostime.Duration(0, int(1e9/hz))

468

469 - def sleep(self):

470 """ 471 Attempt sleep at the specified rate. sleep() takes into 472 account the time elapsed since the last successful 473 sleep(). 474 475 @raise ROSInterruptException: if ROS time is set backwards or if 476 ROS shutdown occurs before sleep completes 477 """ 478 curr_time = rospy.rostime.get_rostime() 479 # detect time jumping backwards 480 if self.last_time > curr_time: 481 self.last_time = curr_time 482 483 # calculate sleep interval 484 elapsed = curr_time - self.last_time 485 sleep(self.sleep_dur - elapsed) 486 self.last_time = self.last_time + self.sleep_dur 487 488 # detect time jumping forwards, as well as loops that are 489 # inherently too slow 490 if curr_time - self.last_time > self.sleep_dur * 2: 491 self.last_time = curr_time

492 493 # TODO: may want more specific exceptions for sleep

494 -def sleep(duration):

495 """ 496 sleep for the specified duration in ROS time. If duration 497 is negative, sleep immediately returns. 498 499 @param duration: seconds (or rospy.Duration) to sleep 500 @type duration: float or Duration 501 @raise ROSInterruptException: if ROS time is set backwards or if 502 ROS shutdown occurs before sleep completes 503 """ 504 if rospy.rostime.is_wallclock(): 505 if isinstance(duration, roslib.rostime.Duration): 506 duration = duration.to_sec() 507 if duration < 0: 508 return 509 else: 510 time.sleep(duration) 511 else: 512 initial_rostime = rospy.rostime.get_rostime() 513 if not isinstance(duration, roslib.rostime.Duration): 514 duration = rospy.rostime.Duration.from_sec(duration) 515 516 rostime_cond = rospy.rostime.get_rostime_cond() 517 518 # #3123 519 if initial_rostime == roslib.rostime.Time(0): 520 # break loop if time is initialized or node is shutdown 521 while initial_rostime == roslib.rostime.Time(0) and \ 522 not rospy.core.is_shutdown(): 523 with rostime_cond: 524 rostime_cond.wait(0.3) 525 initial_rostime = rospy.rostime.get_rostime() 526 527 sleep_t = initial_rostime + duration 528 529 # break loop if sleep_t is reached, time moves backwards, or 530 # node is shutdown 531 while rospy.rostime.get_rostime() < sleep_t and \ 532 rospy.rostime.get_rostime() >= initial_rostime and \ 533 not rospy.core.is_shutdown(): 534 with rostime_cond: 535 rostime_cond.wait(0.5) 536 537 if rospy.rostime.get_rostime() < initial_rostime: 538 raise rospy.exceptions.ROSInterruptException("ROS time moved backwards") 539 if rospy.core.is_shutdown(): 540 raise rospy.exceptions.ROSInterruptException("ROS shutdown request")

541

Source Code for Module rospy.client