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 elif last_logging_time > now: 180 logging_func(msg) 181 self.last_logging_time_table = {} 182 self.last_logging_time_table[caller_id] = now

183 184 185 _logging_throttle = LoggingThrottle()

186 187 188 -def _frame_to_caller_id(frame):

189 caller_id = ( 190 inspect.getabsfile(frame), 191 frame.f_lineno, 192 frame.f_lasti, 193 ) 194 return pickle.dumps(caller_id)

195

196 197 -def logdebug_throttle(period, msg):

198 caller_id = _frame_to_caller_id(inspect.currentframe().f_back) 199 _logging_throttle(caller_id, logdebug, period, msg)

200

201 202 -def loginfo_throttle(period, msg):

203 caller_id = _frame_to_caller_id(inspect.currentframe().f_back) 204 _logging_throttle(caller_id, loginfo, period, msg)

205

206 207 -def logwarn_throttle(period, msg):

208 caller_id = _frame_to_caller_id(inspect.currentframe().f_back) 209 _logging_throttle(caller_id, logwarn, period, msg)

210

211 212 -def logerr_throttle(period, msg):

213 caller_id = _frame_to_caller_id(inspect.currentframe().f_back) 214 _logging_throttle(caller_id, logerr, period, msg)

215

216 217 -def logfatal_throttle(period, msg):

218 caller_id = _frame_to_caller_id(inspect.currentframe().f_back) 219 _logging_throttle(caller_id, logfatal, period, msg)

220 221 222 ######################################################### 223 # CONSTANTS 224 225 MASTER_NAME = "master" #master is a reserved node name for the central master 226 227 import warnings 228 import functools

229 -def deprecated(func):

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

238 return newFunc 239

240 @deprecated 241 -def get_ros_root(required=False, env=None):

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

257 258 259 ######################################################### 260 # API 261 262 _uri = None

263 -def get_node_uri():

264 """ 265 Get this Node's URI. 266 @return: this Node's XMLRPC URI 267 @rtype: str 268 """ 269 return _uri

270

271 -def set_node_uri(uri):

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

276 277 ######################################################### 278 # Logging 279 280 _log_filename = None

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

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

304

305 -class NullHandler(logging.Handler):

306 - def emit(self, record):

307 pass

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

317 318 319 -def is_initialized():

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

327 -def set_initialized(initialized):

328 """ 329 set the initialization state of the local node 330 @param initialized: True if node initialized 331 @type initialized: bool 332 """ 333 global _client_ready 334 _client_ready = initialized

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

355 356 -def is_shutdown():

357 """ 358 @return: True if shutdown flag has been set 359 @rtype: bool 360 """ 361 return _shutdown_flag

362

363 -def is_shutdown_requested():

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

375

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

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

394

395 -def _add_shutdown_thread(t):

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

413

414 -def add_client_shutdown_hook(h):

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

424

425 -def add_preshutdown_hook(h):

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

435

436 -def add_shutdown_hook(h):

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

449

450 -def signal_shutdown(reason):

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

502

503 -def _ros_signal(sig, stackframe):

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

511

512 -def _ros_atexit():

513 signal_shutdown('atexit')

514 atexit.register(_ros_atexit)

515 516 # #687 517 -def register_signals():

518 """ 519 register system signal handlers for SIGTERM and SIGINT 520 """ 521 _signalChain[signal.SIGTERM] = signal.signal(signal.SIGTERM, _ros_signal) 522 _signalChain[signal.SIGINT] = signal.signal(signal.SIGINT, _ros_signal)

523

524 # Validators ###################################### 525 526 -def is_topic(param_name):

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

535 return validator 536

537 -def xmlrpcapi(uri):

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

548