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.REMAP in 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.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

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 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 # Param Server Access 432 433 _param_server = None 434 _param_server_lock = Lock()

435 -def _init_param_server():

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

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

447 -class _Unspecified(object): pass

448 _unspecified = _Unspecified() 449

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

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] #MasterProxy does all the magic for us 466 except KeyError: 467 if default != _unspecified: 468 return default 469 else: 470 raise

471

472 -def get_param_names():

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() #MasterProxy does all the magic for us 484 if code != 1: 485 raise rospy.exceptions.ROSException("Unable to retrieve parameter names: %s"%msg) 486 else: 487 return val

488

489 -def set_param(param_name, param_value):

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 # #2202 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 #MasterProxy does all the magic for us

515

516 -def search_param(param_name):

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

531 -def delete_param(param_name):

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] #MasterProxy does all the magic for us

544

545 -def has_param(param_name):

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 #MasterProxy does all the magic for us

557

Source Code for Module rospy.client