Source Code for Module rospy.impl.init

  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  Internal use: rospy initialization. 
 36   
 37  This is mainly routines for initializing the master or slave based on 
 38  the OS environment. 
 39  """ 
 40   
 41  import os 
 42  import sys 
 43  import logging 
 44  import time 
 45  import traceback 
 46   
 47  import rosgraph 
 48  import rosgraph.xmlrpc 
 49   
 50  from ..names import _set_caller_id 
 51  from ..core import is_shutdown, signal_shutdown, rospyerr 
 52  from ..rostime import is_wallclock, get_time 
 53   
 54  from .tcpros import init_tcpros 
 55  from .masterslave import ROSHandler 
 56   
 57  DEFAULT_NODE_PORT = 0 #bind to any open port 
 58  DEFAULT_MASTER_PORT=11311 #default port for master's to bind to 
 59  DEFAULT_MASTER_URI = 'http://localhost:%s/'%DEFAULT_MASTER_PORT 
 60   
 61  ################################################### 
 62  # rospy module lower-level initialization 
 63   
 64 -def _node_run_error(e): 
 65      """ 
 66      If XML-RPC errors out of the run() method, this handler is invoked 
 67      """ 
 68      rospyerr(traceback.format_exc()) 
 69      signal_shutdown('error in XML-RPC server: %s'%(e)) 
 70   
 71 -def start_node(environ, resolved_name, master_uri=None, port=None): 
 72      """ 
 73      Load ROS slave node, initialize from environment variables 
 74      @param environ: environment variables 
 75      @type  environ: dict 
 76      @param resolved_name: resolved node name 
 77      @type  resolved_name: str 
 78      @param master_uri: override ROS_MASTER_URI: XMlRPC URI of central ROS server 
 79      @type  master_uri: str 
 80      @param port: override ROS_PORT: port of slave xml-rpc node 
 81      @type  port: int 
 82      @return: node server instance 
 83      @rtype rosgraph.xmlrpc.XmlRpcNode 
 84      @raise ROSInitException: if node has already been started 
 85      """ 
 86      init_tcpros() 
 87      if not master_uri: 
 88          master_uri = rosgraph.get_master_uri() 
 89      if not master_uri: 
 90          master_uri = DEFAULT_MASTER_URI 
 91   
 92      # this will go away in future versions of API 
 93      _set_caller_id(resolved_name)  
 94   
 95      handler = ROSHandler(resolved_name, master_uri) 
 96      node = rosgraph.xmlrpc.XmlRpcNode(port, handler, on_run_error=_node_run_error) 
 97      node.start() 
 98      while not node.uri and not is_shutdown(): 
 99          time.sleep(0.00001) #poll for XMLRPC init 
100      logging.getLogger("rospy.init").info("ROS Slave URI: [%s]", node.uri) 
101   
102      while not handler._is_registered() and not is_shutdown(): 
103          time.sleep(0.1) #poll for master registration 
104      logging.getLogger("rospy.init").info("registered with master") 
105      return node 
106   
107  _logging_to_rospy_names = { 
108        'DEBUG':    'DEBUG', 
109        'INFO':     'INFO', 
110        'WARNING':  'WARN', 
111        'ERROR':    'ERROR', 
112        'CRITICAL': 'FATAL', 
113        } 
114   
115 -class RosStreamHandler(logging.Handler): 
116 -   def emit(self, record): 
117        # TODO: (AJH) convert levelname CRITICAL to FATAL 
118        level = _logging_to_rospy_names[record.levelname] 
119        if is_wallclock(): 
120           msg = "[%s] [WallTime: %f] %s\n"%(level, time.time(), 
121                 record.getMessage()) 
122        else: 
123           msg = "[%s] [WallTime: %f] [%f] %s\n"%(level, time.time(), get_time(), 
124                 record.getMessage()) 
125        if record.levelno < logging.WARNING: 
126           sys.stdout.write(msg) 
127        else: 
128           sys.stderr.write(msg) 
129   
130  _loggers_initialized = False 
131 -def init_log_handlers(): 
132      global _loggers_initialized 
133      if _loggers_initialized: 
134          return 
135   
136      logging.getLogger('').addHandler(RosStreamHandler()) 
137