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 # this test can be eliminated once we change from warning to error in the next check 262 if rosgraph.names.SEP in name: 263 raise ValueError("namespaces are not allowed in node names") 264 265 global _init_node_args 266 267 # #972: allow duplicate init_node args if calls are identical 268 # NOTE: we don't bother checking for node name aliases (e.g. 'foo' == '/foo'). 269 if _init_node_args: 270 if _init_node_args != (name, argv, anonymous, log_level, disable_rostime, disable_signals): 271 raise rospy.exceptions.ROSException("rospy.init_node() has already been called with different arguments: "+str(_init_node_args)) 272 else: 273 return #already initialized 274 275 # for scripting environments, we don't want to use the ROS signal 276 # handlers 277 disable_signals = disable_signals or roslib.is_interactive() 278 _init_node_args = (name, argv, anonymous, log_level, disable_rostime, disable_signals) 279 280 if not disable_signals: 281 # NOTE: register_signals must be called from main thread 282 rospy.core.register_signals() # add handlers for SIGINT/etc... 283 else: 284 logdebug("signal handlers for rospy disabled") 285 286 # check for name override 287 mappings = rospy.names.get_mappings() 288 if '__name' in mappings: 289 name = mappings['__name'] 290 if anonymous: 291 logdebug("[%s] WARNING: due to __name setting, anonymous setting is being changed to false"%name) 292 anonymous = False 293 294 if anonymous: 295 # not as good as a uuid/guid, but more readable. can't include 296 # hostname as that is not guaranteed to be a legal ROS name 297 name = "%s_%s_%s"%(name, os.getpid(), int(time.time()*1000)) 298 299 # check for legal base name once all changes have been made to the name 300 if not rosgraph.names.is_legal_base_name(name): 301 import warnings 302 warnings.warn("'%s' is not a legal ROS base name. This may cause problems with other ROS tools."%name, stacklevel=2) 303 304 # use rosgraph version of resolve_name to avoid remapping 305 resolved_node_name = rosgraph.names.resolve_name(name, rospy.core.get_caller_id()) 306 rospy.core.configure_logging(resolved_node_name) 307 # #1810 308 rospy.names.initialize_mappings(resolved_node_name) 309 310 logger = logging.getLogger("rospy.client") 311 logger.info("init_node, name[%s], pid[%s]", resolved_node_name, os.getpid()) 312 313 # node initialization blocks until registration with master 314 node = rospy.impl.init.start_node(os.environ, resolved_node_name, port=xmlrpc_port, tcpros_port=tcpros_port) 315 rospy.core.set_node_uri(node.uri) 316 rospy.core.add_shutdown_hook(node.shutdown) 317 318 if rospy.core.is_shutdown(): 319 logger.warn("aborting node initialization as shutdown has been triggered") 320 raise rospy.exceptions.ROSInitException("init_node interrupted before it could complete") 321 322 # upload private params (set via command-line) to parameter server 323 _init_node_params(argv, name) 324 325 rospy.core.set_initialized(True) 326 327 if not disable_rosout: 328 rospy.impl.rosout.init_rosout() 329 rospy.impl.rosout.load_rosout_handlers(log_level) 330 331 if not disable_rostime: 332 if not rospy.impl.simtime.init_simtime(): 333 raise rospy.exceptions.ROSInitException("Failed to initialize time. Please check logs for additional details") 334 else: 335 rospy.rostime.set_rostime_initialized(True) 336 337 logdebug("init_node, name[%s], pid[%s]", resolved_node_name, os.getpid()) 338 # advertise logging level services 339 Service('~get_loggers', GetLoggers, _get_loggers) 340 Service('~set_logger_level', SetLoggerLevel, _set_logger_level)

341 342 343 #_master_proxy is a MasterProxy wrapper 344 _master_proxy = None 345 _master_proxy_lock = Lock() 346

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

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

365 366 ######################################################### 367 # Topic helpers 368

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

370 """ 371 Retrieve list of topics that the master is reporting as being published. 372 373 @return: List of topic names and types: [[topic1, type1]...[topicN, typeN]] 374 @rtype: [[str, str]] 375 """ 376 code, msg, val = get_master().getPublishedTopics(namespace) 377 if code != 1: 378 raise rospy.exceptions.ROSException("unable to get published topics: %s"%msg) 379 return val

380

381 -class _WFM(object):

382 - def __init__(self):

383 self.msg = None

384 - def cb(self, msg):

385 if self.msg is None: 386 self.msg = msg

387

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

389 """ 390 Receive one message from topic. 391 392 This will create a new subscription to the topic, receive one message, then unsubscribe. 393 394 @param topic: name of topic 395 @type topic: str 396 @param topic_type: topic type 397 @type topic_type: L{rospy.Message} class 398 @param timeout: timeout time in seconds 399 @type timeout: double 400 @return: Message 401 @rtype: L{rospy.Message} 402 @raise ROSException: if specified timeout is exceeded 403 @raise ROSInterruptException: if shutdown interrupts wait 404 """ 405 wfm = _WFM() 406 s = None 407 try: 408 s = rospy.topics.Subscriber(topic, topic_type, wfm.cb) 409 if timeout is not None: 410 timeout_t = time.time() + timeout 411 while not rospy.core.is_shutdown() and wfm.msg is None: 412 rospy.rostime.wallsleep(0.01) 413 if time.time() >= timeout_t: 414 raise rospy.exceptions.ROSException("timeout exceeded while waiting for message on topic %s"%topic) 415 416 else: 417 while not rospy.core.is_shutdown() and wfm.msg is None: 418 rospy.rostime.wallsleep(0.01) 419 finally: 420 if s is not None: 421 s.unregister() 422 if rospy.core.is_shutdown(): 423 raise rospy.exceptions.ROSInterruptException("rospy shutdown") 424 return wfm.msg

425 426 427 ######################################################### 428 # Param Server Access 429 430 _param_server = None 431 _param_server_lock = Lock()

432 -def _init_param_server():

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

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

444 -class _Unspecified(object): pass

445 _unspecified = _Unspecified() 446

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

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

468

469 -def get_param_names():

470 """ 471 Retrieve list of parameter names. 472 473 NOTE: this method is thread-safe. 474 475 @return: parameter names 476 @rtype: [str] 477 @raise ROSException: if parameter server reports an error 478 """ 479 _init_param_server() 480 code, msg, val = _param_server.getParamNames() #MasterProxy does all the magic for us 481 if code != 1: 482 raise rospy.exceptions.ROSException("Unable to retrieve parameter names: %s"%msg) 483 else: 484 return val

485

486 -def set_param(param_name, param_value):

487 """ 488 Set a parameter on the param server 489 490 NOTE: this method is thread-safe. 491 492 @param param_name: parameter name 493 @type param_name: str 494 @param param_value: parameter value 495 @type param_value: XmlRpcLegalValue 496 @raise ROSException: if parameter server reports an error 497 """ 498 # #2202 499 if not rosgraph.names.is_legal_name(param_name): 500 import warnings 501 warnings.warn("'%s' is not a legal ROS graph resource name. This may cause problems with other ROS tools"%param_name, stacklevel=2) 502 503 _init_param_server() 504 _param_server[param_name] = param_value #MasterProxy does all the magic for us

505

506 -def search_param(param_name):

507 """ 508 Search for a parameter on the param server 509 510 NOTE: this method is thread-safe. 511 512 @param param_name: parameter name 513 @type param_name: str 514 @return: key of matching parameter or None if no matching parameter. 515 @rtype: str 516 @raise ROSException: if parameter server reports an error 517 """ 518 _init_param_server() 519 return _param_server.search_param(param_name)

520

521 -def delete_param(param_name):

522 """ 523 Delete a parameter on the param server 524 525 NOTE: this method is thread-safe. 526 527 @param param_name: parameter name 528 @type param_name: str 529 @raise KeyError: if parameter is not set 530 @raise ROSException: if parameter server reports an error 531 """ 532 _init_param_server() 533 del _param_server[param_name] #MasterProxy does all the magic for us

534

535 -def has_param(param_name):

536 """ 537 Test if parameter exists on the param server 538 539 NOTE: this method is thread-safe. 540 541 @param param_name: parameter name 542 @type param_name: str 543 @raise ROSException: if parameter server reports an error 544 """ 545 _init_param_server() 546 return param_name in _param_server #MasterProxy does all the magic for us

547

Source Code for Module rospy.client