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 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 import rospkg 60 61 import rosgraph.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 # we keep a separate, non-rosout log file to contain stack traces and 125 # other sorts of information that scare users but are essential for 126 # debugging 127 128 -def rospydebug(msg, *args):

129 """Internal rospy client library debug logging""" 130 _rospy_logger.debug(msg, *args)

131 -def rospyinfo(msg, *args):

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

134 -def rospyerr(msg, *args):

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

137 -def rospywarn(msg, *args):

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

140 141 logdebug = logging.getLogger('rosout').debug 142 143 logwarn = logging.getLogger('rosout').warning 144 145 loginfo = logging.getLogger('rosout').info 146 logout = loginfo # alias deprecated name 147 148 logerr = logging.getLogger('rosout').error 149 logerror = logerr # alias logerr 150 151 logfatal = logging.getLogger('rosout').critical 152 153 ######################################################### 154 # CONSTANTS 155 156 MASTER_NAME = "master" #master is a reserved node name for the central master 157 158 import warnings

159 -def deprecated(func):

160 """This is a decorator which can be used to mark functions 161 as deprecated. It will result in a warning being emmitted 162 when the function is used.""" 163 def newFunc(*args, **kwargs): 164 warnings.warn("Call to deprecated function %s." % func.__name__, 165 category=DeprecationWarning, stacklevel=2) 166 return func(*args, **kwargs)

167 newFunc.__name__ = func.__name__ 168 newFunc.__doc__ = func.__doc__ 169 newFunc.__dict__.update(func.__dict__) 170 return newFunc 171

172 @deprecated 173 -def get_ros_root(required=False, env=None):

174 """ 175 Get the value of ROS_ROOT. 176 @param env: override environment dictionary 177 @type env: dict 178 @param required: if True, fails with ROSException 179 @return: Value of ROS_ROOT environment 180 @rtype: str 181 @raise ROSException: if require is True and ROS_ROOT is not set 182 """ 183 if env is None: 184 env = os.environ 185 ros_root = rospkg.get_ros_root(env) 186 if required and not ros_root: 187 raise rospy.exceptions.ROSException('%s is not set'%rospkg.environment.ROS_ROOT) 188 return ros_root

189 190 191 ######################################################### 192 # API 193 194 _uri = None

195 -def get_node_uri():

196 """ 197 Get this Node's URI. 198 @return: this Node's XMLRPC URI 199 @rtype: str 200 """ 201 return _uri

202

203 -def set_node_uri(uri):

204 """set the URI of the local node. 205 This is an internal API method, it does not actually affect the XMLRPC URI of the Node.""" 206 global _uri 207 _uri = uri

208 209 ######################################################### 210 # Logging 211 212 _log_filename = None

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

214 """ 215 Setup filesystem logging for this node 216 @param node_name: Node's name 217 @type node_name str 218 @param level: (optional) Python logging level (INFO, DEBUG, etc...). (Default: logging.INFO) 219 @type level: int 220 """ 221 global _log_filename 222 223 # #988 __log command-line remapping argument 224 mappings = get_mappings() 225 if '__log' in get_mappings(): 226 logfilename_remap = mappings['__log'] 227 filename = os.path.abspath(logfilename_remap) 228 else: 229 # fix filesystem-unsafe chars 230 filename = node_name.replace('/', '_') + '.log' 231 if filename[0] == '_': 232 filename = filename[1:] 233 if not filename: 234 raise rospy.exceptions.ROSException('invalid configure_logging parameter: %s'%node_name) 235 _log_filename = rosgraph.roslogging.configure_logging('rospy', level, filename=filename)

236

237 -class NullHandler(logging.Handler):

238 - def emit(self, record):

239 pass

240 241 # keep logging happy until we have the node name to configure with 242 logging.getLogger('rospy').addHandler(NullHandler()) 243 244 245 ######################################################### 246 # Init/Shutdown/Exit API and Handlers 247 248 _client_ready = False

249 250 251 -def is_initialized():

252 """ 253 Get the initialization state of the local node. If True, node has 254 been configured. 255 @return: True if local node initialized 256 @rtype: bool 257 """ 258 return _client_ready

259 -def set_initialized(initialized):

260 """ 261 set the initialization state of the local node 262 @param initialized: True if node initialized 263 @type initialized: bool 264 """ 265 global _client_ready 266 _client_ready = initialized

267 268 _shutdown_lock = threading.RLock() 269 270 # _shutdown_flag flags that rospy is in shutdown mode, in_shutdown 271 # flags that the shutdown routine has started. These are separate 272 # because 'pre-shutdown' hooks require rospy to be in a non-shutdown 273 # mode. These hooks are executed during the shutdown routine. 274 _shutdown_flag = False 275 _in_shutdown = False 276 277 # various hooks to call on shutdown. shutdown hooks are called in the 278 # shutdown state, preshutdown are called just before entering shutdown 279 # state, and client shutdown is called before both of these. 280 _shutdown_hooks = [] 281 _preshutdown_hooks = [] 282 _client_shutdown_hooks = [] 283 # threads that must be joined on shutdown 284 _shutdown_threads = [] 285 286 _signalChain = {}

287 288 -def is_shutdown():

289 """ 290 @return: True if shutdown flag has been set 291 @rtype: bool 292 """ 293 return _shutdown_flag

294

295 -def is_shutdown_requested():

296 """ 297 is_shutdown_requested is a state that occurs just before 298 is_shutdown. It is initiated when a shutdown requested is 299 received and continues until client shutdown handlers have been 300 called. After client shutdown handlers have been serviced, the 301 is_shutdown state becomes true. 302 303 @return: True if shutdown has been requested (but possibly not yet initiated) 304 @rtype: bool 305 """ 306 return _in_shutdown

307

308 -def _add_shutdown_hook(h, hooks):

309 """ 310 shared implementation of add_shutdown_hook and add_preshutdown_hook 311 """ 312 if type(h) not in [types.FunctionType, types.MethodType]: 313 raise TypeError("shutdown hook [%s] must be a function: %s"%(h, type(h))) 314 if _shutdown_flag: 315 _logger.warn("add_shutdown_hook called after shutdown") 316 h("already shutdown") 317 return 318 with _shutdown_lock: 319 if hooks is None: 320 # race condition check, don't log as we are deep into shutdown 321 return 322 hooks.append(h)

323

324 -def _add_shutdown_thread(t):

325 """ 326 Register thread that must be joined() on shutdown 327 """ 328 if _shutdown_flag: 329 #TODO 330 return 331 with _shutdown_lock: 332 if _shutdown_threads is None: 333 # race condition check, don't log as we are deep into shutdown 334 return 335 # in order to prevent memory leaks, reap dead threads. The 336 # last thread may not get reaped until shutdown, but this is 337 # relatively minor 338 for other in _shutdown_threads[:]: 339 if not other.isAlive(): 340 _shutdown_threads.remove(other) 341 _shutdown_threads.append(t)

342

343 -def add_client_shutdown_hook(h):

344 """ 345 Add client method to invoke when system shuts down. Unlike 346 L{add_shutdown_hook} and L{add_preshutdown_hooks}, these methods 347 will be called before any rospy internal shutdown code. 348 349 @param h: function that takes in a single string argument (shutdown reason) 350 @type h: fn(str) 351 """ 352 _add_shutdown_hook(h, _client_shutdown_hooks)

353

354 -def add_preshutdown_hook(h):

355 """ 356 Add method to invoke when system shuts down. Unlike 357 L{add_shutdown_hook}, these methods will be called before any 358 other shutdown hooks. 359 360 @param h: function that takes in a single string argument (shutdown reason) 361 @type h: fn(str) 362 """ 363 _add_shutdown_hook(h, _preshutdown_hooks)

364

365 -def add_shutdown_hook(h):

366 """ 367 Add method to invoke when system shuts down. 368 369 Shutdown hooks are called in the order that they are 370 registered. This is an internal API method that is used to 371 cleanup. See the client X{on_shutdown()} method if you wish to 372 register client hooks. 373 374 @param h: function that takes in a single string argument (shutdown reason) 375 @type h: fn(str) 376 """ 377 _add_shutdown_hook(h, _shutdown_hooks)

378

379 -def signal_shutdown(reason):

380 """ 381 Initiates shutdown process by signaling objects waiting on _shutdown_lock. 382 Shutdown and pre-shutdown hooks are invoked. 383 @param reason: human-readable shutdown reason, if applicable 384 @type reason: str 385 """ 386 global _shutdown_flag, _in_shutdown, _shutdown_lock, _shutdown_hooks 387 _logger.info("signal_shutdown [%s]"%reason) 388 if _shutdown_flag or _in_shutdown: 389 return 390 with _shutdown_lock: 391 if _shutdown_flag or _in_shutdown: 392 return 393 _in_shutdown = True 394 395 # make copy just in case client re-invokes shutdown 396 for h in _client_shutdown_hooks: 397 try: 398 # client shutdown hooks do not accept a reason arg 399 h() 400 except: 401 traceback.print_exc() 402 del _client_shutdown_hooks[:] 403 404 for h in _preshutdown_hooks: 405 try: 406 h(reason) 407 except: 408 traceback.print_exc() 409 del _preshutdown_hooks[:] 410 411 # now that pre-shutdown hooks have been called, raise shutdown 412 # flag. This allows preshutdown hooks to still publish and use 413 # service calls properly 414 _shutdown_flag = True 415 for h in _shutdown_hooks: 416 try: 417 h(reason) 418 except Exception as e: 419 sys.stderr.write("signal_shutdown hook error[%s]\n"%e) 420 del _shutdown_hooks[:] 421 422 threads = _shutdown_threads[:] 423 424 for t in threads: 425 if t.isAlive(): 426 t.join(_TIMEOUT_SHUTDOWN_JOIN) 427 del _shutdown_threads[:] 428 try: 429 rospy.rostime.wallsleep(0.1) #hack for now until we get rid of all the extra threads 430 except KeyboardInterrupt: pass

431

432 -def _ros_signal(sig, stackframe):

433 signal_shutdown("signal-"+str(sig)) 434 prev_handler = _signalChain.get(sig, None) 435 if prev_handler is not None and not type(prev_handler) == int: 436 try: 437 prev_handler(sig, stackframe) 438 except KeyboardInterrupt: 439 pass #filter out generic keyboard interrupt handler

440

441 -def _ros_atexit():

442 signal_shutdown('atexit')

443 atexit.register(_ros_atexit)

444 445 # #687 446 -def register_signals():

447 """ 448 register system signal handlers for SIGTERM and SIGINT 449 """ 450 _signalChain[signal.SIGTERM] = signal.signal(signal.SIGTERM, _ros_signal) 451 _signalChain[signal.SIGINT] = signal.signal(signal.SIGINT, _ros_signal)

452

453 # Validators ###################################### 454 455 -def is_topic(param_name):

456 """ 457 Validator that checks that parameter is a valid ROS topic name 458 """ 459 def validator(param_value, caller_id): 460 v = valid_name_validator_resolved(param_name, param_value, caller_id) 461 if param_value == '/': 462 raise ParameterInvalid("ERROR: parameter [%s] cannot be the global namespace"%param_name) 463 return v

464 return validator 465

466 -def xmlrpcapi(uri):

467 """ 468 @return: instance for calling remote server or None if not a valid URI 469 @rtype: xmlrpclib.ServerProxy 470 """ 471 if uri is None: 472 return None 473 uriValidate = urlparse.urlparse(uri) 474 if not uriValidate[0] or not uriValidate[1]: 475 return None 476 return xmlrpcclient.ServerProxy(uri)

477