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 from functools import partial 76 77 _logger = logging.getLogger("rospy.core") 78 79 # number of seconds to wait to join on threads. network issue can 80 # cause joins to be not terminate gracefully, and it's better to 81 # teardown dirty than to hang 82 _TIMEOUT_SHUTDOWN_JOIN = 5. 83 84 import warnings

85 -def deprecated(func):

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

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

102 103 -def parse_rosrpc_uri(uri):

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

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

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

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

137 -def rospyinfo(msg, *args):

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

140 -def rospyerr(msg, *args):

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

143 -def rospywarn(msg, *args):

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

146

147 148 -def _base_logger(msg, *args, **kwargs):

149 150 name = kwargs.pop('logger_name', None) 151 throttle = kwargs.pop('logger_throttle', None) 152 level = kwargs.pop('logger_level', None) 153 once = kwargs.pop('logger_once', False) 154 155 rospy_logger = logging.getLogger('rosout') 156 if name: 157 rospy_logger = rospy_logger.getChild(name) 158 logfunc = getattr(rospy_logger, level) 159 160 if once: 161 caller_id = _frame_to_caller_id(inspect.currentframe().f_back.f_back) 162 if _logging_once(caller_id): 163 logfunc(msg, *args) 164 elif throttle: 165 caller_id = _frame_to_caller_id(inspect.currentframe().f_back.f_back) 166 if _logging_throttle(caller_id, throttle): 167 logfunc(msg, *args) 168 else: 169 logfunc(msg, *args)

170

171 172 -def logdebug(msg, *args, **kwargs):

173 _base_logger(msg, *args, logger_level='debug', **kwargs)

174

175 -def loginfo(msg, *args, **kwargs):

176 _base_logger(msg, *args, logger_level='info', **kwargs)

177

178 -def logwarn(msg, *args, **kwargs):

179 _base_logger(msg, *args, logger_level='warn', **kwargs)

180

181 -def logerr(msg, *args, **kwargs):

182 _base_logger(msg, *args, logger_level='error', **kwargs)

183

184 -def logfatal(msg, *args, **kwargs):

185 _base_logger(msg, *args, logger_level='critical', **kwargs)

186 187 logout = loginfo # alias deprecated name 188 189 logerror = logerr # alias logerr

190 191 192 -class LoggingThrottle(object):

193 194 last_logging_time_table = {} 195

196 - def __call__(self, caller_id, period):

197 """Do logging specified message periodically. 198 199 - caller_id (str): Id to identify the caller 200 - logging_func (function): Function to do logging. 201 - period (float): Period to do logging in second unit. 202 - msg (object): Message to do logging. 203 """ 204 now = rospy.Time.now() 205 206 last_logging_time = self.last_logging_time_table.get(caller_id) 207 208 if (last_logging_time is None or 209 (now - last_logging_time) > rospy.Duration(period)): 210 self.last_logging_time_table[caller_id] = now 211 return True 212 return False

213 214 215 _logging_throttle = LoggingThrottle()

216 217 218 -def _frame_to_caller_id(frame):

219 caller_id = ( 220 inspect.getabsfile(frame), 221 frame.f_lineno, 222 frame.f_lasti, 223 ) 224 return pickle.dumps(caller_id)

225

226 227 -def logdebug_throttle(period, msg):

228 _base_logger(msg, logger_throttle=period, logger_level='debug')

229

230 -def loginfo_throttle(period, msg):

231 _base_logger(msg, logger_throttle=period, logger_level='info')

232

233 -def logwarn_throttle(period, msg):

234 _base_logger(msg, logger_throttle=period, logger_level='warn')

235

236 -def logerr_throttle(period, msg):

237 _base_logger(msg, logger_throttle=period, logger_level='error')

238

239 -def logfatal_throttle(period, msg):

240 _base_logger(msg, logger_throttle=period, logger_level='critical')

241

242 -class LoggingOnce(object):

243 244 called_caller_ids = set() 245

246 - def __call__(self, caller_id):

247 if caller_id not in self.called_caller_ids: 248 self.called_caller_ids.add(caller_id) 249 return True 250 return False

251 252 _logging_once = LoggingOnce()

253 254 255 -def logdebug_once(msg):

256 _base_logger(msg, logger_once=True, logger_level='debug')

257

258 -def loginfo_once(msg):

259 _base_logger(msg, logger_once=True, logger_level='info')

260

261 -def logwarn_once(msg):

262 _base_logger(msg, logger_once=True, logger_level='warn')

263

264 -def logerr_once(msg):

265 _base_logger(msg, logger_once=True, logger_level='error')

266

267 -def logfatal_once(msg):

268 _base_logger(msg, logger_once=True, logger_level='critical')

269 270 271 ######################################################### 272 # CONSTANTS 273 274 MASTER_NAME = "master" #master is a reserved node name for the central master 275 276 import warnings 277 import functools

278 -def deprecated(func):

279 """This is a decorator which can be used to mark functions 280 as deprecated. It will result in a warning being emmitted 281 when the function is used.""" 282 @functools.wraps(func) 283 def newFunc(*args, **kwargs): 284 warnings.warn("Call to deprecated function %s." % func.__name__, 285 category=DeprecationWarning, stacklevel=2) 286 return func(*args, **kwargs)

287 return newFunc 288

289 @deprecated 290 -def get_ros_root(required=False, env=None):

291 """ 292 Get the value of ROS_ROOT. 293 @param env: override environment dictionary 294 @type env: dict 295 @param required: if True, fails with ROSException 296 @return: Value of ROS_ROOT environment 297 @rtype: str 298 @raise ROSException: if require is True and ROS_ROOT is not set 299 """ 300 if env is None: 301 env = os.environ 302 ros_root = rospkg.get_ros_root(env) 303 if required and not ros_root: 304 raise rospy.exceptions.ROSException('%s is not set'%rospkg.environment.ROS_ROOT) 305 return ros_root

306 307 308 ######################################################### 309 # API 310 311 _uri = None

312 -def get_node_uri():

313 """ 314 Get this Node's URI. 315 @return: this Node's XMLRPC URI 316 @rtype: str 317 """ 318 return _uri

319

320 -def set_node_uri(uri):

321 """set the URI of the local node. 322 This is an internal API method, it does not actually affect the XMLRPC URI of the Node.""" 323 global _uri 324 _uri = uri

325 326 ######################################################### 327 # Logging 328 329 _log_filename = None

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

331 """ 332 Setup filesystem logging for this node 333 @param node_name: Node's name 334 @type node_name str 335 @param level: (optional) Python logging level (INFO, DEBUG, etc...). (Default: logging.INFO) 336 @type level: int 337 """ 338 global _log_filename 339 340 # #988 __log command-line remapping argument 341 mappings = get_mappings() 342 if '__log' in get_mappings(): 343 logfilename_remap = mappings['__log'] 344 filename = os.path.abspath(logfilename_remap) 345 else: 346 # fix filesystem-unsafe chars 347 suffix = '.log' 348 filename = node_name.replace('/', '_') + suffix 349 if filename[0] == '_': 350 filename = filename[1:] 351 if filename == suffix: 352 raise rospy.exceptions.ROSException('invalid configure_logging parameter: %s'%node_name) 353 _log_filename = rosgraph.roslogging.configure_logging('rospy', level, filename=filename)

354

355 -class NullHandler(logging.Handler):

356 - def emit(self, record):

357 pass

358 359 # keep logging happy until we have the node name to configure with 360 logging.getLogger('rospy').addHandler(NullHandler()) 361 362 363 ######################################################### 364 # Init/Shutdown/Exit API and Handlers 365 366 _client_ready = False

367 368 369 -def is_initialized():

370 """ 371 Get the initialization state of the local node. If True, node has 372 been configured. 373 @return: True if local node initialized 374 @rtype: bool 375 """ 376 return _client_ready

377 -def set_initialized(initialized):

378 """ 379 set the initialization state of the local node 380 @param initialized: True if node initialized 381 @type initialized: bool 382 """ 383 global _client_ready 384 _client_ready = initialized

385 386 _shutdown_lock = threading.RLock() 387 388 # _shutdown_flag flags that rospy is in shutdown mode, in_shutdown 389 # flags that the shutdown routine has started. These are separate 390 # because 'pre-shutdown' hooks require rospy to be in a non-shutdown 391 # mode. These hooks are executed during the shutdown routine. 392 _shutdown_flag = False 393 _in_shutdown = False 394 395 # various hooks to call on shutdown. shutdown hooks are called in the 396 # shutdown state, preshutdown are called just before entering shutdown 397 # state, and client shutdown is called before both of these. 398 _shutdown_hooks = [] 399 _preshutdown_hooks = [] 400 _client_shutdown_hooks = [] 401 # threads that must be joined on shutdown 402 _shutdown_threads = [] 403 404 _signalChain = {}

405 406 -def is_shutdown():

407 """ 408 @return: True if shutdown flag has been set 409 @rtype: bool 410 """ 411 return _shutdown_flag

412

413 -def is_shutdown_requested():

414 """ 415 is_shutdown_requested is a state that occurs just before 416 is_shutdown. It is initiated when a shutdown requested is 417 received and continues until client shutdown handlers have been 418 called. After client shutdown handlers have been serviced, the 419 is_shutdown state becomes true. 420 421 @return: True if shutdown has been requested (but possibly not yet initiated) 422 @rtype: bool 423 """ 424 return _in_shutdown

425

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

427 """ 428 shared implementation of add_shutdown_hook and add_preshutdown_hook 429 """ 430 if not callable(h): 431 raise TypeError("shutdown hook [%s] must be a function or callable object: %s"%(h, type(h))) 432 if _shutdown_flag: 433 _logger.warn("add_shutdown_hook called after shutdown") 434 if pass_reason_argument: 435 h("already shutdown") 436 else: 437 h() 438 return 439 with _shutdown_lock: 440 if hooks is None: 441 # race condition check, don't log as we are deep into shutdown 442 return 443 hooks.append(h)

444

445 -def _add_shutdown_thread(t):

446 """ 447 Register thread that must be joined() on shutdown 448 """ 449 if _shutdown_flag: 450 #TODO 451 return 452 with _shutdown_lock: 453 if _shutdown_threads is None: 454 # race condition check, don't log as we are deep into shutdown 455 return 456 # in order to prevent memory leaks, reap dead threads. The 457 # last thread may not get reaped until shutdown, but this is 458 # relatively minor 459 for other in _shutdown_threads[:]: 460 if not other.isAlive(): 461 _shutdown_threads.remove(other) 462 _shutdown_threads.append(t)

463

464 -def add_client_shutdown_hook(h):

465 """ 466 Add client method to invoke when system shuts down. Unlike 467 L{add_shutdown_hook} and L{add_preshutdown_hooks}, these methods 468 will be called before any rospy internal shutdown code. 469 470 @param h: function with zero args 471 @type h: fn() 472 """ 473 _add_shutdown_hook(h, _client_shutdown_hooks, pass_reason_argument=False)

474

475 -def add_preshutdown_hook(h):

476 """ 477 Add method to invoke when system shuts down. Unlike 478 L{add_shutdown_hook}, these methods will be called before any 479 other shutdown hooks. 480 481 @param h: function that takes in a single string argument (shutdown reason) 482 @type h: fn(str) 483 """ 484 _add_shutdown_hook(h, _preshutdown_hooks)

485

486 -def add_shutdown_hook(h):

487 """ 488 Add method to invoke when system shuts down. 489 490 Shutdown hooks are called in the order that they are 491 registered. This is an internal API method that is used to 492 cleanup. See the client X{on_shutdown()} method if you wish to 493 register client hooks. 494 495 @param h: function that takes in a single string argument (shutdown reason) 496 @type h: fn(str) 497 """ 498 _add_shutdown_hook(h, _shutdown_hooks)

499

500 -def signal_shutdown(reason):

501 """ 502 Initiates shutdown process by signaling objects waiting on _shutdown_lock. 503 Shutdown and pre-shutdown hooks are invoked. 504 @param reason: human-readable shutdown reason, if applicable 505 @type reason: str 506 """ 507 global _shutdown_flag, _in_shutdown, _shutdown_lock, _shutdown_hooks 508 _logger.info("signal_shutdown [%s]"%reason) 509 if _shutdown_flag or _in_shutdown: 510 return 511 with _shutdown_lock: 512 if _shutdown_flag or _in_shutdown: 513 return 514 _in_shutdown = True 515 516 # make copy just in case client re-invokes shutdown 517 for h in _client_shutdown_hooks: 518 try: 519 # client shutdown hooks do not accept a reason arg 520 h() 521 except: 522 traceback.print_exc() 523 del _client_shutdown_hooks[:] 524 525 for h in _preshutdown_hooks: 526 try: 527 h(reason) 528 except: 529 traceback.print_exc() 530 del _preshutdown_hooks[:] 531 532 # now that pre-shutdown hooks have been called, raise shutdown 533 # flag. This allows preshutdown hooks to still publish and use 534 # service calls properly 535 _shutdown_flag = True 536 for h in _shutdown_hooks: 537 try: 538 h(reason) 539 except Exception as e: 540 sys.stderr.write("signal_shutdown hook error[%s]\n"%e) 541 del _shutdown_hooks[:] 542 543 threads = _shutdown_threads[:] 544 545 for t in threads: 546 if t.isAlive(): 547 t.join(_TIMEOUT_SHUTDOWN_JOIN) 548 del _shutdown_threads[:] 549 try: 550 rospy.rostime.wallsleep(0.1) #hack for now until we get rid of all the extra threads 551 except KeyboardInterrupt: pass

552

553 -def _ros_signal(sig, stackframe):

554 signal_shutdown("signal-"+str(sig)) 555 prev_handler = _signalChain.get(sig, None) 556 if prev_handler is not None and not type(prev_handler) == int: 557 try: 558 prev_handler(sig, stackframe) 559 except KeyboardInterrupt: 560 pass #filter out generic keyboard interrupt handler

561

562 -def _ros_atexit():

563 signal_shutdown('atexit')

564 atexit.register(_ros_atexit)

565 566 # #687 567 -def register_signals():

568 """ 569 register system signal handlers for SIGTERM and SIGINT 570 """ 571 _signalChain[signal.SIGTERM] = signal.signal(signal.SIGTERM, _ros_signal) 572 _signalChain[signal.SIGINT] = signal.signal(signal.SIGINT, _ros_signal)

573

574 # Validators ###################################### 575 576 -def is_topic(param_name):

577 """ 578 Validator that checks that parameter is a valid ROS topic name 579 """ 580 def validator(param_value, caller_id): 581 v = valid_name_validator_resolved(param_name, param_value, caller_id) 582 if param_value == '/': 583 raise ParameterInvalid("ERROR: parameter [%s] cannot be the global namespace"%param_name) 584 return v

585 return validator 586

587 -def xmlrpcapi(uri):

588 """ 589 @return: instance for calling remote server or None if not a valid URI 590 @rtype: xmlrpclib.ServerProxy 591 """ 592 if uri is None: 593 return None 594 uriValidate = urlparse.urlparse(uri) 595 if not uriValidate[0] or not uriValidate[1]: 596 return None 597 return xmlrpcclient.ServerProxy(uri)

598