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 emmitted 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 return False
227 228 229 _logging_throttle = LoggingThrottle()
230 231 232 -def logdebug_throttle(period, msg, *args, **kwargs):
233 _base_logger(msg, args, kwargs, throttle=period, level='debug')
234
235 -def loginfo_throttle(period, msg, *args, **kwargs):
236 _base_logger(msg, args, kwargs, throttle=period, level='info')
237
238 -def logwarn_throttle(period, msg, *args, **kwargs):
239 _base_logger(msg, args, kwargs, throttle=period, level='warn')
240
241 -def logerr_throttle(period, msg, *args, **kwargs):
242 _base_logger(msg, args, kwargs, throttle=period, level='error')
243
244 -def logfatal_throttle(period, msg, *args, **kwargs):
245 _base_logger(msg, args, kwargs, throttle=period, level='critical')
246
247 248 -class LoggingIdentical(object):
249 250 last_logging_msg_table = {} 251
252 - def __call__(self, caller_id, msg):
253 """Do logging specified message only if distinct from last message. 254 255 - caller_id (str): Id to identify the caller 256 - msg (str): Contents of message to log 257 """ 258 msg_hash = md5(msg.encode()).hexdigest() 259 260 if msg_hash != self.last_logging_msg_table.get(caller_id): 261 self.last_logging_msg_table[caller_id] = msg_hash 262 return True 263 return False
264 265 266 _logging_identical = LoggingIdentical()
267 268 269 -def logdebug_throttle_identical(period, msg, *args, **kwargs):
270 _base_logger(msg, args, kwargs, throttle=period, throttle_identical=True, 271 level='debug')
272
273 -def loginfo_throttle_identical(period, msg, *args, **kwargs):
274 _base_logger(msg, args, kwargs, throttle=period, throttle_identical=True, 275 level='info')
276
277 -def logwarn_throttle_identical(period, msg, *args, **kwargs):
278 _base_logger(msg, args, kwargs, throttle=period, throttle_identical=True, 279 level='warn')
280
281 -def logerr_throttle_identical(period, msg, *args, **kwargs):
282 _base_logger(msg, args, kwargs, throttle=period, throttle_identical=True, 283 level='error')
284
285 -def logfatal_throttle_identical(period, msg, *args, **kwargs):
286 _base_logger(msg, args, kwargs, throttle=period, throttle_identical=True, 287 level='critical')
288
289 290 -class LoggingOnce(object):
291 292 called_caller_ids = set() 293
294 - def __call__(self, caller_id):
295 if caller_id not in self.called_caller_ids: 296 self.called_caller_ids.add(caller_id) 297 return True 298 return False
299 300 _logging_once = LoggingOnce()
301 302 303 -def logdebug_once(msg, *args, **kwargs):
304 _base_logger(msg, args, kwargs, once=True, level='debug')
305
306 -def loginfo_once(msg, *args, **kwargs):
307 _base_logger(msg, args, kwargs, once=True, level='info')
308
309 -def logwarn_once(msg, *args, **kwargs):
310 _base_logger(msg, args, kwargs, once=True, level='warn')
311
312 -def logerr_once(msg, *args, **kwargs):
313 _base_logger(msg, args, kwargs, once=True, level='error')
314
315 -def logfatal_once(msg, *args, **kwargs):
316 _base_logger(msg, args, kwargs, once=True, level='critical')
317 318 319 ######################################################### 320 # CONSTANTS 321 322 MASTER_NAME = "master" #master is a reserved node name for the central master 323 324 import warnings 325 import functools
326 -def deprecated(func):
327 """This is a decorator which can be used to mark functions 328 as deprecated. It will result in a warning being emmitted 329 when the function is used.""" 330 @functools.wraps(func) 331 def newFunc(*args, **kwargs): 332 warnings.warn("Call to deprecated function %s." % func.__name__, 333 category=DeprecationWarning, stacklevel=2) 334 return func(*args, **kwargs)
335 return newFunc 336
337 @deprecated 338 -def get_ros_root(required=False, env=None):
339 """ 340 Get the value of ROS_ROOT. 341 @param env: override environment dictionary 342 @type env: dict 343 @param required: if True, fails with ROSException 344 @return: Value of ROS_ROOT environment 345 @rtype: str 346 @raise ROSException: if require is True and ROS_ROOT is not set 347 """ 348 if env is None: 349 env = os.environ 350 ros_root = rospkg.get_ros_root(env) 351 if required and not ros_root: 352 raise rospy.exceptions.ROSException('%s is not set'%rospkg.environment.ROS_ROOT) 353 return ros_root
354 355 356 ######################################################### 357 # API 358 359 _uri = None
360 -def get_node_uri():
361 """ 362 Get this Node's URI. 363 @return: this Node's XMLRPC URI 364 @rtype: str 365 """ 366 return _uri
367
368 -def set_node_uri(uri):
369 """set the URI of the local node. 370 This is an internal API method, it does not actually affect the XMLRPC URI of the Node.""" 371 global _uri 372 _uri = uri
373 374 ######################################################### 375 # Logging 376 377 _log_filename = None
378 -def configure_logging(node_name, level=logging.INFO):
379 """ 380 Setup filesystem logging for this node 381 @param node_name: Node's name 382 @type node_name str 383 @param level: (optional) Python logging level (INFO, DEBUG, etc...). (Default: logging.INFO) 384 @type level: int 385 """ 386 global _log_filename 387 388 # #988 __log command-line remapping argument 389 mappings = get_mappings() 390 if '__log' in get_mappings(): 391 logfilename_remap = mappings['__log'] 392 filename = os.path.abspath(logfilename_remap) 393 else: 394 # fix filesystem-unsafe chars 395 suffix = '.log' 396 filename = node_name.replace('/', '_') + suffix 397 if filename[0] == '_': 398 filename = filename[1:] 399 if filename == suffix: 400 raise rospy.exceptions.ROSException('invalid configure_logging parameter: %s'%node_name) 401 _log_filename = rosgraph.roslogging.configure_logging('rospy', level, filename=filename)
402
403 -class NullHandler(logging.Handler):
404 - def emit(self, record):
405 pass
406 407 # keep logging happy until we have the node name to configure with 408 logging.getLogger('rospy').addHandler(NullHandler()) 409 410 411 ######################################################### 412 # Init/Shutdown/Exit API and Handlers 413 414 _client_ready = False
415 416 417 -def is_initialized():
418 """ 419 Get the initialization state of the local node. If True, node has 420 been configured. 421 @return: True if local node initialized 422 @rtype: bool 423 """ 424 return _client_ready
425 -def set_initialized(initialized):
426 """ 427 set the initialization state of the local node 428 @param initialized: True if node initialized 429 @type initialized: bool 430 """ 431 global _client_ready 432 _client_ready = initialized
433 434 _shutdown_lock = threading.RLock() 435 436 # _shutdown_flag flags that rospy is in shutdown mode, in_shutdown 437 # flags that the shutdown routine has started. These are separate 438 # because 'pre-shutdown' hooks require rospy to be in a non-shutdown 439 # mode. These hooks are executed during the shutdown routine. 440 _shutdown_flag = False 441 _in_shutdown = False 442 443 # various hooks to call on shutdown. shutdown hooks are called in the 444 # shutdown state, preshutdown are called just before entering shutdown 445 # state, and client shutdown is called before both of these. 446 _shutdown_hooks = [] 447 _preshutdown_hooks = [] 448 _client_shutdown_hooks = [] 449 # threads that must be joined on shutdown 450 _shutdown_threads = [] 451 452 _signalChain = {}
453 454 -def is_shutdown():
455 """ 456 @return: True if shutdown flag has been set 457 @rtype: bool 458 """ 459 return _shutdown_flag
460
461 -def is_shutdown_requested():
462 """ 463 is_shutdown_requested is a state that occurs just before 464 is_shutdown. It is initiated when a shutdown requested is 465 received and continues until client shutdown handlers have been 466 called. After client shutdown handlers have been serviced, the 467 is_shutdown state becomes true. 468 469 @return: True if shutdown has been requested (but possibly not yet initiated) 470 @rtype: bool 471 """ 472 return _in_shutdown
473
474 -def _add_shutdown_hook(h, hooks, pass_reason_argument=True):
475 """ 476 shared implementation of add_shutdown_hook and add_preshutdown_hook 477 """ 478 if not callable(h): 479 raise TypeError("shutdown hook [%s] must be a function or callable object: %s"%(h, type(h))) 480 if _shutdown_flag: 481 _logger.warn("add_shutdown_hook called after shutdown") 482 if pass_reason_argument: 483 h("already shutdown") 484 else: 485 h() 486 return 487 with _shutdown_lock: 488 if hooks is None: 489 # race condition check, don't log as we are deep into shutdown 490 return 491 hooks.append(h)
492
493 -def _add_shutdown_thread(t):
494 """ 495 Register thread that must be joined() on shutdown 496 """ 497 if _shutdown_flag: 498 #TODO 499 return 500 with _shutdown_lock: 501 if _shutdown_threads is None: 502 # race condition check, don't log as we are deep into shutdown 503 return 504 # in order to prevent memory leaks, reap dead threads. The 505 # last thread may not get reaped until shutdown, but this is 506 # relatively minor 507 for other in _shutdown_threads[:]: 508 if not other.isAlive(): 509 _shutdown_threads.remove(other) 510 _shutdown_threads.append(t)
511
512 -def add_client_shutdown_hook(h):
513 """ 514 Add client method to invoke when system shuts down. Unlike 515 L{add_shutdown_hook} and L{add_preshutdown_hooks}, these methods 516 will be called before any rospy internal shutdown code. 517 518 @param h: function with zero args 519 @type h: fn() 520 """ 521 _add_shutdown_hook(h, _client_shutdown_hooks, pass_reason_argument=False)
522
523 -def add_preshutdown_hook(h):
524 """ 525 Add method to invoke when system shuts down. Unlike 526 L{add_shutdown_hook}, these methods will be called before any 527 other shutdown hooks. 528 529 @param h: function that takes in a single string argument (shutdown reason) 530 @type h: fn(str) 531 """ 532 _add_shutdown_hook(h, _preshutdown_hooks)
533
534 -def add_shutdown_hook(h):
535 """ 536 Add method to invoke when system shuts down. 537 538 Shutdown hooks are called in the order that they are 539 registered. This is an internal API method that is used to 540 cleanup. See the client X{on_shutdown()} method if you wish to 541 register client hooks. 542 543 @param h: function that takes in a single string argument (shutdown reason) 544 @type h: fn(str) 545 """ 546 _add_shutdown_hook(h, _shutdown_hooks)
547
548 -def signal_shutdown(reason):
549 """ 550 Initiates shutdown process by signaling objects waiting on _shutdown_lock. 551 Shutdown and pre-shutdown hooks are invoked. 552 @param reason: human-readable shutdown reason, if applicable 553 @type reason: str 554 """ 555 global _shutdown_flag, _in_shutdown, _shutdown_lock, _shutdown_hooks 556 _logger.info("signal_shutdown [%s]"%reason) 557 if _shutdown_flag or _in_shutdown: 558 return 559 with _shutdown_lock: 560 if _shutdown_flag or _in_shutdown: 561 return 562 _in_shutdown = True 563 564 # make copy just in case client re-invokes shutdown 565 for h in _client_shutdown_hooks: 566 try: 567 # client shutdown hooks do not accept a reason arg 568 h() 569 except: 570 traceback.print_exc() 571 del _client_shutdown_hooks[:] 572 573 for h in _preshutdown_hooks: 574 try: 575 h(reason) 576 except: 577 traceback.print_exc() 578 del _preshutdown_hooks[:] 579 580 # now that pre-shutdown hooks have been called, raise shutdown 581 # flag. This allows preshutdown hooks to still publish and use 582 # service calls properly 583 _shutdown_flag = True 584 for h in _shutdown_hooks: 585 try: 586 h(reason) 587 except Exception as e: 588 sys.stderr.write("signal_shutdown hook error[%s]\n"%e) 589 del _shutdown_hooks[:] 590 591 threads = _shutdown_threads[:] 592 593 for t in threads: 594 if t.isAlive(): 595 t.join(_TIMEOUT_SHUTDOWN_JOIN) 596 del _shutdown_threads[:] 597 try: 598 rospy.rostime.wallsleep(0.1) #hack for now until we get rid of all the extra threads 599 except KeyboardInterrupt: pass
600
601 -def _ros_signal(sig, stackframe):
602 signal_shutdown("signal-"+str(sig)) 603 prev_handler = _signalChain.get(sig, None) 604 if callable(prev_handler): 605 try: 606 prev_handler(sig, stackframe) 607 except KeyboardInterrupt: 608 pass #filter out generic keyboard interrupt handler
609
610 -def _ros_atexit():
611 signal_shutdown('atexit')
612 atexit.register(_ros_atexit)
613 614 # #687 615 -def register_signals():
616 """ 617 register system signal handlers for SIGTERM and SIGINT 618 """ 619 _signalChain[signal.SIGTERM] = signal.signal(signal.SIGTERM, _ros_signal) 620 _signalChain[signal.SIGINT] = signal.signal(signal.SIGINT, _ros_signal)
621
622 # Validators ###################################### 623 624 -def is_topic(param_name):
625 """ 626 Validator that checks that parameter is a valid ROS topic name 627 """ 628 def validator(param_value, caller_id): 629 v = valid_name_validator_resolved(param_name, param_value, caller_id) 630 if param_value == '/': 631 raise ParameterInvalid("ERROR: parameter [%s] cannot be the global namespace"%param_name) 632 return v
633 return validator 634 635 _xmlrpc_cache = {} 636 _xmlrpc_lock = threading.Lock()
637 638 -def xmlrpcapi(uri, cache=True):
639 """ 640 @return: instance for calling remote server or None if not a valid URI 641 @rtype: xmlrpclib.ServerProxy 642 """ 643 if uri is None: 644 return None 645 uriValidate = urlparse.urlparse(uri) 646 if not uriValidate[0] or not uriValidate[1]: 647 return None 648 if not cache: 649 return xmlrpcclient.ServerProxy(uri) 650 if uri not in _xmlrpc_cache: 651 with _xmlrpc_lock: 652 if uri not in _xmlrpc_cache: # allows lazy locking 653 _xmlrpc_cache[uri] = _LockedServerProxy(uri) 654 return _xmlrpc_cache[uri]
655
656 657 -class _LockedServerProxy(xmlrpcclient.ServerProxy):
658
659 - def __init__(self, *args, **kwargs):
660 xmlrpcclient.ServerProxy.__init__(self, *args, **kwargs) 661 self._lock = threading.Lock()
662
663 - def _ServerProxy__request(self, methodname, params):
664 with self._lock: 665 return xmlrpcclient.ServerProxy._ServerProxy__request( 666 self, methodname, params)
667