rospy.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 import os 47 import signal 48 import sys 49 import threading 50 import time 51 import traceback 52 import types 53 54 try: 55 import urllib.parse as urlparse #Python 3.x 56 except ImportError: 57 import urlparse 58 59 try: 60 import xmlrpc.client as xmlrpcclient #Python 3.x 61 except ImportError: 62 import xmlrpclib as xmlrpcclient #Python 2.x 63 64 import rospkg 65 66 import rosgraph.roslogging 67 68 import rospy.exceptions 69 import rospy.rostime 70 71 from rospy.names import * 72 from rospy.impl.validators import ParameterInvalid 73 74 from rosgraph_msgs.msg import Log 75 76 _logger = logging.getLogger("rospy.core") 77 78 # number of seconds to wait to join on threads. network issue can 79 # cause joins to be not terminate gracefully, and it's better to 80 # teardown dirty than to hang 81 _TIMEOUT_SHUTDOWN_JOIN = 5. 82 83 import warnings

84 -def deprecated(func):

85 """This is a decorator which can be used to mark functions 86 as deprecated. It will result in a warning being emmitted 87 when the function is used.""" 88 def newFunc(*args, **kwargs): 89 warnings.warn("Call to deprecated function %s." % func.__name__, 90 category=DeprecationWarning, stacklevel=2) 91 return func(*args, **kwargs)

92 newFunc.__name__ = func.__name__ 93 newFunc.__doc__ = func.__doc__ 94 newFunc.__dict__.update(func.__dict__) 95 return newFunc 96 97 ######################################################### 98 # ROSRPC 99 100 ROSRPC = "rosrpc://"

101 102 -def parse_rosrpc_uri(uri):

103 """ 104 utility function for parsing ROS-RPC URIs 105 @param uri: ROSRPC URI 106 @type uri: str 107 @return: address, port 108 @rtype: (str, int) 109 @raise ParameterInvalid: if uri is not a valid ROSRPC URI 110 """ 111 if uri.startswith(ROSRPC): 112 dest_addr = uri[len(ROSRPC):] 113 else: 114 raise ParameterInvalid("Invalid protocol for ROS service URL: %s"%uri) 115 try: 116 if '/' in dest_addr: 117 dest_addr = dest_addr[:dest_addr.find('/')] 118 dest_addr, dest_port = dest_addr.split(':') 119 dest_port = int(dest_port) 120 except: 121 raise ParameterInvalid("ROS service URL is invalid: %s"%uri) 122 return dest_addr, dest_port

123 124 ######################################################### 125 126 # rospy logger 127 _rospy_logger = logging.getLogger("rospy.internal")

128 129 # we keep a separate, non-rosout log file to contain stack traces and 130 # other sorts of information that scare users but are essential for 131 # debugging 132 133 -def rospydebug(msg, *args):

134 """Internal rospy client library debug logging""" 135 _rospy_logger.debug(msg, *args)

136 -def rospyinfo(msg, *args):

137 """Internal rospy client library debug logging""" 138 _rospy_logger.info(msg, *args)

139 -def rospyerr(msg, *args):

140 """Internal rospy client library error logging""" 141 _rospy_logger.error(msg, *args)

142 -def rospywarn(msg, *args):

143 """Internal rospy client library warn logging""" 144 _rospy_logger.warn(msg, *args)

145 146 logdebug = logging.getLogger('rosout').debug 147 148 logwarn = logging.getLogger('rosout').warning 149 150 loginfo = logging.getLogger('rosout').info 151 logout = loginfo # alias deprecated name 152 153 logerr = logging.getLogger('rosout').error 154 logerror = logerr # alias logerr 155 156 logfatal = logging.getLogger('rosout').critical

157 158 159 -class LoggingThrottle(object):

160 161 last_logging_time_table = {} 162

163 - def __call__(self, caller_id, logging_func, period, msg):

164 """Do logging specified message periodically. 165 166 - caller_id (str): Id to identify the caller 167 - logging_func (function): Function to do logging. 168 - period (float): Period to do logging in second unit. 169 - msg (object): Message to do logging. 170 """ 171 now = rospy.Time.now() 172 173 last_logging_time = self.last_logging_time_table.get(caller_id) 174 175 if (last_logging_time is None or 176 (now - last_logging_time) > rospy.Duration(period)): 177 logging_func(msg) 178 self.last_logging_time_table[caller_id] = now

179 180 181 _logging_throttle = LoggingThrottle()

182 183 184 -def _frame_record_to_caller_id(frame_record):

185 frame, _, lineno, _, code, _ = frame_record 186 caller_id = ( 187 inspect.getabsfile(frame), 188 lineno, 189 frame.f_lasti, 190 ) 191 return pickle.dumps(caller_id)

192

193 194 -def logdebug_throttle(period, msg):

195 caller_id = _frame_record_to_caller_id(inspect.stack()[1]) 196 _logging_throttle(caller_id, logdebug, period, msg)

197

198 199 -def loginfo_throttle(period, msg):

200 caller_id = _frame_record_to_caller_id(inspect.stack()[1]) 201 _logging_throttle(caller_id, loginfo, period, msg)

202

203 204 -def logwarn_throttle(period, msg):

205 caller_id = _frame_record_to_caller_id(inspect.stack()[1]) 206 _logging_throttle(caller_id, logwarn, period, msg)

207

208 209 -def logerr_throttle(period, msg):

210 caller_id = _frame_record_to_caller_id(inspect.stack()[1]) 211 _logging_throttle(caller_id, logerr, period, msg)

212

213 214 -def logfatal_throttle(period, msg):

215 caller_id = _frame_record_to_caller_id(inspect.stack()[1]) 216 _logging_throttle(caller_id, logfatal, period, msg)

217 218 219 ######################################################### 220 # CONSTANTS 221 222 MASTER_NAME = "master" #master is a reserved node name for the central master 223 224 import warnings 225 import functools

226 -def deprecated(func):

227 """This is a decorator which can be used to mark functions 228 as deprecated. It will result in a warning being emmitted 229 when the function is used.""" 230 @functools.wraps(func) 231 def newFunc(*args, **kwargs): 232 warnings.warn("Call to deprecated function %s." % func.__name__, 233 category=DeprecationWarning, stacklevel=2) 234 return func(*args, **kwargs)

235 return newFunc 236

237 @deprecated 238 -def get_ros_root(required=False, env=None):

239 """ 240 Get the value of ROS_ROOT. 241 @param env: override environment dictionary 242 @type env: dict 243 @param required: if True, fails with ROSException 244 @return: Value of ROS_ROOT environment 245 @rtype: str 246 @raise ROSException: if require is True and ROS_ROOT is not set 247 """ 248 if env is None: 249 env = os.environ 250 ros_root = rospkg.get_ros_root(env) 251 if required and not ros_root: 252 raise rospy.exceptions.ROSException('%s is not set'%rospkg.environment.ROS_ROOT) 253 return ros_root

254 255 256 ######################################################### 257 # API 258 259 _uri = None

260 -def get_node_uri():

261 """ 262 Get this Node's URI. 263 @return: this Node's XMLRPC URI 264 @rtype: str 265 """ 266 return _uri

267

268 -def set_node_uri(uri):

269 """set the URI of the local node. 270 This is an internal API method, it does not actually affect the XMLRPC URI of the Node.""" 271 global _uri 272 _uri = uri

273 274 ######################################################### 275 # Logging 276 277 _log_filename = None

278 -def configure_logging(node_name, level=logging.INFO):

279 """ 280 Setup filesystem logging for this node 281 @param node_name: Node's name 282 @type node_name str 283 @param level: (optional) Python logging level (INFO, DEBUG, etc...). (Default: logging.INFO) 284 @type level: int 285 """ 286 global _log_filename 287 288 # #988 __log command-line remapping argument 289 mappings = get_mappings() 290 if '__log' in get_mappings(): 291 logfilename_remap = mappings['__log'] 292 filename = os.path.abspath(logfilename_remap) 293 else: 294 # fix filesystem-unsafe chars 295 filename = node_name.replace('/', '_') + '.log' 296 if filename[0] == '_': 297 filename = filename[1:] 298 if not filename: 299 raise rospy.exceptions.ROSException('invalid configure_logging parameter: %s'%node_name) 300 _log_filename = rosgraph.roslogging.configure_logging('rospy', level, filename=filename)

301

302 -class NullHandler(logging.Handler):

303 - def emit(self, record):

304 pass

305 306 # keep logging happy until we have the node name to configure with 307 logging.getLogger('rospy').addHandler(NullHandler()) 308 309 310 ######################################################### 311 # Init/Shutdown/Exit API and Handlers 312 313 _client_ready = False

314 315 316 -def is_initialized():

317 """ 318 Get the initialization state of the local node. If True, node has 319 been configured. 320 @return: True if local node initialized 321 @rtype: bool 322 """ 323 return _client_ready

324 -def set_initialized(initialized):

325 """ 326 set the initialization state of the local node 327 @param initialized: True if node initialized 328 @type initialized: bool 329 """ 330 global _client_ready 331 _client_ready = initialized

332 333 _shutdown_lock = threading.RLock() 334 335 # _shutdown_flag flags that rospy is in shutdown mode, in_shutdown 336 # flags that the shutdown routine has started. These are separate 337 # because 'pre-shutdown' hooks require rospy to be in a non-shutdown 338 # mode. These hooks are executed during the shutdown routine. 339 _shutdown_flag = False 340 _in_shutdown = False 341 342 # various hooks to call on shutdown. shutdown hooks are called in the 343 # shutdown state, preshutdown are called just before entering shutdown 344 # state, and client shutdown is called before both of these. 345 _shutdown_hooks = [] 346 _preshutdown_hooks = [] 347 _client_shutdown_hooks = [] 348 # threads that must be joined on shutdown 349 _shutdown_threads = [] 350 351 _signalChain = {}

352 353 -def is_shutdown():

354 """ 355 @return: True if shutdown flag has been set 356 @rtype: bool 357 """ 358 return _shutdown_flag

359

360 -def is_shutdown_requested():

361 """ 362 is_shutdown_requested is a state that occurs just before 363 is_shutdown. It is initiated when a shutdown requested is 364 received and continues until client shutdown handlers have been 365 called. After client shutdown handlers have been serviced, the 366 is_shutdown state becomes true. 367 368 @return: True if shutdown has been requested (but possibly not yet initiated) 369 @rtype: bool 370 """ 371 return _in_shutdown

372

373 -def _add_shutdown_hook(h, hooks, pass_reason_argument=True):

374 """ 375 shared implementation of add_shutdown_hook and add_preshutdown_hook 376 """ 377 if not callable(h): 378 raise TypeError("shutdown hook [%s] must be a function or callable object: %s"%(h, type(h))) 379 if _shutdown_flag: 380 _logger.warn("add_shutdown_hook called after shutdown") 381 if pass_reason_argument: 382 h("already shutdown") 383 else: 384 h() 385 return 386 with _shutdown_lock: 387 if hooks is None: 388 # race condition check, don't log as we are deep into shutdown 389 return 390 hooks.append(h)

391

392 -def _add_shutdown_thread(t):

393 """ 394 Register thread that must be joined() on shutdown 395 """ 396 if _shutdown_flag: 397 #TODO 398 return 399 with _shutdown_lock: 400 if _shutdown_threads is None: 401 # race condition check, don't log as we are deep into shutdown 402 return 403 # in order to prevent memory leaks, reap dead threads. The 404 # last thread may not get reaped until shutdown, but this is 405 # relatively minor 406 for other in _shutdown_threads[:]: 407 if not other.isAlive(): 408 _shutdown_threads.remove(other) 409 _shutdown_threads.append(t)

410

411 -def add_client_shutdown_hook(h):

412 """ 413 Add client method to invoke when system shuts down. Unlike 414 L{add_shutdown_hook} and L{add_preshutdown_hooks}, these methods 415 will be called before any rospy internal shutdown code. 416 417 @param h: function with zero args 418 @type h: fn() 419 """ 420 _add_shutdown_hook(h, _client_shutdown_hooks, pass_reason_argument=False)

421

422 -def add_preshutdown_hook(h):

423 """ 424 Add method to invoke when system shuts down. Unlike 425 L{add_shutdown_hook}, these methods will be called before any 426 other shutdown hooks. 427 428 @param h: function that takes in a single string argument (shutdown reason) 429 @type h: fn(str) 430 """ 431 _add_shutdown_hook(h, _preshutdown_hooks)

432

433 -def add_shutdown_hook(h):

434 """ 435 Add method to invoke when system shuts down. 436 437 Shutdown hooks are called in the order that they are 438 registered. This is an internal API method that is used to 439 cleanup. See the client X{on_shutdown()} method if you wish to 440 register client hooks. 441 442 @param h: function that takes in a single string argument (shutdown reason) 443 @type h: fn(str) 444 """ 445 _add_shutdown_hook(h, _shutdown_hooks)

446

447 -def signal_shutdown(reason):

448 """ 449 Initiates shutdown process by signaling objects waiting on _shutdown_lock. 450 Shutdown and pre-shutdown hooks are invoked. 451 @param reason: human-readable shutdown reason, if applicable 452 @type reason: str 453 """ 454 global _shutdown_flag, _in_shutdown, _shutdown_lock, _shutdown_hooks 455 _logger.info("signal_shutdown [%s]"%reason) 456 if _shutdown_flag or _in_shutdown: 457 return 458 with _shutdown_lock: 459 if _shutdown_flag or _in_shutdown: 460 return 461 _in_shutdown = True 462 463 # make copy just in case client re-invokes shutdown 464 for h in _client_shutdown_hooks: 465 try: 466 # client shutdown hooks do not accept a reason arg 467 h() 468 except: 469 traceback.print_exc() 470 del _client_shutdown_hooks[:] 471 472 for h in _preshutdown_hooks: 473 try: 474 h(reason) 475 except: 476 traceback.print_exc() 477 del _preshutdown_hooks[:] 478 479 # now that pre-shutdown hooks have been called, raise shutdown 480 # flag. This allows preshutdown hooks to still publish and use 481 # service calls properly 482 _shutdown_flag = True 483 for h in _shutdown_hooks: 484 try: 485 h(reason) 486 except Exception as e: 487 sys.stderr.write("signal_shutdown hook error[%s]\n"%e) 488 del _shutdown_hooks[:] 489 490 threads = _shutdown_threads[:] 491 492 for t in threads: 493 if t.isAlive(): 494 t.join(_TIMEOUT_SHUTDOWN_JOIN) 495 del _shutdown_threads[:] 496 try: 497 rospy.rostime.wallsleep(0.1) #hack for now until we get rid of all the extra threads 498 except KeyboardInterrupt: pass

499

500 -def _ros_signal(sig, stackframe):

501 signal_shutdown("signal-"+str(sig)) 502 prev_handler = _signalChain.get(sig, None) 503 if prev_handler is not None and not type(prev_handler) == int: 504 try: 505 prev_handler(sig, stackframe) 506 except KeyboardInterrupt: 507 pass #filter out generic keyboard interrupt handler

508

509 -def _ros_atexit():

510 signal_shutdown('atexit')

511 atexit.register(_ros_atexit)

512 513 # #687 514 -def register_signals():

515 """ 516 register system signal handlers for SIGTERM and SIGINT 517 """ 518 _signalChain[signal.SIGTERM] = signal.signal(signal.SIGTERM, _ros_signal) 519 _signalChain[signal.SIGINT] = signal.signal(signal.SIGINT, _ros_signal)

520

521 # Validators ###################################### 522 523 -def is_topic(param_name):

524 """ 525 Validator that checks that parameter is a valid ROS topic name 526 """ 527 def validator(param_value, caller_id): 528 v = valid_name_validator_resolved(param_name, param_value, caller_id) 529 if param_value == '/': 530 raise ParameterInvalid("ERROR: parameter [%s] cannot be the global namespace"%param_name) 531 return v

532 return validator 533

534 -def xmlrpcapi(uri):

535 """ 536 @return: instance for calling remote server or None if not a valid URI 537 @rtype: xmlrpclib.ServerProxy 538 """ 539 if uri is None: 540 return None 541 uriValidate = urlparse.urlparse(uri) 542 if not uriValidate[0] or not uriValidate[1]: 543 return None 544 return xmlrpcclient.ServerProxy(uri)

545