Package rospy :: Module core

Source Code for Module rospy.core

  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  """rospy internal core implementation library""" 
 36   
 37   
 38   
 39  import atexit 
 40  try: 
 41      import cPickle as pickle 
 42  except ImportError: 
 43      import pickle 
 44  import inspect 
 45  import logging 
 46  from hashlib import md5 
 47  import os 
 48  import signal 
 49  import sys 
 50  import threading 
 51  import time 
 52  import traceback 
 53  import types 
 54   
 55  try: 
 56      import urllib.parse as urlparse #Python 3.x 
 57  except ImportError: 
 58      import urlparse 
 59   
 60  try: 
 61      import xmlrpc.client as xmlrpcclient #Python 3.x 
 62  except ImportError: 
 63      import xmlrpclib as xmlrpcclient #Python 2.x 
 64   
 65  import rospkg 
 66   
 67  import rosgraph.roslogging 
 68   
 69  import rospy.exceptions 
 70  import rospy.rostime 
 71   
 72  from rospy.names import * 
 73  from rospy.impl.validators import ParameterInvalid 
 74   
 75  from rosgraph_msgs.msg import Log 
 76  from functools import partial 
 77   
 78  _logger = logging.getLogger("rospy.core") 
 79   
 80  # number of seconds to wait to join on threads. network issue can 
 81  # cause joins to be not terminate gracefully, and it's better to 
 82  # teardown dirty than to hang 
 83  _TIMEOUT_SHUTDOWN_JOIN = 5. 
 84   
 85  import warnings 
86 -def deprecated(func):
87 """This is a decorator which can be used to mark functions 88 as deprecated. It will result in a warning being emitted 89 when the function is used.""" 90 def newFunc(*args, **kwargs): 91 warnings.warn("Call to deprecated function %s." % func.__name__, 92 category=DeprecationWarning, stacklevel=2) 93 return func(*args, **kwargs)
94 newFunc.__name__ = func.__name__ 95 newFunc.__doc__ = func.__doc__ 96 newFunc.__dict__.update(func.__dict__) 97 return newFunc 98 99 ######################################################### 100 # ROSRPC 101 102 ROSRPC = "rosrpc://"
103 104 -def parse_rosrpc_uri(uri):
105 """ 106 utility function for parsing ROS-RPC URIs 107 @param uri: ROSRPC URI 108 @type uri: str 109 @return: address, port 110 @rtype: (str, int) 111 @raise ParameterInvalid: if uri is not a valid ROSRPC URI 112 """ 113 if uri.startswith(ROSRPC): 114 dest_addr = uri[len(ROSRPC):] 115 else: 116 raise ParameterInvalid("Invalid protocol for ROS service URL: %s"%uri) 117 try: 118 if '/' in dest_addr: 119 dest_addr = dest_addr[:dest_addr.find('/')] 120 dest_addr, dest_port = dest_addr.split(':') 121 dest_port = int(dest_port) 122 except: 123 raise ParameterInvalid("ROS service URL is invalid: %s"%uri) 124 return dest_addr, dest_port
125 126 ######################################################### 127 128 # rospy logger 129 _rospy_logger = logging.getLogger("rospy.internal")
130 131 # we keep a separate, non-rosout log file to contain stack traces and 132 # other sorts of information that scare users but are essential for 133 # debugging 134 135 -def rospydebug(msg, *args, **kwargs):
136 """Internal rospy client library debug logging""" 137 _rospy_logger.debug(msg, *args, **kwargs)
138 -def rospyinfo(msg, *args, **kwargs):
139 """Internal rospy client library debug logging""" 140 _rospy_logger.info(msg, *args, **kwargs)
141 -def rospyerr(msg, *args, **kwargs):
142 """Internal rospy client library error logging""" 143 _rospy_logger.error(msg, *args, **kwargs)
144 -def rospywarn(msg, *args, **kwargs):
145 """Internal rospy client library warn logging""" 146 _rospy_logger.warn(msg, *args, **kwargs)
147
148 149 -def _frame_to_caller_id(frame):
150 caller_id = ( 151 inspect.getabsfile(frame), 152 frame.f_lineno, 153 frame.f_lasti, 154 ) 155 return pickle.dumps(caller_id)
156
157 158 -def _base_logger(msg, args, kwargs, throttle=None, 159 throttle_identical=False, level=None, once=False):
160 161 rospy_logger = logging.getLogger('rosout') 162 name = kwargs.pop('logger_name', None) 163 if name: 164 rospy_logger = rospy_logger.getChild(name) 165 logfunc = getattr(rospy_logger, level) 166 167 if once: 168 caller_id = _frame_to_caller_id(inspect.currentframe().f_back.f_back) 169 if _logging_once(caller_id): 170 logfunc(msg, *args, **kwargs) 171 elif throttle_identical: 172 caller_id = _frame_to_caller_id(inspect.currentframe().f_back.f_back) 173 throttle_elapsed = False 174 if throttle is not None: 175 throttle_elapsed = _logging_throttle(caller_id, throttle) 176 if _logging_identical(caller_id, msg) or throttle_elapsed: 177 logfunc(msg, *args, **kwargs) 178 elif throttle: 179 caller_id = _frame_to_caller_id(inspect.currentframe().f_back.f_back) 180 if _logging_throttle(caller_id, throttle): 181 logfunc(msg, *args, **kwargs) 182 else: 183 logfunc(msg, *args, **kwargs)
184
185 186 -def logdebug(msg, *args, **kwargs):
187 _base_logger(msg, args, kwargs, level='debug')
188
189 -def loginfo(msg, *args, **kwargs):
190 _base_logger(msg, args, kwargs, level='info')
191
192 -def logwarn(msg, *args, **kwargs):
193 _base_logger(msg, args, kwargs, level='warn')
194
195 -def logerr(msg, *args, **kwargs):
196 _base_logger(msg, args, kwargs, level='error')
197
198 -def logfatal(msg, *args, **kwargs):
199 _base_logger(msg, args, kwargs, level='critical')
200 201 logout = loginfo # alias deprecated name 202 203 logerror = logerr # alias logerr
204 205 206 -class LoggingThrottle(object):
207 208 last_logging_time_table = {} 209
210 - def __call__(self, caller_id, period):
211 """Do logging specified message periodically. 212 213 - caller_id (str): Id to identify the caller 214 - logging_func (function): Function to do logging. 215 - period (float): Period to do logging in second unit. 216 - msg (object): Message to do logging. 217 """ 218 now = rospy.Time.now() 219 220 last_logging_time = self.last_logging_time_table.get(caller_id) 221 222 if (last_logging_time is None or 223 (now - last_logging_time) > rospy.Duration(period)): 224 self.last_logging_time_table[caller_id] = now 225 return True 226 elif last_logging_time > now: 227 self.last_logging_time_table = {} 228 self.last_logging_time_table[caller_id] = now 229 return True 230 return False
231 232 233 _logging_throttle = LoggingThrottle()
234 235 236 -def logdebug_throttle(period, msg, *args, **kwargs):
237 _base_logger(msg, args, kwargs, throttle=period, level='debug')
238
239 -def loginfo_throttle(period, msg, *args, **kwargs):
240 _base_logger(msg, args, kwargs, throttle=period, level='info')
241
242 -def logwarn_throttle(period, msg, *args, **kwargs):
243 _base_logger(msg, args, kwargs, throttle=period, level='warn')
244
245 -def logerr_throttle(period, msg, *args, **kwargs):
246 _base_logger(msg, args, kwargs, throttle=period, level='error')
247
248 -def logfatal_throttle(period, msg, *args, **kwargs):
249 _base_logger(msg, args, kwargs, throttle=period, level='critical')
250
251 252 -class LoggingIdentical(object):
253 254 last_logging_msg_table = {} 255
256 - def __call__(self, caller_id, msg):
257 """Do logging specified message only if distinct from last message. 258 259 - caller_id (str): Id to identify the caller 260 - msg (str): Contents of message to log 261 """ 262 msg_hash = md5(msg.encode()).hexdigest() 263 264 if msg_hash != self.last_logging_msg_table.get(caller_id): 265 self.last_logging_msg_table[caller_id] = msg_hash 266 return True 267 return False
268 269 270 _logging_identical = LoggingIdentical()
271 272 273 -def logdebug_throttle_identical(period, msg, *args, **kwargs):
274 _base_logger(msg, args, kwargs, throttle=period, throttle_identical=True, 275 level='debug')
276
277 -def loginfo_throttle_identical(period, msg, *args, **kwargs):
278 _base_logger(msg, args, kwargs, throttle=period, throttle_identical=True, 279 level='info')
280
281 -def logwarn_throttle_identical(period, msg, *args, **kwargs):
282 _base_logger(msg, args, kwargs, throttle=period, throttle_identical=True, 283 level='warn')
284
285 -def logerr_throttle_identical(period, msg, *args, **kwargs):
286 _base_logger(msg, args, kwargs, throttle=period, throttle_identical=True, 287 level='error')
288
289 -def logfatal_throttle_identical(period, msg, *args, **kwargs):
290 _base_logger(msg, args, kwargs, throttle=period, throttle_identical=True, 291 level='critical')
292
293 294 -class LoggingOnce(object):
295 296 called_caller_ids = set() 297
298 - def __call__(self, caller_id):
299 if caller_id not in self.called_caller_ids: 300 self.called_caller_ids.add(caller_id) 301 return True 302 return False
303 304 _logging_once = LoggingOnce()
305 306 307 -def logdebug_once(msg, *args, **kwargs):
308 _base_logger(msg, args, kwargs, once=True, level='debug')
309
310 -def loginfo_once(msg, *args, **kwargs):
311 _base_logger(msg, args, kwargs, once=True, level='info')
312
313 -def logwarn_once(msg, *args, **kwargs):
314 _base_logger(msg, args, kwargs, once=True, level='warn')
315
316 -def logerr_once(msg, *args, **kwargs):
317 _base_logger(msg, args, kwargs, once=True, level='error')
318
319 -def logfatal_once(msg, *args, **kwargs):
320 _base_logger(msg, args, kwargs, once=True, level='critical')
321 322 323 ######################################################### 324 # CONSTANTS 325 326 MASTER_NAME = "master" #master is a reserved node name for the central master 327 328 import warnings 329 import functools
330 -def deprecated(func):
331 """This is a decorator which can be used to mark functions 332 as deprecated. It will result in a warning being emitted 333 when the function is used.""" 334 @functools.wraps(func) 335 def newFunc(*args, **kwargs): 336 warnings.warn("Call to deprecated function %s." % func.__name__, 337 category=DeprecationWarning, stacklevel=2) 338 return func(*args, **kwargs)
339 return newFunc 340
341 @deprecated 342 -def get_ros_root(required=False, env=None):
343 """ 344 Get the value of ROS_ROOT. 345 @param env: override environment dictionary 346 @type env: dict 347 @param required: if True, fails with ROSException 348 @return: Value of ROS_ROOT environment 349 @rtype: str 350 @raise ROSException: if require is True and ROS_ROOT is not set 351 """ 352 if env is None: 353 env = os.environ 354 ros_root = rospkg.get_ros_root(env) 355 if required and not ros_root: 356 raise rospy.exceptions.ROSException('%s is not set'%rospkg.environment.ROS_ROOT) 357 return ros_root
358 359 360 ######################################################### 361 # API 362 363 _uri = None
364 -def get_node_uri():
365 """ 366 Get this Node's URI. 367 @return: this Node's XMLRPC URI 368 @rtype: str 369 """ 370 return _uri
371
372 -def set_node_uri(uri):
373 """set the URI of the local node. 374 This is an internal API method, it does not actually affect the XMLRPC URI of the Node.""" 375 global _uri 376 _uri = uri
377 378 ######################################################### 379 # Logging 380 381 _log_filename = None
382 -def configure_logging(node_name, level=logging.INFO):
383 """ 384 Setup filesystem logging for this node 385 @param node_name: Node's name 386 @type node_name str 387 @param level: (optional) Python logging level (INFO, DEBUG, etc...). (Default: logging.INFO) 388 @type level: int 389 """ 390 global _log_filename 391 392 # #988 __log command-line remapping argument 393 mappings = get_mappings() 394 if '__log' in get_mappings(): 395 logfilename_remap = mappings['__log'] 396 filename = os.path.abspath(logfilename_remap) 397 else: 398 # fix filesystem-unsafe chars 399 suffix = '.log' 400 filename = node_name.replace('/', '_') + suffix 401 if filename[0] == '_': 402 filename = filename[1:] 403 if filename == suffix: 404 raise rospy.exceptions.ROSException('invalid configure_logging parameter: %s'%node_name) 405 _log_filename = rosgraph.roslogging.configure_logging('rospy', level, filename=filename)
406
407 -class NullHandler(logging.Handler):
408 - def emit(self, record):
409 pass
410 411 # keep logging happy until we have the node name to configure with 412 logging.getLogger('rospy').addHandler(NullHandler()) 413 414 415 ######################################################### 416 # Init/Shutdown/Exit API and Handlers 417 418 _client_ready = False
419 420 421 -def is_initialized():
422 """ 423 Get the initialization state of the local node. If True, node has 424 been configured. 425 @return: True if local node initialized 426 @rtype: bool 427 """ 428 return _client_ready
429 -def set_initialized(initialized):
430 """ 431 set the initialization state of the local node 432 @param initialized: True if node initialized 433 @type initialized: bool 434 """ 435 global _client_ready 436 _client_ready = initialized
437 438 _shutdown_lock = threading.RLock() 439 440 # _shutdown_flag flags that rospy is in shutdown mode, in_shutdown 441 # flags that the shutdown routine has started. These are separate 442 # because 'pre-shutdown' hooks require rospy to be in a non-shutdown 443 # mode. These hooks are executed during the shutdown routine. 444 _shutdown_flag = False 445 _in_shutdown = False 446 447 # various hooks to call on shutdown. shutdown hooks are called in the 448 # shutdown state, preshutdown are called just before entering shutdown 449 # state, and client shutdown is called before both of these. 450 _shutdown_hooks = [] 451 _preshutdown_hooks = [] 452 _client_shutdown_hooks = [] 453 # threads that must be joined on shutdown 454 _shutdown_threads = [] 455 456 _signalChain = {}
457 458 -def is_shutdown():
459 """ 460 @return: True if shutdown flag has been set 461 @rtype: bool 462 """ 463 return _shutdown_flag
464
465 -def is_shutdown_requested():
466 """ 467 is_shutdown_requested is a state that occurs just before 468 is_shutdown. It is initiated when a shutdown requested is 469 received and continues until client shutdown handlers have been 470 called. After client shutdown handlers have been serviced, the 471 is_shutdown state becomes true. 472 473 @return: True if shutdown has been requested (but possibly not yet initiated) 474 @rtype: bool 475 """ 476 return _in_shutdown
477
478 -def _add_shutdown_hook(h, hooks, pass_reason_argument=True):
479 """ 480 shared implementation of add_shutdown_hook and add_preshutdown_hook 481 """ 482 if not callable(h): 483 raise TypeError("shutdown hook [%s] must be a function or callable object: %s"%(h, type(h))) 484 if _shutdown_flag: 485 _logger.warn("add_shutdown_hook called after shutdown") 486 if pass_reason_argument: 487 h("already shutdown") 488 else: 489 h() 490 return 491 with _shutdown_lock: 492 if hooks is None: 493 # race condition check, don't log as we are deep into shutdown 494 return 495 hooks.append(h)
496
497 -def _add_shutdown_thread(t):
498 """ 499 Register thread that must be joined() on shutdown 500 """ 501 if _shutdown_flag: 502 #TODO 503 return 504 with _shutdown_lock: 505 if _shutdown_threads is None: 506 # race condition check, don't log as we are deep into shutdown 507 return 508 # in order to prevent memory leaks, reap dead threads. The 509 # last thread may not get reaped until shutdown, but this is 510 # relatively minor 511 for other in _shutdown_threads[:]: 512 if not other.is_alive(): 513 _shutdown_threads.remove(other) 514 _shutdown_threads.append(t)
515
516 -def add_client_shutdown_hook(h):
517 """ 518 Add client method to invoke when system shuts down. Unlike 519 L{add_shutdown_hook} and L{add_preshutdown_hooks}, these methods 520 will be called before any rospy internal shutdown code. 521 522 @param h: function with zero args 523 @type h: fn() 524 """ 525 _add_shutdown_hook(h, _client_shutdown_hooks, pass_reason_argument=False)
526
527 -def add_preshutdown_hook(h):
528 """ 529 Add method to invoke when system shuts down. Unlike 530 L{add_shutdown_hook}, these methods will be called before any 531 other shutdown hooks. 532 533 @param h: function that takes in a single string argument (shutdown reason) 534 @type h: fn(str) 535 """ 536 _add_shutdown_hook(h, _preshutdown_hooks)
537
538 -def add_shutdown_hook(h):
539 """ 540 Add method to invoke when system shuts down. 541 542 Shutdown hooks are called in the order that they are 543 registered. This is an internal API method that is used to 544 cleanup. See the client X{on_shutdown()} method if you wish to 545 register client hooks. 546 547 @param h: function that takes in a single string argument (shutdown reason) 548 @type h: fn(str) 549 """ 550 _add_shutdown_hook(h, _shutdown_hooks)
551
552 -def signal_shutdown(reason):
553 """ 554 Initiates shutdown process by signaling objects waiting on _shutdown_lock. 555 Shutdown and pre-shutdown hooks are invoked. 556 @param reason: human-readable shutdown reason, if applicable 557 @type reason: str 558 """ 559 global _shutdown_flag, _in_shutdown, _shutdown_lock, _shutdown_hooks 560 _logger.info("signal_shutdown [%s]"%reason) 561 if _shutdown_flag or _in_shutdown: 562 return 563 with _shutdown_lock: 564 if _shutdown_flag or _in_shutdown: 565 return 566 _in_shutdown = True 567 568 # make copy just in case client re-invokes shutdown 569 for h in _client_shutdown_hooks: 570 try: 571 # client shutdown hooks do not accept a reason arg 572 h() 573 except: 574 traceback.print_exc() 575 del _client_shutdown_hooks[:] 576 577 for h in _preshutdown_hooks: 578 try: 579 h(reason) 580 except: 581 traceback.print_exc() 582 del _preshutdown_hooks[:] 583 584 # now that pre-shutdown hooks have been called, raise shutdown 585 # flag. This allows preshutdown hooks to still publish and use 586 # service calls properly 587 _shutdown_flag = True 588 for h in _shutdown_hooks: 589 try: 590 h(reason) 591 except Exception as e: 592 sys.stderr.write("signal_shutdown hook error[%s]\n"%e) 593 del _shutdown_hooks[:] 594 595 threads = _shutdown_threads[:] 596 597 for t in threads: 598 if t.is_alive(): 599 t.join(_TIMEOUT_SHUTDOWN_JOIN) 600 del _shutdown_threads[:] 601 try: 602 rospy.rostime.wallsleep(0.1) #hack for now until we get rid of all the extra threads 603 except KeyboardInterrupt: pass
604
605 -def _ros_signal(sig, stackframe):
606 signal_shutdown("signal-"+str(sig)) 607 prev_handler = _signalChain.get(sig, None) 608 if callable(prev_handler): 609 try: 610 prev_handler(sig, stackframe) 611 except KeyboardInterrupt: 612 pass #filter out generic keyboard interrupt handler
613
614 -def _ros_atexit():
615 signal_shutdown('atexit')
616 atexit.register(_ros_atexit)
617 618 # #687 619 -def register_signals():
620 """ 621 register system signal handlers for SIGTERM and SIGINT 622 """ 623 _signalChain[signal.SIGTERM] = signal.signal(signal.SIGTERM, _ros_signal) 624 _signalChain[signal.SIGINT] = signal.signal(signal.SIGINT, _ros_signal)
625
626 # Validators ###################################### 627 628 -def is_topic(param_name):
629 """ 630 Validator that checks that parameter is a valid ROS topic name 631 """ 632 def validator(param_value, caller_id): 633 v = valid_name_validator_resolved(param_name, param_value, caller_id) 634 if param_value == '/': 635 raise ParameterInvalid("ERROR: parameter [%s] cannot be the global namespace"%param_name) 636 return v
637 return validator 638 639 _xmlrpc_cache = {} 640 _xmlrpc_lock = threading.Lock()
641 642 -def xmlrpcapi(uri, cache=True):
643 """ 644 @return: instance for calling remote server or None if not a valid URI 645 @rtype: xmlrpclib.ServerProxy 646 """ 647 if uri is None: 648 return None 649 uriValidate = urlparse.urlparse(uri) 650 if not uriValidate[0] or not uriValidate[1]: 651 return None 652 if not cache: 653 return xmlrpcclient.ServerProxy(uri) 654 if uri not in _xmlrpc_cache: 655 with _xmlrpc_lock: 656 if uri not in _xmlrpc_cache: # allows lazy locking 657 _xmlrpc_cache[uri] = _LockedServerProxy(uri) 658 return _xmlrpc_cache[uri]
659
660 661 -class _LockedServerProxy(xmlrpcclient.ServerProxy):
662
663 - def __init__(self, *args, **kwargs):
664 xmlrpcclient.ServerProxy.__init__(self, *args, **kwargs) 665 self._lock = threading.Lock()
666
667 - def _ServerProxy__request(self, methodname, params):
668 with self._lock: 669 return xmlrpcclient.ServerProxy._ServerProxy__request( 670 self, methodname, params)
671