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 16449 2012-03-06 00:34:27Z kwc $ 34 35 """rospy internal core implementation library""" 36 37 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 49 try: 50 import urllib.parse as urlparse #Python 3.x 51 except ImportError: 52 import urlparse 53 54 try: 55 import xmlrpc.client as xmlrpcclient #Python 3.x 56 except ImportError: 57 import xmlrpclib as xmlrpcclient #Python 2.x 58 59 60 import roslib.rosenv 61 import roslib.roslogging 62 63 import rospy.exceptions 64 import rospy.rostime 65 66 from rospy.names import * 67 from rospy.impl.validators import ParameterInvalid 68 69 from rosgraph_msgs.msg import Log 70 71 _logger = logging.getLogger("rospy.core") 72 73 # number of seconds to wait to join on threads. network issue can 74 # cause joins to be not terminate gracefully, and it's better to 75 # teardown dirty than to hang 76 _TIMEOUT_SHUTDOWN_JOIN = 5. 77 78 import warnings

79 -def deprecated(func):

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

87 newFunc.__name__ = func.__name__ 88 newFunc.__doc__ = func.__doc__ 89 newFunc.__dict__.update(func.__dict__) 90 return newFunc 91 92 ######################################################### 93 # ROSRPC 94 95 ROSRPC = "rosrpc://" 96

97 -def parse_rosrpc_uri(uri):

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

118 119 ######################################################### 120 121 # rospy logger 122 _rospy_logger = logging.getLogger("rospy.internal") 123 124 _logdebug_handlers = [] 125 _loginfo_handlers = [] 126 _logwarn_handlers = [] 127 _logerr_handlers = [] 128 _logfatal_handlers = [] 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 -def add_log_handler(level, h):

148 """ 149 Add handler for specified level 150 @param level: log level (use constants from Log) 151 @type level: int 152 @param h: log message handler 153 @type h: fn 154 @raise ROSInternalException: if level is invalid 155 """ 156 if level == Log.DEBUG: 157 _logdebug_handlers.append(h) 158 elif level == Log.INFO: 159 _loginfo_handlers.append(h) 160 elif level == Log.WARN: 161 _logwarn_handlers.append(h) 162 elif level == Log.ERROR: 163 _logerr_handlers.append(h) 164 elif level == Log.FATAL: 165 _logfatal_handlers.append(h) 166 else: 167 raise rospy.exceptions.ROSInternalException("invalid log level: %s"%level)

168

169 -def logdebug(msg, *args):

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

180

181 -def logwarn(msg, *args):

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

192

193 -def loginfo(msg, *args):

194 """ 195 Log an info 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 _loginfo_handlers: 203 h(msg)

204 logout = loginfo # alias deprecated name 205

206 -def logerr(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 _logerr_handlers: 216 h(msg)

217 logerror = logerr # alias logerr 218

219 -def logfatal(msg, *args):

220 """ 221 Log an error message to the /rosout topic 222 @param msg: message to log, may include formatting arguments 223 @type msg: str 224 @param args: format-string arguments, if necessary 225 """ 226 if args: 227 msg = msg%args 228 for h in _logfatal_handlers: 229 h(msg)

230 231 ######################################################### 232 # CONSTANTS 233 234 MASTER_NAME = "master" #master is a reserved node name for the central master 235

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

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

252 253 254 ######################################################### 255 # API 256 257 _uri = None

258 -def get_node_uri():

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

265

266 -def set_node_uri(uri):

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

271 272 ######################################################### 273 # Logging 274 275 _log_filename = None

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

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

299

300 -class NullHandler(logging.Handler):

301 - def emit(self, record):

302 pass

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

314 -def is_initialized():

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

322 -def set_initialized(initialized):

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

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

351 -def is_shutdown():

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

357

358 -def is_shutdown_requested():

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

370

371 -def _add_shutdown_hook(h, hooks):

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

386

387 -def _add_shutdown_thread(t):

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

405

406 -def add_client_shutdown_hook(h):

407 """ 408 Add client method to invoke when system shuts down. Unlike 409 L{add_shutdown_hook} and L{add_preshutdown_hooks}, these methods 410 will be called before any rospy internal shutdown code. 411 412 @param h: function that takes in a single string argument (shutdown reason) 413 @type h: fn(str) 414 """ 415 _add_shutdown_hook(h, _client_shutdown_hooks)

416

417 -def add_preshutdown_hook(h):

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

427

428 -def add_shutdown_hook(h):

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

441

442 -def signal_shutdown(reason):

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

494

495 -def _ros_signal(sig, stackframe):

496 signal_shutdown("signal-"+str(sig)) 497 prev_handler = _signalChain.get(sig, None) 498 if prev_handler is not None and not type(prev_handler) == int: 499 try: 500 prev_handler(sig, stackframe) 501 except KeyboardInterrupt: 502 pass #filter out generic keyboard interrupt handler

503

504 -def _ros_atexit():

505 signal_shutdown('atexit')

506 atexit.register(_ros_atexit) 507 508 # #687

509 -def register_signals():

510 """ 511 register system signal handlers for SIGTERM and SIGINT 512 """ 513 _signalChain[signal.SIGTERM] = signal.signal(signal.SIGTERM, _ros_signal) 514 _signalChain[signal.SIGINT] = signal.signal(signal.SIGINT, _ros_signal)

515 516 # Validators ###################################### 517

518 -def is_topic(param_name):

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

527 return validator 528

529 -def xmlrpcapi(uri):

530 """ 531 @return: instance for calling remote server or None if not a valid URI 532 @rtype: xmlrpclib.ServerProxy 533 """ 534 if uri is None: 535 return None 536 uriValidate = urlparse.urlparse(uri) 537 if not uriValidate[0] or not uriValidate[1]: 538 return None 539 return xmlrpcclient.ServerProxy(uri)

540