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$ 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 #seconds 65 66 # log level constants 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

77 -def myargv(argv=None):

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.is_legal_remap(a)]

86

87 -def load_command_line_node_params(argv):

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.is_legal_remap(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.safe_load(dst) 105 return mappings 106 except Exception as e: 107 raise rospy.exceptions.ROSInitException("invalid command-line parameters: %s"%(str(e)))

108

109 -def on_shutdown(h):

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

118 -def spin():

119 """ 120 Blocks until ROS node is shutdown. Yields activity to other threads. 121 @raise ROSInitException: if node is not in a properly initialized state 122 """ 123 124 if not rospy.core.is_initialized(): 125 raise rospy.exceptions.ROSInitException("client code must call rospy.init_node() first") 126 logdebug("node[%s, %s] entering spin(), pid[%s]", rospy.core.get_caller_id(), rospy.core.get_node_uri(), os.getpid()) 127 try: 128 while not rospy.core.is_shutdown(): 129 rospy.rostime.wallsleep(0.5) 130 except KeyboardInterrupt: 131 logdebug("keyboard interrupt, shutting down") 132 rospy.core.signal_shutdown('keyboard interrupt')

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

142 -def _get_loggers(request):

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

161 -def _set_logger_level(request):

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

174 -def _init_node_params(argv, node_name):

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 # #1027: load in param name mappings 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 # reload the mapping table. Any previously created rospy data 258 # structure does *not* reinitialize based on the new mappings. 259 rospy.names.reload_mappings(argv) 260 261 if not name: 262 raise ValueError("name must not be empty") 263 264 # this test can be eliminated once we change from warning to error in the next check 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 # #972: allow duplicate init_node args if calls are identical 271 # NOTE: we don't bother checking for node name aliases (e.g. 'foo' == '/foo'). 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 #already initialized 277 278 # for scripting environments, we don't want to use the ROS signal 279 # handlers 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 # NOTE: register_signals must be called from main thread 285 rospy.core.register_signals() # add handlers for SIGINT/etc... 286 else: 287 logdebug("signal handlers for rospy disabled") 288 289 # check for name override 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 # not as good as a uuid/guid, but more readable. can't include 299 # hostname as that is not guaranteed to be a legal ROS name 300 name = "%s_%s_%s"%(name, os.getpid(), int(time.time()*1000)) 301 302 # check for legal base name once all changes have been made to the name 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 # use rosgraph version of resolve_name to avoid remapping 308 resolved_node_name = rosgraph.names.resolve_name(name, rospy.core.get_caller_id()) 309 rospy.core.configure_logging(resolved_node_name) 310 # #1810 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 # node initialization blocks until registration with master 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 # upload private params (set via command-line) to parameter server 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 # advertise logging level services 342 Service('~get_loggers', GetLoggers, _get_loggers) 343 Service('~set_logger_level', SetLoggerLevel, _set_logger_level)

344 345 346 #_master_proxy is a MasterProxy wrapper 347 _master_proxy = None 348 _master_proxy_lock = Lock() 349

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

351 """ 352 Get a remote handle to the ROS Master. 353 This method can be called independent of running a ROS node, 354 though the ROS_MASTER_URI must be declared in the environment. 355 356 @return: ROS Master remote object 357 @rtype: L{rospy.MasterProxy} 358 @raise Exception: if server cannot be located or system cannot be 359 initialized 360 """ 361 global _master_proxy, _master_proxy_lock 362 if _master_proxy is None: 363 with _master_proxy_lock: 364 if _master_proxy is None: 365 _master_proxy = rospy.msproxy.MasterProxy( 366 rosgraph.get_master_uri()) 367 return _master_proxy

368 369 ######################################################### 370 # Topic helpers 371

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

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

384 -class _WFM(object):

385 - def __init__(self):

386 self.msg = None

387 - def cb(self, msg):

388 if self.msg is None: 389 self.msg = msg

390

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

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 or ROS Duration 402 @type timeout: double|rospy.Duration 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 if isinstance(timeout, rospy.Duration): 414 timeout = timeout.to_sec() 415 timeout_t = time.time() + timeout 416 while not rospy.core.is_shutdown() and wfm.msg is None: 417 rospy.rostime.wallsleep(0.01) 418 if time.time() >= timeout_t: 419 raise rospy.exceptions.ROSException("timeout exceeded while waiting for message on topic %s"%topic) 420 421 else: 422 while not rospy.core.is_shutdown() and wfm.msg is None: 423 rospy.rostime.wallsleep(0.01) 424 finally: 425 if s is not None: 426 s.unregister() 427 if rospy.core.is_shutdown(): 428 raise rospy.exceptions.ROSInterruptException("rospy shutdown") 429 return wfm.msg

430 431 432 ######################################################### 433 # Param Server Access 434 435 _param_server = None 436 _param_server_lock = Lock()

437 -def _init_param_server():

438 """ 439 Initialize parameter server singleton 440 """ 441 global _param_server, _param_server_lock 442 if _param_server is None: 443 with _param_server_lock: 444 if _param_server is None: 445 _param_server = get_master() 446 return _param_server_lock

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

449 -class _Unspecified(object): pass

450 _unspecified = _Unspecified() 451

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

453 """ 454 Retrieve a parameter from the param server 455 456 NOTE: this method is thread-safe. 457 458 @param default: (optional) default value to return if key is not set 459 @type default: any 460 @return: parameter value 461 @rtype: XmlRpcLegalValue 462 @raise ROSException: if parameter server reports an error 463 @raise KeyError: if value not set and default is not given 464 """ 465 try: 466 _init_param_server() 467 return _param_server[param_name] #MasterProxy does all the magic for us 468 except KeyError: 469 if default != _unspecified: 470 return default 471 else: 472 raise

473 474

475 -def get_param_cached(param_name, default=_unspecified):

476 """ 477 Retrieve a parameter from the param server with local caching 478 479 NOTE: this method is thread-safe. 480 481 @param default: (optional) default value to return if key is not set 482 @type default: any 483 @return: parameter value 484 @rtype: XmlRpcLegalValue 485 @raise ROSException: if parameter server reports an error 486 @raise KeyError: if value not set and default is not given 487 """ 488 try: 489 _init_param_server() 490 return _param_server.get_param_cached(param_name) 491 except KeyError: 492 if default != _unspecified: 493 return default 494 else: 495 raise

496

497 -def get_param_names():

498 """ 499 Retrieve list of parameter names. 500 501 NOTE: this method is thread-safe. 502 503 @return: parameter names 504 @rtype: [str] 505 @raise ROSException: if parameter server reports an error 506 """ 507 _init_param_server() 508 code, msg, val = _param_server.getParamNames() #MasterProxy does all the magic for us 509 if code != 1: 510 raise rospy.exceptions.ROSException("Unable to retrieve parameter names: %s"%msg) 511 else: 512 return val

513

514 -def set_param(param_name, param_value):

515 """ 516 Set a parameter on the param server 517 518 NOTE: this method is thread-safe. 519 If param_value is a dictionary it will be treated as a parameter 520 tree, where param_name is the namespace. For example::: 521 {'x':1,'y':2,'sub':{'z':3}} 522 will set param_name/x=1, param_name/y=2, and param_name/sub/z=3. 523 Furthermore, it will replace all existing parameters in the 524 param_name namespace with the parameters in param_value. You must 525 set parameters individually if you wish to perform a union update. 526 527 @param param_name: parameter name 528 @type param_name: str 529 @param param_value: parameter value 530 @type param_value: XmlRpcLegalValue 531 @raise ROSException: if parameter server reports an error 532 """ 533 # #2202 534 if not rosgraph.names.is_legal_name(param_name): 535 import warnings 536 warnings.warn("'%s' is not a legal ROS graph resource name. This may cause problems with other ROS tools"%param_name, stacklevel=2) 537 538 _init_param_server() 539 _param_server[param_name] = param_value #MasterProxy does all the magic for us

540

541 -def search_param(param_name):

542 """ 543 Search for a parameter on the param server 544 545 NOTE: this method is thread-safe. 546 547 @param param_name: parameter name 548 @type param_name: str 549 @return: key of matching parameter or None if no matching parameter. 550 @rtype: str 551 @raise ROSException: if parameter server reports an error 552 """ 553 _init_param_server() 554 return _param_server.search_param(param_name)

555

556 -def delete_param(param_name):

557 """ 558 Delete a parameter on the param server 559 560 NOTE: this method is thread-safe. 561 562 @param param_name: parameter name 563 @type param_name: str 564 @raise KeyError: if parameter is not set 565 @raise ROSException: if parameter server reports an error 566 """ 567 _init_param_server() 568 del _param_server[param_name] #MasterProxy does all the magic for us

569

570 -def has_param(param_name):

571 """ 572 Test if parameter exists on the param server 573 574 NOTE: this method is thread-safe. 575 576 @param param_name: parameter name 577 @type param_name: str 578 @raise ROSException: if parameter server reports an error 579 """ 580 _init_param_server() 581 return param_name in _param_server #MasterProxy does all the magic for us

582

Source Code for Module rospy.client