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: core.py 13839 2011-06-01 01:22:34Z kwc $ 34 35 """rospy internal core implementation library""" 36 37 from __future__ import with_statement 38 39 import atexit 40 import logging 41 import os 42 import signal 43 import sys 44 import threading 45 import time 46 import traceback 47 import types 48 import urlparse 49 import xmlrpclib 50 51 import roslib.rosenv 52 import roslib.roslogging 53 54 import rospy.exceptions 55 56 from rospy.names import * 57 from rospy.impl.validators import ParameterInvalid 58 59 from rosgraph_msgs.msg import Log 60 61 _logger = logging.getLogger("rospy.core") 62 63 # number of seconds to wait to join on threads. network issue can 64 # cause joins to be not terminate gracefully, and it's better to 65 # teardown dirty than to hang 66 _TIMEOUT_SHUTDOWN_JOIN = 5. 67 68 import warnings

69 -def deprecated(func):

70 """This is a decorator which can be used to mark functions 71 as deprecated. It will result in a warning being emmitted 72 when the function is used.""" 73 def newFunc(*args, **kwargs): 74 warnings.warn("Call to deprecated function %s." % func.__name__, 75 category=DeprecationWarning, stacklevel=2) 76 return func(*args, **kwargs)

77 newFunc.__name__ = func.__name__ 78 newFunc.__doc__ = func.__doc__ 79 newFunc.__dict__.update(func.__dict__) 80 return newFunc 81 82 ######################################################### 83 # ROSRPC 84 85 ROSRPC = "rosrpc://" 86

87 -def parse_rosrpc_uri(uri):

88 """ 89 utility function for parsing ROS-RPC URIs 90 @param uri: ROSRPC URI 91 @type uri: str 92 @return: address, port 93 @rtype: (str, int) 94 @raise ParameterInvalid: if uri is not a valid ROSRPC URI 95 """ 96 if uri.startswith(ROSRPC): 97 dest_addr = uri[len(ROSRPC):] 98 else: 99 raise ParameterInvalid("Invalid protocol for ROS service URL: %s"%uri) 100 try: 101 if '/' in dest_addr: 102 dest_addr = dest_addr[:dest_addr.find('/')] 103 dest_addr, dest_port = dest_addr.split(':') 104 dest_port = int(dest_port) 105 except: 106 raise ParameterInvalid("ROS service URL is invalid: %s"%uri) 107 return dest_addr, dest_port

108 109 ######################################################### 110 111 # rospy logger 112 _rospy_logger = logging.getLogger("rospy.internal") 113 114 _logdebug_handlers = [] 115 _loginfo_handlers = [] 116 _logwarn_handlers = [] 117 _logerr_handlers = [] 118 _logfatal_handlers = [] 119 120 # we keep a separate, non-rosout log file to contain stack traces and 121 # other sorts of information that scare users but are essential for 122 # debugging 123

124 -def rospydebug(msg, *args):

125 """Internal rospy client library debug logging""" 126 _rospy_logger.debug(msg, *args)

127 -def rospyerr(msg, *args):

128 """Internal rospy client library error logging""" 129 _rospy_logger.error(msg, *args)

130 -def rospywarn(msg, *args):

131 """Internal rospy client library warn logging""" 132 _rospy_logger.warn(msg, *args)

133

134 -def add_log_handler(level, h):

135 """ 136 Add handler for specified level 137 @param level: log level (use constants from Log) 138 @type level: int 139 @param h: log message handler 140 @type h: fn 141 @raise ROSInternalException: if level is invalid 142 """ 143 if level == Log.DEBUG: 144 _logdebug_handlers.append(h) 145 elif level == Log.INFO: 146 _loginfo_handlers.append(h) 147 elif level == Log.WARN: 148 _logwarn_handlers.append(h) 149 elif level == Log.ERROR: 150 _logerr_handlers.append(h) 151 elif level == Log.FATAL: 152 _logfatal_handlers.append(h) 153 else: 154 raise rospy.exceptions.ROSInternalException("invalid log level: %s"%level)

155

156 -def logdebug(msg, *args):

157 """ 158 Log a debug message to the /rosout topic 159 @param msg: message to log, may include formatting arguments 160 @type msg: str 161 @param args: format-string arguments, if necessary 162 """ 163 if args: 164 msg = msg%args 165 for h in _logdebug_handlers: 166 h(msg)

167

168 -def logwarn(msg, *args):

169 """ 170 Log a warning message to the /rosout topic 171 @param msg: message to log, may include formatting arguments 172 @type msg: str 173 @param args: format-string arguments, if necessary 174 """ 175 if args: 176 msg = msg%args 177 for h in _logwarn_handlers: 178 h(msg)

179

180 -def loginfo(msg, *args):

181 """ 182 Log an info message to the /rosout topic 183 @param msg: message to log, may include formatting arguments 184 @type msg: str 185 @param args: format-string arguments, if necessary 186 """ 187 if args: 188 msg = msg%args 189 for h in _loginfo_handlers: 190 h(msg)

191 logout = loginfo # alias deprecated name 192

193 -def logerr(msg, *args):

194 """ 195 Log an error message to the /rosout topic 196 @param msg: message to log, may include formatting arguments 197 @type msg: str 198 @param args: format-string arguments, if necessary 199 """ 200 if args: 201 msg = msg%args 202 for h in _logerr_handlers: 203 h(msg)

204 logerror = logerr # alias logerr 205

206 -def logfatal(msg, *args):

207 """ 208 Log an error message to the /rosout topic 209 @param msg: message to log, may include formatting arguments 210 @type msg: str 211 @param args: format-string arguments, if necessary 212 """ 213 if args: 214 msg = msg%args 215 for h in _logfatal_handlers: 216 h(msg)

217 218 ######################################################### 219 # CONSTANTS 220 221 MASTER_NAME = "master" #master is a reserved node name for the central master 222

223 -def get_ros_root(required=False, env=None):

224 """ 225 Get the value of ROS_ROOT. 226 @param env: override environment dictionary 227 @type env: dict 228 @param required: if True, fails with ROSException 229 @return: Value of ROS_ROOT environment 230 @rtype: str 231 @raise ROSException: if require is True and ROS_ROOT is not set 232 """ 233 if env is None: 234 env = os.environ 235 ros_root = env.get(roslib.rosenv.ROS_ROOT, None) 236 if required and not ros_root: 237 raise rospy.exceptions.ROSException('%s is not set'%roslib.rosenv.ROS_ROOT) 238 return ros_root

239 240 241 ######################################################### 242 # API 243 244 _uri = None

245 -def get_node_uri():

246 """ 247 Get this Node's URI. 248 @return: this Node's XMLRPC URI 249 @rtype: str 250 """ 251 return _uri

252

253 -def set_node_uri(uri):

254 """set the URI of the local node. 255 This is an internal API method, it does not actually affect the XMLRPC URI of the Node.""" 256 global _uri 257 _uri = uri

258 259 ######################################################### 260 # Logging 261 262 _log_filename = None

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

264 """ 265 Setup filesystem logging for this node 266 @param node_name: Node's name 267 @type node_name str 268 @param level: (optional) Python logging level (INFO, DEBUG, etc...). (Default: logging.INFO) 269 @type level: int 270 """ 271 global _log_filename 272 273 # #988 __log command-line remapping argument 274 mappings = get_mappings() 275 if '__log' in get_mappings(): 276 logfilename_remap = mappings['__log'] 277 filename = os.path.abspath(logfilename_remap) 278 else: 279 # fix filesystem-unsafe chars 280 filename = node_name.replace('/', '_') + '.log' 281 if filename[0] == '_': 282 filename = filename[1:] 283 if not filename: 284 raise rospy.exceptions.ROSException('invalid configure_logging parameter: %s'%node_name) 285 _log_filename = roslib.roslogging.configure_logging('rospy', level, filename=filename)

286

287 -class NullHandler(logging.Handler):

288 - def emit(self, record):

289 pass

290 291 # keep logging happy until we have the node name to configure with 292 logging.getLogger('rospy').addHandler(NullHandler()) 293 294 295 ######################################################### 296 # Init/Shutdown/Exit API and Handlers 297 298 _client_ready = False 299 300

301 -def is_initialized():

302 """ 303 Get the initialization state of the local node. If True, node has 304 been configured. 305 @return: True if local node initialized 306 @rtype: bool 307 """ 308 return _client_ready

309 -def set_initialized(initialized):

310 """ 311 set the initialization state of the local node 312 @param initialized: True if node initialized 313 @type initialized: bool 314 """ 315 global _client_ready 316 _client_ready = initialized

317 318 _shutdown_lock = threading.RLock() 319 320 # _shutdown_flag flags that rospy is in shutdown mode, in_shutdown 321 # flags that the shutdown routine has started. These are separate 322 # because 'pre-shutdown' hooks require rospy to be in a non-shutdown 323 # mode. These hooks are executed during the shutdown routine. 324 _shutdown_flag = False 325 _in_shutdown = False 326 327 # various hooks to call on shutdown. shutdown hooks are called in the 328 # shutdown state, preshutdown are called just before entering shutdown 329 # state, and client shutdown is called before both of these. 330 _shutdown_hooks = [] 331 _preshutdown_hooks = [] 332 _client_shutdown_hooks = [] 333 # threads that must be joined on shutdown 334 _shutdown_threads = [] 335 336 _signalChain = {} 337

338 -def is_shutdown():

339 """ 340 @return: True if shutdown flag has been set 341 @rtype: bool 342 """ 343 return _shutdown_flag

344

345 -def is_shutdown_requested():

346 """ 347 is_shutdown_requested is a state that occurs just before 348 is_shutdown. It is initiated when a shutdown requested is 349 received and continues until client shutdown handlers have been 350 called. After client shutdown handlers have been serviced, the 351 is_shutdown state becomes true. 352 353 @return: True if shutdown has been requested (but possibly not yet initiated) 354 @rtype: bool 355 """ 356 return _in_shutdown

357

358 -def _add_shutdown_hook(h, hooks):

359 """ 360 shared implementation of add_shutdown_hook and add_preshutdown_hook 361 """ 362 if type(h) not in [types.FunctionType, types.MethodType]: 363 raise TypeError("shutdown hook [%s] must be a function: %s"%(h, type(h))) 364 if _shutdown_flag: 365 _logger.warn("add_shutdown_hook called after shutdown") 366 h("already shutdown") 367 return 368 with _shutdown_lock: 369 if hooks is None: 370 # race condition check, don't log as we are deep into shutdown 371 return 372 hooks.append(h)

373

374 -def _add_shutdown_thread(t):

375 """ 376 Register thread that must be joined() on shutdown 377 """ 378 if _shutdown_flag: 379 #TODO 380 return 381 with _shutdown_lock: 382 if _shutdown_threads is None: 383 # race condition check, don't log as we are deep into shutdown 384 return 385 # in order to prevent memory leaks, reap dead threads. The 386 # last thread may not get reaped until shutdown, but this is 387 # relatively minor 388 for other in _shutdown_threads[:]: 389 if not other.isAlive(): 390 _shutdown_threads.remove(other) 391 _shutdown_threads.append(t)

392

393 -def add_client_shutdown_hook(h):

394 """ 395 Add client method to invoke when system shuts down. Unlike 396 L{add_shutdown_hook} and L{add_preshutdown_hooks}, these methods 397 will be called before any rospy internal shutdown code. 398 399 @param h: function that takes in a single string argument (shutdown reason) 400 @type h: fn(str) 401 """ 402 _add_shutdown_hook(h, _client_shutdown_hooks)

403

404 -def add_preshutdown_hook(h):

405 """ 406 Add method to invoke when system shuts down. Unlike 407 L{add_shutdown_hook}, these methods will be called before any 408 other shutdown hooks. 409 410 @param h: function that takes in a single string argument (shutdown reason) 411 @type h: fn(str) 412 """ 413 _add_shutdown_hook(h, _preshutdown_hooks)

414

415 -def add_shutdown_hook(h):

416 """ 417 Add method to invoke when system shuts down. 418 419 Shutdown hooks are called in the order that they are 420 registered. This is an internal API method that is used to 421 cleanup. See the client X{on_shutdown()} method if you wish to 422 register client hooks. 423 424 @param h: function that takes in a single string argument (shutdown reason) 425 @type h: fn(str) 426 """ 427 _add_shutdown_hook(h, _shutdown_hooks)

428

429 -def signal_shutdown(reason):

430 """ 431 Initiates shutdown process by signaling objects waiting on _shutdown_lock. 432 Shutdown and pre-shutdown hooks are invoked. 433 @param reason: human-readable shutdown reason, if applicable 434 @type reason: str 435 """ 436 global _shutdown_flag, _in_shutdown, _shutdown_lock, _shutdown_hooks 437 _logger.info("signal_shutdown [%s]"%reason) 438 if _shutdown_flag or _in_shutdown: 439 return 440 with _shutdown_lock: 441 if _shutdown_flag or _in_shutdown: 442 return 443 _in_shutdown = True 444 445 # make copy just in case client re-invokes shutdown 446 for h in _client_shutdown_hooks: 447 try: 448 # client shutdown hooks do not accept a reason arg 449 h() 450 except: 451 traceback.print_exc() 452 del _client_shutdown_hooks[:] 453 454 for h in _preshutdown_hooks: 455 try: 456 h(reason) 457 except: 458 traceback.print_exc() 459 del _preshutdown_hooks[:] 460 461 # now that pre-shutdown hooks have been called, raise shutdown 462 # flag. This allows preshutdown hooks to still publish and use 463 # service calls properly 464 _shutdown_flag = True 465 for h in _shutdown_hooks: 466 try: 467 h(reason) 468 except Exception, e: 469 print >> sys.stderr, "signal_shutdown hook error[%s]"%e 470 del _shutdown_hooks[:] 471 472 threads = _shutdown_threads[:] 473 474 for t in threads: 475 if t.isAlive(): 476 t.join(_TIMEOUT_SHUTDOWN_JOIN) 477 del _shutdown_threads[:] 478 try: 479 time.sleep(0.1) #hack for now until we get rid of all the extra threads 480 except KeyboardInterrupt: pass

481

482 -def _ros_signal(sig, stackframe):

483 signal_shutdown("signal-"+str(sig)) 484 prev_handler = _signalChain.get(sig, None) 485 if prev_handler is not None and not type(prev_handler) == int: 486 try: 487 prev_handler(sig, stackframe) 488 except KeyboardInterrupt: 489 pass #filter out generic keyboard interrupt handler

490

491 -def _ros_atexit():

492 signal_shutdown('atexit')

493 atexit.register(_ros_atexit) 494 495 # #687

496 -def register_signals():

497 """ 498 register system signal handlers for SIGTERM and SIGINT 499 """ 500 _signalChain[signal.SIGTERM] = signal.signal(signal.SIGTERM, _ros_signal) 501 _signalChain[signal.SIGINT] = signal.signal(signal.SIGINT, _ros_signal)

502 503 # Validators ###################################### 504

505 -def is_topic(param_name):

506 """ 507 Validator that checks that parameter is a valid ROS topic name 508 """ 509 def validator(param_value, caller_id): 510 v = valid_name_validator_resolved(param_name, param_value, caller_id) 511 if param_value == '/': 512 raise ParameterInvalid("ERROR: parameter [%s] cannot be the global namespace"%param_name) 513 return v

514 return validator 515

516 -def xmlrpcapi(uri):

517 """ 518 @return: instance for calling remote server or None if not a valid URI 519 @rtype: xmlrpclib.ServerProxy 520 """ 521 if uri is None: 522 return None 523 uriValidate = urlparse.urlparse(uri) 524 if not uriValidate[0] or not uriValidate[1]: 525 return None 526 return xmlrpclib.ServerProxy(uri)

527