Ejemplo n.º 1
0
class ROSMasterHandler(object):
    """
    XML-RPC handler for ROS master APIs.
    API routines for the ROS Master Node. The Master Node is a
    superset of the Slave Node and contains additional API methods for
    creating and monitoring a graph of slave nodes.

    By convention, ROS nodes take in caller_id as the first parameter
    of any API call.  The setting of this parameter is rarely done by
    client code as ros::msproxy::MasterProxy automatically inserts
    this parameter (see ros::client::getMaster()).
    """
    def __init__(self, num_workers=NUM_WORKERS):
        """ctor."""

        self.uri = None
        self.done = False

        self.thread_pool = rosmaster.threadpool.MarkedThreadPool(num_workers)
        # pub/sub/providers: dict { topicName : [publishers/subscribers names] }
        self.ps_lock = threading.Condition(threading.Lock())

        self.reg_manager = RegistrationManager(self.thread_pool)

        # maintain refs to reg_manager fields
        self.publishers = self.reg_manager.publishers
        self.subscribers = self.reg_manager.subscribers
        self.services = self.reg_manager.services
        self.param_subscribers = self.reg_manager.param_subscribers

        self.topics_types = {}  #dict { topicName : type }

        # parameter server dictionary
        self.param_server = rosmaster.paramserver.ParamDictionary(
            self.reg_manager)

    def _shutdown(self, reason=''):
        if self.thread_pool is not None:
            self.thread_pool.join_all(wait_for_tasks=False,
                                      wait_for_threads=False)
            self.thread_pool = None
        self.done = True

    def _ready(self, uri):
        """
        Initialize the handler with the XMLRPC URI. This is a standard callback from the XmlRpcNode API.

        @param uri: XML-RPC URI
        @type  uri: str
        """
        self.uri = uri

    def _ok(self):
        return not self.done

    ###############################################################################
    # EXTERNAL API

    @apivalidate(0, (None, ))
    def shutdown(self, caller_id, msg=''):
        """
        Stop this server
        @param caller_id: ROS caller id
        @type  caller_id: str
        @param msg: a message describing why the node is being shutdown.
        @type  msg: str
        @return: [code, msg, 0]
        @rtype: [int, str, int]
        """
        if msg:
            print("shutdown request: %s" % msg, file=sys.stdout)
        else:
            print("shutdown requst", file=sys.stdout)
        self._shutdown('external shutdown request from [%s]: %s' %
                       (caller_id, msg))
        return 1, "shutdown", 0

    @apivalidate('')
    def getUri(self, caller_id):
        """
        Get the XML-RPC URI of this server.
        @param caller_id str: ROS caller id    
        @return [int, str, str]: [1, "", xmlRpcUri]
        """
        return 1, "", self.uri

    @apivalidate(-1)
    def getPid(self, caller_id):
        """
        Get the PID of this server
        @param caller_id: ROS caller id
        @type  caller_id: str
        @return: [1, "", serverProcessPID]
        @rtype: [int, str, int]
        """
        return 1, "", os.getpid()

    ################################################################
    # PARAMETER SERVER ROUTINES

    @apivalidate(0, (non_empty_str('key'), ))
    def deleteParam(self, caller_id, key):
        """
        Parameter Server: delete parameter
        @param caller_id: ROS caller id
        @type  caller_id: str
        @param key: parameter name
        @type  key: str
        @return: [code, msg, 0]
        @rtype: [int, str, int]
        """
        try:
            key = resolve_name(key, caller_id)
            self.param_server.delete_param(key, self._notify_param_subscribers)
            mloginfo("-PARAM [%s] by %s", key, caller_id)
            return 1, "parameter %s deleted" % key, 0
        except KeyError as e:
            return -1, "parameter [%s] is not set" % key, 0

    @apivalidate(0, (non_empty_str('key'), not_none('value')))
    def setParam(self, caller_id, key, value):
        """
        Parameter Server: set parameter.  NOTE: if value is a
        dictionary it will be treated as a parameter tree, where key
        is the parameter namespace. For example:::
          {'x':1,'y':2,'sub':{'z':3}}

        will set key/x=1, key/y=2, and key/sub/z=3. Furthermore, it
        will replace all existing parameters in the key parameter
        namespace with the parameters in value. You must set
        parameters individually if you wish to perform a union update.
        
        @param caller_id: ROS caller id
        @type  caller_id: str
        @param key: parameter name
        @type  key: str
        @param value: parameter value.
        @type  value: XMLRPCLegalValue
        @return: [code, msg, 0]
        @rtype: [int, str, int]
        """
        key = resolve_name(key, caller_id)
        self.param_server.set_param(key, value, self._notify_param_subscribers)
        mloginfo("+PARAM [%s] by %s", key, caller_id)
        return 1, "parameter %s set" % key, 0

    @apivalidate(0, (non_empty_str('key'), ))
    def getParam(self, caller_id, key):
        """
        Retrieve parameter value from server.
        @param caller_id: ROS caller id
        @type  caller_id: str
        @param key: parameter to lookup. If key is a namespace,
        getParam() will return a parameter tree.
        @type  key: str
        getParam() will return a parameter tree.

        @return: [code, statusMessage, parameterValue]. If code is not
            1, parameterValue should be ignored. If key is a namespace,
            the return value will be a dictionary, where each key is a
            parameter in that namespace. Sub-namespaces are also
            represented as dictionaries.
        @rtype: [int, str, XMLRPCLegalValue]
        """
        try:
            key = resolve_name(key, caller_id)
            return 1, "Parameter [%s]" % key, self.param_server.get_param(key)
        except KeyError as e:
            return -1, "Parameter [%s] is not set" % key, 0

    @apivalidate(0, (non_empty_str('key'), ))
    def searchParam(self, caller_id, key):
        """
        Search for parameter key on parameter server. Search starts in caller's namespace and proceeds
        upwards through parent namespaces until Parameter Server finds a matching key.

        searchParam's behavior is to search for the first partial match.
        For example, imagine that there are two 'robot_description' parameters::
          
           /robot_description
             /robot_description/arm
             /robot_description/base
           /pr2/robot_description
             /pr2/robot_description/base

        If I start in the namespace /pr2/foo and search for
        'robot_description', searchParam will match
        /pr2/robot_description. If I search for 'robot_description/arm'
        it will return /pr2/robot_description/arm, even though that
        parameter does not exist (yet).

        @param caller_id str: ROS caller id
        @type  caller_id: str
        @param key: parameter key to search for.
        @type  key: str
        @return: [code, statusMessage, foundKey]. If code is not 1, foundKey should be
            ignored. 
        @rtype: [int, str, str]
        """
        search_key = self.param_server.search_param(caller_id, key)
        if search_key:
            return 1, "Found [%s]" % search_key, search_key
        else:
            return -1, "Cannot find parameter [%s] in an upwards search" % key, ''

    @apivalidate(0, (
        is_api('caller_api'),
        non_empty_str('key'),
    ))
    def subscribeParam(self, caller_id, caller_api, key):
        """
        Retrieve parameter value from server and subscribe to updates to that param. See
        paramUpdate() in the Node API. 
        @param caller_id str: ROS caller id
        @type  caller_id: str
        @param key: parameter to lookup.
        @type  key: str
        @param caller_api: API URI for paramUpdate callbacks.
        @type  caller_api: str
        @return: [code, statusMessage, parameterValue]. If code is not
           1, parameterValue should be ignored. parameterValue is an empty dictionary if the parameter
           has not been set yet.
        @rtype: [int, str, XMLRPCLegalValue]
        """
        key = resolve_name(key, caller_id)
        try:
            # ps_lock has precedence and is required due to
            # potential self.reg_manager modification
            self.ps_lock.acquire()
            val = self.param_server.subscribe_param(key,
                                                    (caller_id, caller_api))
        finally:
            self.ps_lock.release()
        return 1, "Subscribed to parameter [%s]" % key, val

    @apivalidate(0, (
        is_api('caller_api'),
        non_empty_str('key'),
    ))
    def unsubscribeParam(self, caller_id, caller_api, key):
        """
        Retrieve parameter value from server and subscribe to updates to that param. See
        paramUpdate() in the Node API. 
        @param caller_id str: ROS caller id
        @type  caller_id: str
        @param key: parameter to lookup.
        @type  key: str
        @param caller_api: API URI for paramUpdate callbacks.
        @type  caller_api: str
        @return: [code, statusMessage, numUnsubscribed]. 
           If numUnsubscribed is zero it means that the caller was not subscribed to the parameter.
        @rtype: [int, str, int]
        """
        key = resolve_name(key, caller_id)
        try:
            # ps_lock is required due to potential self.reg_manager modification
            self.ps_lock.acquire()
            retval = self.param_server.unsubscribe_param(
                key, (caller_id, caller_api))
        finally:
            self.ps_lock.release()
        return 1, "Unsubscribe to parameter [%s]" % key, 1

    @apivalidate(False, (non_empty_str('key'), ))
    def hasParam(self, caller_id, key):
        """
        Check if parameter is stored on server. 
        @param caller_id str: ROS caller id
        @type  caller_id: str
        @param key: parameter to check
        @type  key: str
        @return: [code, statusMessage, hasParam]
        @rtype: [int, str, bool]
        """
        key = resolve_name(key, caller_id)
        if self.param_server.has_param(key):
            return 1, key, True
        else:
            return 1, key, False

    @apivalidate([])
    def getParamNames(self, caller_id):
        """
        Get list of all parameter names stored on this server.
        This does not adjust parameter names for caller's scope.
        
        @param caller_id: ROS caller id    
        @type  caller_id: str
        @return: [code, statusMessage, parameterNameList]
        @rtype: [int, str, [str]]
        """
        return 1, "Parameter names", self.param_server.get_param_names()

    ##################################################################################
    # NOTIFICATION ROUTINES

    def _notify(self, registrations, task, key, value, node_apis):
        """
        Generic implementation of callback notification
        @param registrations: Registrations
        @type  registrations: L{Registrations}
        @param task: task to queue
        @type  task: fn
        @param key: registration key
        @type  key: str
        @param value: value to pass to task
        @type  value: Any
        """
        # cache thread_pool for thread safety
        thread_pool = self.thread_pool
        if not thread_pool:
            return

        try:
            for node_api in node_apis:
                # use the api as a marker so that we limit one thread per subscriber
                thread_pool.queue_task(node_api, task, (node_api, key, value))
        except KeyError:
            _logger.warn(
                'subscriber data stale (key [%s], listener [%s]): node API unknown'
                % (key, s))

    def _notify_param_subscribers(self, updates):
        """
        Notify parameter subscribers of new parameter value
        @param updates [([str], str, any)*]: [(subscribers, param_key, param_value)*]
        @param param_value str: parameter value
        """
        # cache thread_pool for thread safety
        thread_pool = self.thread_pool
        if not thread_pool:
            return

        for subscribers, key, value in updates:
            # use the api as a marker so that we limit one thread per subscriber
            for caller_id, caller_api in subscribers:
                self.thread_pool.queue_task(
                    caller_api, self.param_update_task,
                    (caller_id, caller_api, key, value))

    def param_update_task(self, caller_id, caller_api, param_key, param_value):
        """
        Contact api.paramUpdate with specified parameters
        @param caller_id: caller ID
        @type  caller_id: str
        @param caller_api: XML-RPC URI of node to contact
        @type  caller_api: str
        @param param_key: parameter key to pass to node
        @type  param_key: str
        @param param_value: parameter value to pass to node
        @type  param_value: str
        """
        mloginfo("paramUpdate[%s]", param_key)
        code, _, _ = xmlrpcapi(caller_api).paramUpdate('/master', param_key,
                                                       param_value)
        if code == -1:
            try:
                # ps_lock is required due to potential self.reg_manager modification
                self.ps_lock.acquire()
                # reverse lookup to figure out who we just called
                matches = self.reg_manager.reverse_lookup(caller_api)
                for m in matches:
                    retval = self.param_server.unsubscribe_param(
                        param_key, (m.id, caller_api))
            finally:
                self.ps_lock.release()

    def _notify_topic_subscribers(self, topic, pub_uris, sub_uris):
        """
        Notify subscribers with new publisher list
        @param topic: name of topic
        @type  topic: str
        @param pub_uris: list of URIs of publishers.
        @type  pub_uris: [str]
        """
        self._notify(self.subscribers, publisher_update_task, topic, pub_uris,
                     sub_uris)

    ##################################################################################
    # SERVICE PROVIDER

    @apivalidate(
        0,
        (is_service('service'), is_api('service_api'), is_api('caller_api')))
    def registerService(self, caller_id, service, service_api, caller_api):
        """
        Register the caller as a provider of the specified service.
        @param caller_id str: ROS caller id
        @type  caller_id: str
        @param service: Fully-qualified name of service 
        @type  service: str
        @param service_api: Service URI 
        @type  service_api: str
        @param caller_api: XML-RPC URI of caller node 
        @type  caller_api: str
        @return: (code, message, ignore)
        @rtype: (int, str, int)
        """
        try:
            self.ps_lock.acquire()
            self.reg_manager.register_service(service, caller_id, caller_api,
                                              service_api)
            mloginfo("+SERVICE [%s] %s %s", service, caller_id, caller_api)
        finally:
            self.ps_lock.release()
        return 1, "Registered [%s] as provider of [%s]" % (caller_id,
                                                           service), 1

    @apivalidate('', (is_service('service'), ))
    def lookupService(self, caller_id, service):
        """
        Lookup all provider of a particular service.
        @param caller_id str: ROS caller id
        @type  caller_id: str
        @param service: fully-qualified name of service to lookup.
        @type: service: str
        @return: (code, message, serviceUrl). service URL is provider's
           ROSRPC URI with address and port.  Fails if there is no provider.
        @rtype: (int, str, str)
        """
        try:
            self.ps_lock.acquire()
            service_url = self.services.get_service_api(service)
        finally:
            self.ps_lock.release()
        if service_url:
            return 1, "rosrpc URI: [%s]" % service_url, service_url
        else:
            return -1, "no provider", ''

    @apivalidate(0, (is_service('service'), is_api('service_api')))
    def unregisterService(self, caller_id, service, service_api):
        """
        Unregister the caller as a provider of the specified service.
        @param caller_id str: ROS caller id
        @type  caller_id: str
        @param service: Fully-qualified name of service
        @type  service: str
        @param service_api: API URI of service to unregister. Unregistration will only occur if current
           registration matches.
        @type  service_api: str
        @return: (code, message, numUnregistered). Number of unregistrations (either 0 or 1).
           If this is zero it means that the caller was not registered as a service provider.
           The call still succeeds as the intended final state is reached.
        @rtype: (int, str, int)
        """
        try:
            self.ps_lock.acquire()
            retval = self.reg_manager.unregister_service(
                service, caller_id, service_api)
            mloginfo("-SERVICE [%s] %s %s", service, caller_id, service_api)
            return retval
        finally:
            self.ps_lock.release()

    ##################################################################################
    # PUBLISH/SUBSCRIBE

    @apivalidate([], (is_topic('topic'), valid_type_name('topic_type'),
                      is_api('caller_api')))
    def registerSubscriber(self, caller_id, topic, topic_type, caller_api):
        """
        Subscribe the caller to the specified topic. In addition to receiving
        a list of current publishers, the subscriber will also receive notifications
        of new publishers via the publisherUpdate API.        
        @param caller_id: ROS caller id
        @type  caller_id: str
        @param topic str: Fully-qualified name of topic to subscribe to. 
        @param topic_type: Datatype for topic. Must be a package-resource name, i.e. the .msg name.
        @type  topic_type: str
        @param caller_api: XML-RPC URI of caller node for new publisher notifications
        @type  caller_api: str
        @return: (code, message, publishers). Publishers is a list of XMLRPC API URIs
           for nodes currently publishing the specified topic.
        @rtype: (int, str, [str])
        """
        #NOTE: subscribers do not get to set topic type
        try:
            self.ps_lock.acquire()
            self.reg_manager.register_subscriber(topic, caller_id, caller_api)

            # ROS 1.1: subscriber can now set type if it is not already set
            #  - don't let '*' type squash valid typing
            if not topic in self.topics_types and topic_type != rosgraph.names.ANYTYPE:
                self.topics_types[topic] = topic_type

            mloginfo("+SUB [%s] %s %s", topic, caller_id, caller_api)
            pub_uris = self.publishers.get_apis(topic)
        finally:
            self.ps_lock.release()
        return 1, "Subscribed to [%s]" % topic, pub_uris

    @apivalidate(0, (is_topic('topic'), is_api('caller_api')))
    def unregisterSubscriber(self, caller_id, topic, caller_api):
        """
        Unregister the caller as a subscriber of the topic.
        @param caller_id: ROS caller id
        @type  caller_id: str
        @param topic: Fully-qualified name of topic to unregister.
        @type  topic: str
        @param caller_api: API URI of service to unregister. Unregistration will only occur if current
           registration matches.    
        @type  caller_api: str
        @return: (code, statusMessage, numUnsubscribed). 
          If numUnsubscribed is zero it means that the caller was not registered as a subscriber.
          The call still succeeds as the intended final state is reached.
        @rtype: (int, str, int)
        """
        try:
            self.ps_lock.acquire()
            retval = self.reg_manager.unregister_subscriber(
                topic, caller_id, caller_api)
            mloginfo("-SUB [%s] %s %s", topic, caller_id, caller_api)
            return retval
        finally:
            self.ps_lock.release()

    @apivalidate([], (is_topic('topic'), valid_type_name('topic_type'),
                      is_api('caller_api')))
    def registerPublisher(self, caller_id, topic, topic_type, caller_api):
        """
        Register the caller as a publisher the topic.
        @param caller_id: ROS caller id
        @type  caller_id: str
        @param topic: Fully-qualified name of topic to register.
        @type  topic: str
        @param topic_type: Datatype for topic. Must be a
        package-resource name, i.e. the .msg name.
        @type  topic_type: str
        @param caller_api str: ROS caller XML-RPC API URI
        @type  caller_api: str
        @return: (code, statusMessage, subscriberApis).
        List of current subscribers of topic in the form of XMLRPC URIs.
        @rtype: (int, str, [str])
        """
        #NOTE: we need topic_type for getPublishedTopics.
        try:
            self.ps_lock.acquire()
            self.reg_manager.register_publisher(topic, caller_id, caller_api)
            # don't let '*' type squash valid typing
            if topic_type != rosgraph.names.ANYTYPE or not topic in self.topics_types:
                self.topics_types[topic] = topic_type
            pub_uris = self.publishers.get_apis(topic)
            sub_uris = self.subscribers.get_apis(topic)
            self._notify_topic_subscribers(topic, pub_uris, sub_uris)
            mloginfo("+PUB [%s] %s %s", topic, caller_id, caller_api)
            sub_uris = self.subscribers.get_apis(topic)
        finally:
            self.ps_lock.release()
        return 1, "Registered [%s] as publisher of [%s]" % (caller_id,
                                                            topic), sub_uris

    @apivalidate(0, (is_topic('topic'), is_api('caller_api')))
    def unregisterPublisher(self, caller_id, topic, caller_api):
        """
        Unregister the caller as a publisher of the topic.
        @param caller_id: ROS caller id
        @type  caller_id: str
        @param topic: Fully-qualified name of topic to unregister.
        @type  topic: str
        @param caller_api str: API URI of service to
           unregister. Unregistration will only occur if current
           registration matches.
        @type  caller_api: str
        @return: (code, statusMessage, numUnregistered). 
           If numUnregistered is zero it means that the caller was not registered as a publisher.
           The call still succeeds as the intended final state is reached.
        @rtype: (int, str, int)
        """
        try:
            self.ps_lock.acquire()
            retval = self.reg_manager.unregister_publisher(
                topic, caller_id, caller_api)
            if retval[VAL]:
                self._notify_topic_subscribers(
                    topic, self.publishers.get_apis(topic),
                    self.subscribers.get_apis(topic))
            mloginfo("-PUB [%s] %s %s", topic, caller_id, caller_api)
        finally:
            self.ps_lock.release()
        return retval

    ##################################################################################
    # GRAPH STATE APIS

    @apivalidate('', (valid_name('node'), ))
    def lookupNode(self, caller_id, node_name):
        """
        Get the XML-RPC URI of the node with the associated
        name/caller_id.  This API is for looking information about
        publishers and subscribers. Use lookupService instead to lookup
        ROS-RPC URIs.
        @param caller_id: ROS caller id
        @type  caller_id: str
        @param node: name of node to lookup
        @type  node: str
        @return: (code, msg, URI)
        @rtype: (int, str, str)
        """
        try:
            self.ps_lock.acquire()
            node = self.reg_manager.get_node(node_name)
            if node is not None:
                retval = 1, "node api", node.api
            else:
                retval = -1, "unknown node [%s]" % node_name, ''
        finally:
            self.ps_lock.release()
        return retval

    @apivalidate(0, (empty_or_valid_name('subgraph'), ))
    def getPublishedTopics(self, caller_id, subgraph):
        """
        Get list of topics that can be subscribed to. This does not return topics that have no publishers.
        See L{getSystemState()} to get more comprehensive list.
        @param caller_id: ROS caller id
        @type  caller_id: str
        @param subgraph: Restrict topic names to match within the specified subgraph. Subgraph namespace
           is resolved relative to the caller's namespace. Use '' to specify all names.
        @type  subgraph: str
        @return: (code, msg, [[topic1, type1]...[topicN, typeN]])
        @rtype: (int, str, [[str, str],])
        """
        try:
            self.ps_lock.acquire()
            # force subgraph to be a namespace with trailing slash
            if subgraph and subgraph[-1] != rosgraph.names.SEP:
                subgraph = subgraph + rosgraph.names.SEP
            #we don't bother with subscribers as subscribers don't report topic types. also, the intended
            #use case is for subscribe-by-topic-type
            retval = [[t, self.topics_types[t]]
                      for t in self.publishers.iterkeys()
                      if t.startswith(subgraph)]
        finally:
            self.ps_lock.release()
        return 1, "current topics", retval

    @apivalidate([])
    def getTopicTypes(self, caller_id):
        """
        Retrieve list topic names and their types.
        @param caller_id: ROS caller id    
        @type  caller_id: str
        @rtype: (int, str, [[str,str]] )
        @return: (code, statusMessage, topicTypes). topicTypes is a list of [topicName, topicType] pairs.
        """
        try:
            self.ps_lock.acquire()
            retval = list(self.topics_types.items())
        finally:
            self.ps_lock.release()
        return 1, "current system state", retval

    @apivalidate([[], [], []])
    def getSystemState(self, caller_id):
        """
        Retrieve list representation of system state (i.e. publishers, subscribers, and services).
        @param caller_id: ROS caller id    
        @type  caller_id: str
        @rtype: (int, str, [[str,[str]], [str,[str]], [str,[str]]])
        @return: (code, statusMessage, systemState).

           System state is in list representation::
             [publishers, subscribers, services].
        
           publishers is of the form::
             [ [topic1, [topic1Publisher1...topic1PublisherN]] ... ]
        
           subscribers is of the form::
             [ [topic1, [topic1Subscriber1...topic1SubscriberN]] ... ]
        
           services is of the form::
             [ [service1, [service1Provider1...service1ProviderN]] ... ]
        """
        edges = []
        try:
            self.ps_lock.acquire()
            retval = [
                r.get_state()
                for r in (self.publishers, self.subscribers, self.services)
            ]
        finally:
            self.ps_lock.release()
        return 1, "current system state", retval
Ejemplo n.º 2
0
    def test_RegistrationManager_services(self):
        from rosmaster.registrations import Registrations, RegistrationManager
        rm = RegistrationManager(ThreadPoolMock())
        
        self.assertEquals(None, rm.get_node('caller1'))

        # do an unregister first, before service_api is initialized
        code, msg, val = rm.unregister_service('s1', 'caller1', 'rosrpc://one:1234')
        self.assertEquals(1, code)
        self.assertEquals(0, val)        
        
        rm.register_service('s1', 'caller1', 'http://one:1234', 'rosrpc://one:1234')
        self.assert_(rm.services.has_key('s1'))
        self.assertEquals('rosrpc://one:1234', rm.services.get_service_api('s1')) 
        self.assertEquals('http://one:1234', rm.get_node('caller1').api)
        self.assertEquals([['s1', ['caller1']]], rm.services.get_state())
        
        # - verify that changed caller_api updates ref
        rm.register_service('s1', 'caller1', 'http://oneB:1234', 'rosrpc://one:1234')
        self.assert_(rm.services.has_key('s1'))
        self.assertEquals('rosrpc://one:1234', rm.services.get_service_api('s1'))        
        self.assertEquals('http://oneB:1234', rm.get_node('caller1').api)
        self.assertEquals([['s1', ['caller1']]], rm.services.get_state())
        
        # - verify that changed service_api updates ref
        rm.register_service('s1', 'caller1', 'http://oneB:1234', 'rosrpc://oneB:1234')
        self.assert_(rm.services.has_key('s1'))
        self.assertEquals('rosrpc://oneB:1234', rm.services.get_service_api('s1'))        
        self.assertEquals('http://oneB:1234', rm.get_node('caller1').api)
        self.assertEquals([['s1', ['caller1']]], rm.services.get_state())
        
        rm.register_service('s2', 'caller2', 'http://two:1234', 'rosrpc://two:1234')
        self.assertEquals('http://two:1234', rm.get_node('caller2').api)

        # - unregister should be noop if service api does not match
        code, msg, val = rm.unregister_service('s2', 'caller2', 'rosrpc://b:1234')
        self.assertEquals(1, code)
        self.assertEquals(0, val)        
        self.assert_(rm.services.has_key('s2'))
        self.assertEquals('http://two:1234', rm.get_node('caller2').api)        
        self.assertEquals('rosrpc://two:1234', rm.services.get_service_api('s2'))
        
        # - unregister should be noop if service is unknown
        code, msg, val = rm.unregister_service('unknown', 'caller2', 'rosrpc://two:1234')
        self.assertEquals(1, code)
        self.assertEquals(0, val)        
        self.assert_(rm.services.has_key('s2'))
        self.assertEquals('http://two:1234', rm.get_node('caller2').api)        
        self.assertEquals('rosrpc://two:1234', rm.services.get_service_api('s2'))

        # - unregister should clear all knowledge of caller2
        code,msg, val = rm.unregister_service('s2', 'caller2', 'rosrpc://two:1234')
        self.assertEquals(1, code)
        self.assertEquals(1, val)        
        self.assert_(rm.services.has_key('s1')) 
        self.failIf(rm.services.has_key('s2'))        
        self.assertEquals(None, rm.get_node('caller2'))

        code, msg, val = rm.unregister_service('s1', 'caller1', 'rosrpc://oneB:1234')
        self.assertEquals(1, code)        
        self.assertEquals(1, val)        
        self.assert_(not rm.services.__nonzero__())
        self.failIf(rm.services.has_key('s1'))        
        self.assertEquals(None, rm.get_node('caller1'))        
Ejemplo n.º 3
0
    def test_RegistrationManager_base(self):
        import rosmaster.exceptions
        from rosmaster.registrations import Registrations, RegistrationManager
        threadpool = ThreadPoolMock()

        rm = RegistrationManager(threadpool)
        self.assert_(isinstance(rm.services, Registrations))
        self.assertEquals(Registrations.SERVICE, rm.services.type)
        self.assert_(isinstance(rm.param_subscribers, Registrations))
        self.assertEquals(Registrations.PARAM_SUBSCRIPTIONS, rm.param_subscribers.type)
        self.assert_(isinstance(rm.subscribers, Registrations))
        self.assertEquals(Registrations.TOPIC_SUBSCRIPTIONS, rm.subscribers.type)
        self.assert_(isinstance(rm.subscribers, Registrations))
        self.assertEquals(Registrations.TOPIC_PUBLICATIONS, rm.publishers.type)
        self.assert_(isinstance(rm.publishers, Registrations))

        #test auto-clearing of registrations if node API changes
        rm.register_publisher('pub1', 'caller1', 'http://one:1')
        rm.register_publisher('pub1', 'caller2', 'http://two:1')
        rm.register_publisher('pub1', 'caller3', 'http://three:1')
        rm.register_subscriber('sub1', 'caller1', 'http://one:1')
        rm.register_subscriber('sub1', 'caller2', 'http://two:1')
        rm.register_subscriber('sub1', 'caller3', 'http://three:1')
        rm.register_param_subscriber('p1', 'caller1', 'http://one:1')
        rm.register_param_subscriber('p1', 'caller2', 'http://two:1')
        rm.register_param_subscriber('p1', 'caller3', 'http://three:1')
        rm.register_service('s1', 'caller1', 'http://one:1', 'rosrpc://one:1')
        self.assertEquals('http://one:1', rm.get_node('caller1').api)
        self.assertEquals('http://two:1', rm.get_node('caller2').api)
        self.assertEquals('http://three:1', rm.get_node('caller3').api)        

        # - first, make sure that changing rosrpc URI does not erase state
        rm.register_service('s1', 'caller1', 'http://one:1', 'rosrpc://oneB:1')
        n = rm.get_node('caller1')
        self.assertEquals(['pub1'], n.topic_publications)
        self.assertEquals(['sub1'], n.topic_subscriptions)
        self.assertEquals(['p1'], n.param_subscriptions)                
        self.assertEquals(['s1'], n.services)
        self.assert_('http://one:1' in rm.publishers.get_apis('pub1'))
        self.assert_('http://one:1' in rm.subscribers.get_apis('sub1'))
        self.assert_('http://one:1' in rm.param_subscribers.get_apis('p1'))
        self.assert_('http://one:1' in rm.services.get_apis('s1'))

        # - also, make sure unregister does not erase state if API changed
        rm.unregister_publisher('pub1', 'caller1', 'http://not:1')
        self.assert_('http://one:1' in rm.publishers.get_apis('pub1'))
        rm.unregister_subscriber('sub1', 'caller1', 'http://not:1')
        self.assert_('http://one:1' in rm.subscribers.get_apis('sub1'))
        rm.unregister_param_subscriber('p1', 'caller1', 'http://not:1')
        self.assert_('http://one:1' in rm.param_subscribers.get_apis('p1'))
        rm.unregister_service('sub1', 'caller1', 'rosrpc://not:1')
        self.assert_('http://one:1' in rm.services.get_apis('s1'))
        
        
        # erase caller1 sub/srvs/params via register_publisher
        rm.register_publisher('pub1', 'caller1', 'http://newone:1')
        self.assertEquals('http://newone:1', rm.get_node('caller1').api)        
        # - check node ref
        n = rm.get_node('caller1')
        self.assertEquals(['pub1'], n.topic_publications)
        self.assertEquals([], n.services)
        self.assertEquals([], n.topic_subscriptions)
        self.assertEquals([], n.param_subscriptions)
        # - checks publishers
        self.assert_('http://newone:1' in rm.publishers.get_apis('pub1'))
        # - checks subscribers
        self.assert_(rm.subscribers.has_key('sub1'))
        self.failIf('http://one:1' in rm.subscribers.get_apis('sub1'))
        # - checks param subscribers
        self.assert_(rm.param_subscribers.has_key('p1'))
        self.failIf('http://one:1' in rm.param_subscribers.get_apis('p1'))

        # erase caller2 pub/sub/params via register_service
        # - initial state
        self.assert_('http://two:1' in rm.publishers.get_apis('pub1'))
        self.assert_('http://two:1' in rm.subscribers.get_apis('sub1'))
        self.assert_('http://two:1' in rm.param_subscribers.get_apis('p1'))
        # - change ownership of s1 to caller2
        rm.register_service('s1', 'caller2', 'http://two:1', 'rosrpc://two:1')
        self.assert_('http://two:1' in rm.services.get_apis('s1'))
        self.assert_('http://two:1' in rm.publishers.get_apis('pub1'))
        self.assert_('http://two:1' in rm.subscribers.get_apis('sub1'))
        self.assert_('http://two:1' in rm.param_subscribers.get_apis('p1'))
        
        rm.register_service('s1', 'caller2', 'http://newtwo:1', 'rosrpc://newtwo:1')
        self.assertEquals('http://newone:1', rm.get_node('caller1').api)        
        # - check node ref
        n = rm.get_node('caller2')
        self.assertEquals([], n.topic_publications)
        self.assertEquals(['s1'], n.services)
        self.assertEquals([], n.topic_subscriptions)
        self.assertEquals([], n.param_subscriptions)
        # - checks publishers
        self.assert_(rm.publishers.has_key('pub1'))
        self.failIf('http://two:1' in rm.publishers.get_apis('pub1'))
        # - checks subscribers
        self.assert_(rm.subscribers.has_key('sub1'))
        self.failIf('http://two:1' in rm.subscribers.get_apis('sub1'))
        self.assertEquals([['sub1', ['caller3']]], rm.subscribers.get_state())
        # - checks param subscribers
        self.assert_(rm.param_subscribers.has_key('p1'))
        self.failIf('http://two:1' in rm.param_subscribers.get_apis('p1'))
        self.assertEquals([['p1', ['caller3']]], rm.param_subscribers.get_state())
Ejemplo n.º 4
0
class ROSMasterHandler(object):
    """
    XML-RPC handler for ROS master APIs.
    API routines for the ROS Master Node. The Master Node is a
    superset of the Slave Node and contains additional API methods for
    creating and monitoring a graph of slave nodes.

    By convention, ROS nodes take in caller_id as the first parameter
    of any API call.  The setting of this parameter is rarely done by
    client code as ros::msproxy::MasterProxy automatically inserts
    this parameter (see ros::client::getMaster()).
    """
    
    def __init__(self, num_workers=NUM_WORKERS):
        """ctor."""

        self.uri = None
        self.done = False

        self.thread_pool = rosmaster.threadpool.MarkedThreadPool(num_workers)
        # pub/sub/providers: dict { topicName : [publishers/subscribers names] }
        self.ps_lock = threading.Condition(threading.Lock())

        self.reg_manager = RegistrationManager(self.thread_pool)

        # maintain refs to reg_manager fields
        self.publishers  = self.reg_manager.publishers
        self.subscribers = self.reg_manager.subscribers
        self.services = self.reg_manager.services
        self.param_subscribers = self.reg_manager.param_subscribers
        
        self.topics_types = {} #dict { topicName : type }

        # parameter server dictionary
        self.param_server = rosmaster.paramserver.ParamDictionary(self.reg_manager)

    def _shutdown(self, reason=''):
        if self.thread_pool is not None:
            self.thread_pool.join_all(wait_for_tasks=False, wait_for_threads=False)
            self.thread_pool = None
        self.done = True
        
    def _ready(self, uri):
        """
        Initialize the handler with the XMLRPC URI. This is a standard callback from the XmlRpcNode API.

        @param uri: XML-RPC URI
        @type  uri: str
        """
        self.uri = uri

    def _ok(self):
        return not self.done
    
    ###############################################################################
    # EXTERNAL API

    @apivalidate(0, (None, ))
    def shutdown(self, caller_id, msg=''):
        """
        Stop this server
        @param caller_id: ROS caller id
        @type  caller_id: str
        @param msg: a message describing why the node is being shutdown.
        @type  msg: str
        @return: [code, msg, 0]
        @rtype: [int, str, int]
        """
        if msg:
            print("shutdown request: %s" % msg, file=sys.stdout)
        else:
            print("shutdown requst", file=sys.stdout)
        self._shutdown('external shutdown request from [%s]: %s'%(caller_id, msg))
        return 1, "shutdown", 0
        
    @apivalidate('')
    def getUri(self, caller_id):
        """
        Get the XML-RPC URI of this server.
        @param caller_id str: ROS caller id    
        @return [int, str, str]: [1, "", xmlRpcUri]
        """
        return 1, "", self.uri

        
    @apivalidate(-1)
    def getPid(self, caller_id):
        """
        Get the PID of this server
        @param caller_id: ROS caller id
        @type  caller_id: str
        @return: [1, "", serverProcessPID]
        @rtype: [int, str, int]
        """
        return 1, "", os.getpid()

    
    ################################################################
    # PARAMETER SERVER ROUTINES
    
    @apivalidate(0, (non_empty_str('key'),))
    def deleteParam(self, caller_id, key):
        """
        Parameter Server: delete parameter
        @param caller_id: ROS caller id
        @type  caller_id: str
        @param key: parameter name
        @type  key: str
        @return: [code, msg, 0]
        @rtype: [int, str, int]
        """
        try:
            key = resolve_name(key, caller_id)
            self.param_server.delete_param(key, self._notify_param_subscribers)
            mloginfo("-PARAM [%s] by %s",key, caller_id)            
            return  1, "parameter %s deleted"%key, 0                
        except KeyError as e:
            return -1, "parameter [%s] is not set"%key, 0
        
    @apivalidate(0, (non_empty_str('key'), not_none('value')))
    def setParam(self, caller_id, key, value):
        """
        Parameter Server: set parameter.  NOTE: if value is a
        dictionary it will be treated as a parameter tree, where key
        is the parameter namespace. For example:::
          {'x':1,'y':2,'sub':{'z':3}}

        will set key/x=1, key/y=2, and key/sub/z=3. Furthermore, it
        will replace all existing parameters in the key parameter
        namespace with the parameters in value. You must set
        parameters individually if you wish to perform a union update.
        
        @param caller_id: ROS caller id
        @type  caller_id: str
        @param key: parameter name
        @type  key: str
        @param value: parameter value.
        @type  value: XMLRPCLegalValue
        @return: [code, msg, 0]
        @rtype: [int, str, int]
        """
        key = resolve_name(key, caller_id)
        self.param_server.set_param(key, value, self._notify_param_subscribers)
        mloginfo("+PARAM [%s] by %s",key, caller_id)
        return 1, "parameter %s set"%key, 0

    @apivalidate(0, (non_empty_str('key'),))
    def getParam(self, caller_id, key):
        """
        Retrieve parameter value from server.
        @param caller_id: ROS caller id
        @type  caller_id: str
        @param key: parameter to lookup. If key is a namespace,
        getParam() will return a parameter tree.
        @type  key: str
        getParam() will return a parameter tree.

        @return: [code, statusMessage, parameterValue]. If code is not
            1, parameterValue should be ignored. If key is a namespace,
            the return value will be a dictionary, where each key is a
            parameter in that namespace. Sub-namespaces are also
            represented as dictionaries.
        @rtype: [int, str, XMLRPCLegalValue]
        """
        try:
            key = resolve_name(key, caller_id)
            return 1, "Parameter [%s]"%key, self.param_server.get_param(key)
        except KeyError as e: 
            return -1, "Parameter [%s] is not set"%key, 0

    @apivalidate(0, (non_empty_str('key'),))
    def searchParam(self, caller_id, key):
        """
        Search for parameter key on parameter server. Search starts in caller's namespace and proceeds
        upwards through parent namespaces until Parameter Server finds a matching key.

        searchParam's behavior is to search for the first partial match.
        For example, imagine that there are two 'robot_description' parameters::
          
           /robot_description
             /robot_description/arm
             /robot_description/base
           /pr2/robot_description
             /pr2/robot_description/base

        If I start in the namespace /pr2/foo and search for
        'robot_description', searchParam will match
        /pr2/robot_description. If I search for 'robot_description/arm'
        it will return /pr2/robot_description/arm, even though that
        parameter does not exist (yet).

        @param caller_id str: ROS caller id
        @type  caller_id: str
        @param key: parameter key to search for.
        @type  key: str
        @return: [code, statusMessage, foundKey]. If code is not 1, foundKey should be
            ignored. 
        @rtype: [int, str, str]
        """
        search_key = self.param_server.search_param(caller_id, key)
        if search_key:
            return 1, "Found [%s]"%search_key, search_key
        else:
            return -1, "Cannot find parameter [%s] in an upwards search"%key, ''

    @apivalidate(0, (is_api('caller_api'), non_empty_str('key'),))
    def subscribeParam(self, caller_id, caller_api, key):
        """
        Retrieve parameter value from server and subscribe to updates to that param. See
        paramUpdate() in the Node API. 
        @param caller_id str: ROS caller id
        @type  caller_id: str
        @param key: parameter to lookup.
        @type  key: str
        @param caller_api: API URI for paramUpdate callbacks.
        @type  caller_api: str
        @return: [code, statusMessage, parameterValue]. If code is not
           1, parameterValue should be ignored. parameterValue is an empty dictionary if the parameter
           has not been set yet.
        @rtype: [int, str, XMLRPCLegalValue]
        """
        key = resolve_name(key, caller_id)        
        try:
            # ps_lock has precedence and is required due to
            # potential self.reg_manager modification
            self.ps_lock.acquire()
            val = self.param_server.subscribe_param(key, (caller_id, caller_api))
        finally:
            self.ps_lock.release()
        return 1, "Subscribed to parameter [%s]"%key, val

    @apivalidate(0, (is_api('caller_api'), non_empty_str('key'),))
    def unsubscribeParam(self, caller_id, caller_api, key):
        """
        Retrieve parameter value from server and subscribe to updates to that param. See
        paramUpdate() in the Node API. 
        @param caller_id str: ROS caller id
        @type  caller_id: str
        @param key: parameter to lookup.
        @type  key: str
        @param caller_api: API URI for paramUpdate callbacks.
        @type  caller_api: str
        @return: [code, statusMessage, numUnsubscribed]. 
           If numUnsubscribed is zero it means that the caller was not subscribed to the parameter.
        @rtype: [int, str, int]
        """        
        key = resolve_name(key, caller_id)        
        try:
            # ps_lock is required due to potential self.reg_manager modification
            self.ps_lock.acquire()
            retval = self.param_server.unsubscribe_param(key, (caller_id, caller_api))
        finally:
            self.ps_lock.release()
        return 1, "Unsubscribe to parameter [%s]"%key, 1


    @apivalidate(False, (non_empty_str('key'),))
    def hasParam(self, caller_id, key):
        """
        Check if parameter is stored on server. 
        @param caller_id str: ROS caller id
        @type  caller_id: str
        @param key: parameter to check
        @type  key: str
        @return: [code, statusMessage, hasParam]
        @rtype: [int, str, bool]
        """
        key = resolve_name(key, caller_id)
        if self.param_server.has_param(key):
            return 1, key, True
        else:
            return 1, key, False            

    @apivalidate([])
    def getParamNames(self, caller_id):
        """
        Get list of all parameter names stored on this server.
        This does not adjust parameter names for caller's scope.
        
        @param caller_id: ROS caller id    
        @type  caller_id: str
        @return: [code, statusMessage, parameterNameList]
        @rtype: [int, str, [str]]
        """
        return 1, "Parameter names", self.param_server.get_param_names()
            
    ##################################################################################
    # NOTIFICATION ROUTINES

    def _notify(self, registrations, task, key, value, node_apis):
        """
        Generic implementation of callback notification
        @param registrations: Registrations
        @type  registrations: L{Registrations}
        @param task: task to queue
        @type  task: fn
        @param key: registration key
        @type  key: str
        @param value: value to pass to task
        @type  value: Any
        """
        # cache thread_pool for thread safety
        thread_pool = self.thread_pool
        if not thread_pool:
            return
        
        try:            
            for node_api in node_apis:
                # use the api as a marker so that we limit one thread per subscriber
                thread_pool.queue_task(node_api, task, (node_api, key, value))
        except KeyError:
            _logger.warn('subscriber data stale (key [%s], listener [%s]): node API unknown'%(key, s))
        
    def _notify_param_subscribers(self, updates):
        """
        Notify parameter subscribers of new parameter value
        @param updates [([str], str, any)*]: [(subscribers, param_key, param_value)*]
        @param param_value str: parameter value
        """
        # cache thread_pool for thread safety
        thread_pool = self.thread_pool
        if not thread_pool:
            return

        for subscribers, key, value in updates:
            # use the api as a marker so that we limit one thread per subscriber
            for caller_id, caller_api in subscribers:
                self.thread_pool.queue_task(caller_api, self.param_update_task, (caller_id, caller_api, key, value))

    def param_update_task(self, caller_id, caller_api, param_key, param_value):
        """
        Contact api.paramUpdate with specified parameters
        @param caller_id: caller ID
        @type  caller_id: str
        @param caller_api: XML-RPC URI of node to contact
        @type  caller_api: str
        @param param_key: parameter key to pass to node
        @type  param_key: str
        @param param_value: parameter value to pass to node
        @type  param_value: str
        """
        mloginfo("paramUpdate[%s]", param_key)
        code, _, _ = xmlrpcapi(caller_api).paramUpdate('/master', param_key, param_value)
        if code == -1:
            try:
                # ps_lock is required due to potential self.reg_manager modification
                self.ps_lock.acquire()
                # reverse lookup to figure out who we just called
                matches = self.reg_manager.reverse_lookup(caller_api)
                for m in matches:
                    retval = self.param_server.unsubscribe_param(param_key, (m.id, caller_api))
            finally:
                self.ps_lock.release()

    def _notify_topic_subscribers(self, topic, pub_uris, sub_uris):
        """
        Notify subscribers with new publisher list
        @param topic: name of topic
        @type  topic: str
        @param pub_uris: list of URIs of publishers.
        @type  pub_uris: [str]
        """
        self._notify(self.subscribers, publisher_update_task, topic, pub_uris, sub_uris)

    ##################################################################################
    # SERVICE PROVIDER

    @apivalidate(0, ( is_service('service'), is_api('service_api'), is_api('caller_api')))
    def registerService(self, caller_id, service, service_api, caller_api):
        """
        Register the caller as a provider of the specified service.
        @param caller_id str: ROS caller id
        @type  caller_id: str
        @param service: Fully-qualified name of service 
        @type  service: str
        @param service_api: Service URI 
        @type  service_api: str
        @param caller_api: XML-RPC URI of caller node 
        @type  caller_api: str
        @return: (code, message, ignore)
        @rtype: (int, str, int)
        """        
        try:
            self.ps_lock.acquire()
            self.reg_manager.register_service(service, caller_id, caller_api, service_api)
            mloginfo("+SERVICE [%s] %s %s", service, caller_id, caller_api)
        finally:
            self.ps_lock.release()
        return 1, "Registered [%s] as provider of [%s]"%(caller_id, service), 1

    @apivalidate(0, (is_service('service'),))
    def lookupService(self, caller_id, service):
        """
        Lookup all provider of a particular service.
        @param caller_id str: ROS caller id
        @type  caller_id: str
        @param service: fully-qualified name of service to lookup.
        @type: service: str
        @return: (code, message, serviceUrl). service URL is provider's
           ROSRPC URI with address and port.  Fails if there is no provider.
        @rtype: (int, str, str)
        """
        try:
            self.ps_lock.acquire()
            service_url = self.services.get_service_api(service)
        finally:
            self.ps_lock.release()
        if service_url:
            return 1, "rosrpc URI: [%s]"%service_url, service_url
        else:
            return -1, "no provider", ''

    @apivalidate(0, ( is_service('service'), is_api('service_api')))
    def unregisterService(self, caller_id, service, service_api):
        """
        Unregister the caller as a provider of the specified service.
        @param caller_id str: ROS caller id
        @type  caller_id: str
        @param service: Fully-qualified name of service
        @type  service: str
        @param service_api: API URI of service to unregister. Unregistration will only occur if current
           registration matches.
        @type  service_api: str
        @return: (code, message, numUnregistered). Number of unregistrations (either 0 or 1).
           If this is zero it means that the caller was not registered as a service provider.
           The call still succeeds as the intended final state is reached.
        @rtype: (int, str, int)
        """
        try:
            self.ps_lock.acquire()
            retval = self.reg_manager.unregister_service(service, caller_id, service_api)
            mloginfo("-SERVICE [%s] %s %s", service, caller_id, service_api)
            return retval
        finally:
            self.ps_lock.release()

    ##################################################################################
    # PUBLISH/SUBSCRIBE

    @apivalidate(0, ( is_topic('topic'), valid_type_name('topic_type'), is_api('caller_api')))
    def registerSubscriber(self, caller_id, topic, topic_type, caller_api):
        """
        Subscribe the caller to the specified topic. In addition to receiving
        a list of current publishers, the subscriber will also receive notifications
        of new publishers via the publisherUpdate API.        
        @param caller_id: ROS caller id
        @type  caller_id: str
        @param topic str: Fully-qualified name of topic to subscribe to. 
        @param topic_type: Datatype for topic. Must be a package-resource name, i.e. the .msg name.
        @type  topic_type: str
        @param caller_api: XML-RPC URI of caller node for new publisher notifications
        @type  caller_api: str
        @return: (code, message, publishers). Publishers is a list of XMLRPC API URIs
           for nodes currently publishing the specified topic.
        @rtype: (int, str, [str])
        """
        #NOTE: subscribers do not get to set topic type
        try:
            self.ps_lock.acquire()
            self.reg_manager.register_subscriber(topic, caller_id, caller_api)

            # ROS 1.1: subscriber can now set type if it is not already set
            #  - don't let '*' type squash valid typing
            if not topic in self.topics_types and topic_type != rosgraph.names.ANYTYPE:
                self.topics_types[topic] = topic_type

            mloginfo("+SUB [%s] %s %s",topic, caller_id, caller_api)
            pub_uris = self.publishers.get_apis(topic)
        finally:
            self.ps_lock.release()
        return 1, "Subscribed to [%s]"%topic, pub_uris

    @apivalidate(0, (is_topic('topic'), is_api('caller_api')))
    def unregisterSubscriber(self, caller_id, topic, caller_api):
        """
        Unregister the caller as a publisher of the topic.
        @param caller_id: ROS caller id
        @type  caller_id: str
        @param topic: Fully-qualified name of topic to unregister.
        @type  topic: str
        @param caller_api: API URI of service to unregister. Unregistration will only occur if current
           registration matches.    
        @type  caller_api: str
        @return: (code, statusMessage, numUnsubscribed). 
          If numUnsubscribed is zero it means that the caller was not registered as a subscriber.
          The call still succeeds as the intended final state is reached.
        @rtype: (int, str, int)
        """
        try:
            self.ps_lock.acquire()
            retval = self.reg_manager.unregister_subscriber(topic, caller_id, caller_api)
            mloginfo("-SUB [%s] %s %s",topic, caller_id, caller_api)
            return retval
        finally:
            self.ps_lock.release()

    @apivalidate(0, ( is_topic('topic'), valid_type_name('topic_type'), is_api('caller_api')))
    def registerPublisher(self, caller_id, topic, topic_type, caller_api):
        """
        Register the caller as a publisher the topic.
        @param caller_id: ROS caller id
        @type  caller_id: str
        @param topic: Fully-qualified name of topic to register.
        @type  topic: str
        @param topic_type: Datatype for topic. Must be a
        package-resource name, i.e. the .msg name.
        @type  topic_type: str
        @param caller_api str: ROS caller XML-RPC API URI
        @type  caller_api: str
        @return: (code, statusMessage, subscriberApis).
        List of current subscribers of topic in the form of XMLRPC URIs.
        @rtype: (int, str, [str])
        """
        #NOTE: we need topic_type for getPublishedTopics.
        try:
            self.ps_lock.acquire()
            self.reg_manager.register_publisher(topic, caller_id, caller_api)
            # don't let '*' type squash valid typing
            if topic_type != rosgraph.names.ANYTYPE or not topic in self.topics_types:
                self.topics_types[topic] = topic_type
            pub_uris = self.publishers.get_apis(topic)
            sub_uris = self.subscribers.get_apis(topic)
            self._notify_topic_subscribers(topic, pub_uris, sub_uris)
            mloginfo("+PUB [%s] %s %s",topic, caller_id, caller_api)
            sub_uris = self.subscribers.get_apis(topic)            
        finally:
            self.ps_lock.release()
        return 1, "Registered [%s] as publisher of [%s]"%(caller_id, topic), sub_uris


    @apivalidate(0, (is_topic('topic'), is_api('caller_api')))
    def unregisterPublisher(self, caller_id, topic, caller_api):
        """
        Unregister the caller as a publisher of the topic.
        @param caller_id: ROS caller id
        @type  caller_id: str
        @param topic: Fully-qualified name of topic to unregister.
        @type  topic: str
        @param caller_api str: API URI of service to
           unregister. Unregistration will only occur if current
           registration matches.
        @type  caller_api: str
        @return: (code, statusMessage, numUnregistered). 
           If numUnregistered is zero it means that the caller was not registered as a publisher.
           The call still succeeds as the intended final state is reached.
        @rtype: (int, str, int)
        """            
        try:
            self.ps_lock.acquire()
            retval = self.reg_manager.unregister_publisher(topic, caller_id, caller_api)
            if retval[VAL]:
                self._notify_topic_subscribers(topic, self.publishers.get_apis(topic), self.subscribers.get_apis(topic))
            mloginfo("-PUB [%s] %s %s",topic, caller_id, caller_api)
        finally:
            self.ps_lock.release()
        return retval

    ##################################################################################
    # GRAPH STATE APIS

    @apivalidate('', (valid_name('node'),))
    def lookupNode(self, caller_id, node_name):
        """
        Get the XML-RPC URI of the node with the associated
        name/caller_id.  This API is for looking information about
        publishers and subscribers. Use lookupService instead to lookup
        ROS-RPC URIs.
        @param caller_id: ROS caller id
        @type  caller_id: str
        @param node: name of node to lookup
        @type  node: str
        @return: (code, msg, URI)
        @rtype: (int, str, str)
        """
        try:
            self.ps_lock.acquire()
            node = self.reg_manager.get_node(node_name)
            if node is not None:
                retval = 1, "node api", node.api
            else:
                retval = -1, "unknown node [%s]"%node_name, ''
        finally:
            self.ps_lock.release()
        return retval
        
    @apivalidate(0, (empty_or_valid_name('subgraph'),))
    def getPublishedTopics(self, caller_id, subgraph):
        """
        Get list of topics that can be subscribed to. This does not return topics that have no publishers.
        See L{getSystemState()} to get more comprehensive list.
        @param caller_id: ROS caller id
        @type  caller_id: str
        @param subgraph: Restrict topic names to match within the specified subgraph. Subgraph namespace
           is resolved relative to the caller's namespace. Use '' to specify all names.
        @type  subgraph: str
        @return: (code, msg, [[topic1, type1]...[topicN, typeN]])
        @rtype: (int, str, [[str, str],])
        """
        try:
            self.ps_lock.acquire()
            # force subgraph to be a namespace with trailing slash
            if subgraph and subgraph[-1] != rosgraph.names.SEP:
                subgraph = subgraph + rosgraph.names.SEP
            #we don't bother with subscribers as subscribers don't report topic types. also, the intended
            #use case is for subscribe-by-topic-type
            retval = [[t, self.topics_types[t]] for t in self.publishers.iterkeys() if t.startswith(subgraph)]
        finally:
            self.ps_lock.release()
        return 1, "current topics", retval
    
    @apivalidate([])
    def getTopicTypes(self, caller_id): 
        """
        Retrieve list topic names and their types.
        @param caller_id: ROS caller id    
        @type  caller_id: str
        @rtype: (int, str, [[str,str]] )
        @return: (code, statusMessage, topicTypes). topicTypes is a list of [topicName, topicType] pairs.
        """
        try: 
            self.ps_lock.acquire()
            retval = list(self.topics_types.items())
        finally:
            self.ps_lock.release()
        return 1, "current system state", retval

    @apivalidate([[],[], []])
    def getSystemState(self, caller_id): 
        """
        Retrieve list representation of system state (i.e. publishers, subscribers, and services).
        @param caller_id: ROS caller id    
        @type  caller_id: str
        @rtype: (int, str, [[str,[str]], [str,[str]], [str,[str]]])
        @return: (code, statusMessage, systemState).

           System state is in list representation::
             [publishers, subscribers, services].
        
           publishers is of the form::
             [ [topic1, [topic1Publisher1...topic1PublisherN]] ... ]
        
           subscribers is of the form::
             [ [topic1, [topic1Subscriber1...topic1SubscriberN]] ... ]
        
           services is of the form::
             [ [service1, [service1Provider1...service1ProviderN]] ... ]
        """
        edges = []
        try: 
            self.ps_lock.acquire()
            retval = [r.get_state() for r in (self.publishers, self.subscribers, self.services)]
        finally:
            self.ps_lock.release()
        return 1, "current system state", retval
Ejemplo n.º 5
0
class GoblinShadowHandler(object):
    """
    Goblin Shadow handler is a client-side local proxy of the original ROS Master.
    This additional intermediary provides some key features with slight overhead.
    """

    def __init__(self, master_uri, num_works, timeout=None):
        # Running status
        self.shadow_uri = None
        self.master_uri = master_uri
        self._caller_id = '/Goblin/Shadow/unbind'
        self._running = True

        # Inner fields
        socket.setdefaulttimeout(timeout)
        self.thread_pool = MarkedThreadPool(num_works)
        self.ps_lock = threading.Condition(threading.Lock())
        self.reg_manager = RegistrationManager(self.thread_pool)
        self.master_proxy = xmlrpclib.ServerProxy(master_uri)
        # TODO: support all local nodes
        self.services = self.reg_manager.services
        # TODO: support local param-server caches

    # APIs for running XML-RPC
    def _shutdown(self, reason=''):
        """
        Forked from ROSMasterHandler
        @param reason:
        @return:
        """
        if self.thread_pool is not None:
            self.thread_pool.join_all(wait_for_tasks=False, wait_for_threads=False)
            self.thread_pool = None
        self._running = False

    def _ready(self, uri):
        """
        Impl standard XML-RPC API to update URI
        @param uri:
        @return:
        """
        self.shadow_uri = uri
        self._caller_id = '/Goblin/Shadow/unbind_{}'.format(uri)

    def _dispatch(self, method, params):
        """
        Dispatch not-covered method to original ROS Master
        """
        logger.info('Required: {}{}'.format(method, params))
        if method in METHODS:
            logger.debug('--  LOCAL')
            status, msg, value = METHODS[method](self, *params)
            logger.debug('>>  LOCAL {}'.format((status, msg, value)))
        else:
            logger.debug('-- REMOTE: {!r}'.format(getattr(self.master_proxy, method)))
            status, msg, value = getattr(self.master_proxy, method)(*params)
            logger.debug('>> REMOTE: {}'.format((status, msg, value)))
        return status, msg, value

    def is_running(self):
        return self._running

    # private methods for running Shadow
    def _lookup_local_service(self, service):
        uri = None
        with self.ps_lock:
            uri = self.services.get_service_api(service)
        return uri

    def _lookup_remote_service(self, service):
        return self.master_proxy.lookupService(self._caller_id, service)[2]

    def _reg_local_service(self, caller_id, service, service_api, caller_api):
        logger.info('-- Reg Local {} {}'.format(service, service_api))
        with self.ps_lock:
            self.reg_manager.register_service(service, caller_id, caller_api, service_api)
            logger.info("+SERVICE [%s] %s %s", service, caller_id, caller_api)

    def _reg_remote_service(self, *args):
        r = self.master_proxy.registerService(*args)
        logger.info('-- Reg Remote {}'.format(r))

    def _unreg_local_service(self, caller_id, service, service_api):
        with self.ps_lock:
            self.reg_manager.unregister_service(service, caller_id, service_api)

    def _unreg_remote_service(self, *args):
        r = self.master_proxy.unregisterService(*args)
        logger.info('-- Unreg Remote {}'.format(r))

    # Overwritten APIs
    @apivalidate(0, (is_service('service'),))
    @overwrite
    def lookupService(self, caller_id, service):
        """
        Lookup all provider of a particular service in following rules:
        1. Local/Auto/Unlabeled
            1. Use local services if possible.
            2. Otherwise, use remote services.
        2. Remote
            1. If average time cost belows the balance-curve, use remote services.
            2. Otherwise, use remote services if possible.
        @param caller_id str: ROS caller id
        @type  caller_id: str
        @param service: fully-qualified name of service to lookup.
        @type: service: str
        @return: (code, message, serviceUrl). service URL is provider's
        ROSRPC URI with address and port.  Fails if there is no provider.
        @rtype: (int, str, str)
        """
        uri = None
        cfg = swcfg.services.get_config(service)

        def skeleton(x, y):
            fallback_p = const(bool(cfg.fallback))
            # TODO: support fallback list
            return on_fallback(x, y, fallback_p, success_uri)

        # local?
        if swcfg.localtype == cfg.priority:
            fn = skeleton(self._lookup_local_service, self._lookup_remote_service)
        else:
            fn = skeleton(self._lookup_remote_service, self._lookup_local_service)
        uri = fn(service)
        if success_uri(uri):
            return ResponseFactory.uri_found(service, uri).pack()
        else:
            return ResponseFactory.unknown_service(service).pack()

    @apivalidate(0, (is_service('service'), is_api('service_api'), is_api('caller_api')))
    @overwrite
    def registerService(self, caller_id, service, service_api, caller_api):
        """
        Forked from ROSMasterHandler.
        Register the caller as a provider of the specified service.
        0. If service is `remote-only`, register with ROS Master
        1. If Service is `local-only`, register with current shadow instance.
        2. Otherwise, register with both sides.
        @param caller_id str: ROS caller id
        @type  caller_id: str
        @param service: Fully-qualified name of service
        @type  service: str
        @param service_api: Service URI
        @type  service_api: str
        @param caller_api: XML-RPC URI of caller node
        @type  caller_api: str
        @return: (code, message, ignore)
        @rtype: (int, str, int)
        """
        cfg = swcfg.services.get_config(service)
        if not cfg.is_local_only():
            self._reg_remote_service(caller_id, service, service_api, caller_api)
        if not cfg.is_remote_only():
            self._reg_local_service(caller_id, service, service_api, caller_api)
        return ResponseFactory.service_reg(caller_id, service).pack()

    @apivalidate(0, (is_service('service'), is_api('service_api')))
    @overwrite
    def unregisterService(self, caller_id, service, service_api):
        """
        Forked from ROSMasterHandler.
        Unregister the caller as a provider of the specified service.
        @param caller_id str: ROS caller id
        @type  caller_id: str
        @param service: Fully-qualified name of service
        @type  service: str
        @param service_api: API URI of service to unregister. Unregistration will only occur if current
           registration matches.
        @type  service_api: str
        @return: (code, message, numUnregistered). Number of unregistrations (either 0 or 1).
           If this is zero it means that the caller was not registered as a service provider.
           The call still succeeds as the intended final state is reached.
        @rtype: (int, str, int)
        """
        cfg = swcfg.services.get_config(service)
        if not cfg.is_local_only():
            self._unreg_remote_service(caller_id, service, service_api)
        if not cfg.is_remote_only():
            self._unreg_local_service(caller_id, service, service_api)
        return ResponseFactory.service_unreg(caller_id, service).pack()
    def test_RegistrationManager_base(self):
        import rosmaster.exceptions
        from rosmaster.registrations import Registrations, RegistrationManager
        threadpool = ThreadPoolMock()

        rm = RegistrationManager(threadpool)
        self.assert_(isinstance(rm.services, Registrations))
        self.assertEquals(Registrations.SERVICE, rm.services.type)
        self.assert_(isinstance(rm.param_subscribers, Registrations))
        self.assertEquals(Registrations.PARAM_SUBSCRIPTIONS,
                          rm.param_subscribers.type)
        self.assert_(isinstance(rm.subscribers, Registrations))
        self.assertEquals(Registrations.TOPIC_SUBSCRIPTIONS,
                          rm.subscribers.type)
        self.assert_(isinstance(rm.subscribers, Registrations))
        self.assertEquals(Registrations.TOPIC_PUBLICATIONS, rm.publishers.type)
        self.assert_(isinstance(rm.publishers, Registrations))

        #test auto-clearing of registrations if node API changes
        rm.register_publisher('pub1', 'caller1', 'http://one:1')
        rm.register_publisher('pub1', 'caller2', 'http://two:1')
        rm.register_publisher('pub1', 'caller3', 'http://three:1')
        rm.register_subscriber('sub1', 'caller1', 'http://one:1')
        rm.register_subscriber('sub1', 'caller2', 'http://two:1')
        rm.register_subscriber('sub1', 'caller3', 'http://three:1')
        rm.register_param_subscriber('p1', 'caller1', 'http://one:1')
        rm.register_param_subscriber('p1', 'caller2', 'http://two:1')
        rm.register_param_subscriber('p1', 'caller3', 'http://three:1')
        rm.register_service('s1', 'caller1', 'http://one:1', 'rosrpc://one:1')
        self.assertEquals('http://one:1', rm.get_node('caller1').api)
        self.assertEquals('http://two:1', rm.get_node('caller2').api)
        self.assertEquals('http://three:1', rm.get_node('caller3').api)

        # - first, make sure that changing rosrpc URI does not erase state
        rm.register_service('s1', 'caller1', 'http://one:1', 'rosrpc://oneB:1')
        n = rm.get_node('caller1')
        self.assertEquals(['pub1'], n.topic_publications)
        self.assertEquals(['sub1'], n.topic_subscriptions)
        self.assertEquals(['p1'], n.param_subscriptions)
        self.assertEquals(['s1'], n.services)
        self.assert_('http://one:1' in rm.publishers.get_apis('pub1'))
        self.assert_('http://one:1' in rm.subscribers.get_apis('sub1'))
        self.assert_('http://one:1' in rm.param_subscribers.get_apis('p1'))
        self.assert_('http://one:1' in rm.services.get_apis('s1'))

        # - also, make sure unregister does not erase state if API changed
        rm.unregister_publisher('pub1', 'caller1', 'http://not:1')
        self.assert_('http://one:1' in rm.publishers.get_apis('pub1'))
        rm.unregister_subscriber('sub1', 'caller1', 'http://not:1')
        self.assert_('http://one:1' in rm.subscribers.get_apis('sub1'))
        rm.unregister_param_subscriber('p1', 'caller1', 'http://not:1')
        self.assert_('http://one:1' in rm.param_subscribers.get_apis('p1'))
        rm.unregister_service('sub1', 'caller1', 'rosrpc://not:1')
        self.assert_('http://one:1' in rm.services.get_apis('s1'))

        # erase caller1 sub/srvs/params via register_publisher
        rm.register_publisher('pub1', 'caller1', 'http://newone:1')
        self.assertEquals('http://newone:1', rm.get_node('caller1').api)
        # - check node ref
        n = rm.get_node('caller1')
        self.assertEquals(['pub1'], n.topic_publications)
        self.assertEquals([], n.services)
        self.assertEquals([], n.topic_subscriptions)
        self.assertEquals([], n.param_subscriptions)
        # - checks publishers
        self.assert_('http://newone:1' in rm.publishers.get_apis('pub1'))
        # - checks subscribers
        self.assert_(rm.subscribers.has_key('sub1'))
        self.failIf('http://one:1' in rm.subscribers.get_apis('sub1'))
        # - checks param subscribers
        self.assert_(rm.param_subscribers.has_key('p1'))
        self.failIf('http://one:1' in rm.param_subscribers.get_apis('p1'))

        # erase caller2 pub/sub/params via register_service
        # - initial state
        self.assert_('http://two:1' in rm.publishers.get_apis('pub1'))
        self.assert_('http://two:1' in rm.subscribers.get_apis('sub1'))
        self.assert_('http://two:1' in rm.param_subscribers.get_apis('p1'))
        # - change ownership of s1 to caller2
        rm.register_service('s1', 'caller2', 'http://two:1', 'rosrpc://two:1')
        self.assert_('http://two:1' in rm.services.get_apis('s1'))
        self.assert_('http://two:1' in rm.publishers.get_apis('pub1'))
        self.assert_('http://two:1' in rm.subscribers.get_apis('sub1'))
        self.assert_('http://two:1' in rm.param_subscribers.get_apis('p1'))

        rm.register_service('s1', 'caller2', 'http://newtwo:1',
                            'rosrpc://newtwo:1')
        self.assertEquals('http://newone:1', rm.get_node('caller1').api)
        # - check node ref
        n = rm.get_node('caller2')
        self.assertEquals([], n.topic_publications)
        self.assertEquals(['s1'], n.services)
        self.assertEquals([], n.topic_subscriptions)
        self.assertEquals([], n.param_subscriptions)
        # - checks publishers
        self.assert_(rm.publishers.has_key('pub1'))
        self.failIf('http://two:1' in rm.publishers.get_apis('pub1'))
        # - checks subscribers
        self.assert_(rm.subscribers.has_key('sub1'))
        self.failIf('http://two:1' in rm.subscribers.get_apis('sub1'))
        self.assertEquals([['sub1', ['caller3']]], rm.subscribers.get_state())
        # - checks param subscribers
        self.assert_(rm.param_subscribers.has_key('p1'))
        self.failIf('http://two:1' in rm.param_subscribers.get_apis('p1'))
        self.assertEquals([['p1', ['caller3']]],
                          rm.param_subscribers.get_state())
    def test_RegistrationManager_services(self):
        from rosmaster.registrations import Registrations, RegistrationManager
        rm = RegistrationManager(ThreadPoolMock())

        self.assertEquals(None, rm.get_node('caller1'))

        # do an unregister first, before service_api is initialized
        code, msg, val = rm.unregister_service('s1', 'caller1',
                                               'rosrpc://one:1234')
        self.assertEquals(1, code)
        self.assertEquals(0, val)

        rm.register_service('s1', 'caller1', 'http://one:1234',
                            'rosrpc://one:1234')
        self.assert_(rm.services.has_key('s1'))
        self.assertEquals('rosrpc://one:1234',
                          rm.services.get_service_api('s1'))
        self.assertEquals('http://one:1234', rm.get_node('caller1').api)
        self.assertEquals([['s1', ['caller1']]], rm.services.get_state())

        # - verify that changed caller_api updates ref
        rm.register_service('s1', 'caller1', 'http://oneB:1234',
                            'rosrpc://one:1234')
        self.assert_(rm.services.has_key('s1'))
        self.assertEquals('rosrpc://one:1234',
                          rm.services.get_service_api('s1'))
        self.assertEquals('http://oneB:1234', rm.get_node('caller1').api)
        self.assertEquals([['s1', ['caller1']]], rm.services.get_state())

        # - verify that changed service_api updates ref
        rm.register_service('s1', 'caller1', 'http://oneB:1234',
                            'rosrpc://oneB:1234')
        self.assert_(rm.services.has_key('s1'))
        self.assertEquals('rosrpc://oneB:1234',
                          rm.services.get_service_api('s1'))
        self.assertEquals('http://oneB:1234', rm.get_node('caller1').api)
        self.assertEquals([['s1', ['caller1']]], rm.services.get_state())

        rm.register_service('s2', 'caller2', 'http://two:1234',
                            'rosrpc://two:1234')
        self.assertEquals('http://two:1234', rm.get_node('caller2').api)

        # - unregister should be noop if service api does not match
        code, msg, val = rm.unregister_service('s2', 'caller2',
                                               'rosrpc://b:1234')
        self.assertEquals(1, code)
        self.assertEquals(0, val)
        self.assert_(rm.services.has_key('s2'))
        self.assertEquals('http://two:1234', rm.get_node('caller2').api)
        self.assertEquals('rosrpc://two:1234',
                          rm.services.get_service_api('s2'))

        # - unregister should be noop if service is unknown
        code, msg, val = rm.unregister_service('unknown', 'caller2',
                                               'rosrpc://two:1234')
        self.assertEquals(1, code)
        self.assertEquals(0, val)
        self.assert_(rm.services.has_key('s2'))
        self.assertEquals('http://two:1234', rm.get_node('caller2').api)
        self.assertEquals('rosrpc://two:1234',
                          rm.services.get_service_api('s2'))

        # - unregister should clear all knowledge of caller2
        code, msg, val = rm.unregister_service('s2', 'caller2',
                                               'rosrpc://two:1234')
        self.assertEquals(1, code)
        self.assertEquals(1, val)
        self.assert_(rm.services.has_key('s1'))
        self.failIf(rm.services.has_key('s2'))
        self.assertEquals(None, rm.get_node('caller2'))

        code, msg, val = rm.unregister_service('s1', 'caller1',
                                               'rosrpc://oneB:1234')
        self.assertEquals(1, code)
        self.assertEquals(1, val)
        self.assert_(not rm.services.__nonzero__())
        self.failIf(rm.services.has_key('s1'))
        self.assertEquals(None, rm.get_node('caller1'))
Ejemplo n.º 8
0
class GoblinShadowHandler(object):
    """
    Goblin Shadow handler is a client-side local proxy of the original ROS Master.
    This additional intermediary provides some key features with slight overhead.
    """
    def __init__(self, master_uri, num_works, timeout=None):
        # Running status
        self.shadow_uri = None
        self.master_uri = master_uri
        self._caller_id = '/Goblin/Shadow/unbind'
        self._running = True

        # Inner fields
        socket.setdefaulttimeout(timeout)
        self.thread_pool = MarkedThreadPool(num_works)
        self.ps_lock = threading.Condition(threading.Lock())
        self.reg_manager = RegistrationManager(self.thread_pool)
        self.master_proxy = xmlrpclib.ServerProxy(master_uri)
        # TODO: support all local nodes
        self.services = self.reg_manager.services
        # TODO: support local param-server caches

    # APIs for running XML-RPC
    def _shutdown(self, reason=''):
        """
        Forked from ROSMasterHandler
        @param reason:
        @return:
        """
        if self.thread_pool is not None:
            self.thread_pool.join_all(wait_for_tasks=False,
                                      wait_for_threads=False)
            self.thread_pool = None
        self._running = False

    def _ready(self, uri):
        """
        Impl standard XML-RPC API to update URI
        @param uri:
        @return:
        """
        self.shadow_uri = uri
        self._caller_id = '/Goblin/Shadow/unbind_{}'.format(uri)

    def _dispatch(self, method, params):
        """
        Dispatch not-covered method to original ROS Master
        """
        logger.info('Required: {}{}'.format(method, params))
        if method in METHODS:
            logger.debug('--  LOCAL')
            status, msg, value = METHODS[method](self, *params)
            logger.debug('>>  LOCAL {}'.format((status, msg, value)))
        else:
            logger.debug('-- REMOTE: {!r}'.format(
                getattr(self.master_proxy, method)))
            status, msg, value = getattr(self.master_proxy, method)(*params)
            logger.debug('>> REMOTE: {}'.format((status, msg, value)))
        return status, msg, value

    def is_running(self):
        return self._running

    # private methods for running Shadow
    def _lookup_local_service(self, service):
        uri = None
        with self.ps_lock:
            uri = self.services.get_service_api(service)
        return uri

    def _lookup_remote_service(self, service):
        return self.master_proxy.lookupService(self._caller_id, service)[2]

    def _reg_local_service(self, caller_id, service, service_api, caller_api):
        logger.info('-- Reg Local {} {}'.format(service, service_api))
        with self.ps_lock:
            self.reg_manager.register_service(service, caller_id, caller_api,
                                              service_api)
            logger.info("+SERVICE [%s] %s %s", service, caller_id, caller_api)

    def _reg_remote_service(self, *args):
        r = self.master_proxy.registerService(*args)
        logger.info('-- Reg Remote {}'.format(r))

    def _unreg_local_service(self, caller_id, service, service_api):
        with self.ps_lock:
            self.reg_manager.unregister_service(service, caller_id,
                                                service_api)

    def _unreg_remote_service(self, *args):
        r = self.master_proxy.unregisterService(*args)
        logger.info('-- Unreg Remote {}'.format(r))

    # Overwritten APIs
    @apivalidate(0, (is_service('service'), ))
    @overwrite
    def lookupService(self, caller_id, service):
        """
        Lookup all provider of a particular service in following rules:
        1. Local/Auto/Unlabeled
            1. Use local services if possible.
            2. Otherwise, use remote services.
        2. Remote
            1. If average time cost belows the balance-curve, use remote services.
            2. Otherwise, use remote services if possible.
        @param caller_id str: ROS caller id
        @type  caller_id: str
        @param service: fully-qualified name of service to lookup.
        @type: service: str
        @return: (code, message, serviceUrl). service URL is provider's
        ROSRPC URI with address and port.  Fails if there is no provider.
        @rtype: (int, str, str)
        """
        uri = None
        cfg = swcfg.services.get_config(service)

        def skeleton(x, y):
            fallback_p = const(bool(cfg.fallback))
            # TODO: support fallback list
            return on_fallback(x, y, fallback_p, success_uri)

        # local?
        if swcfg.localtype == cfg.priority:
            fn = skeleton(self._lookup_local_service,
                          self._lookup_remote_service)
        else:
            fn = skeleton(self._lookup_remote_service,
                          self._lookup_local_service)
        uri = fn(service)
        if success_uri(uri):
            return ResponseFactory.uri_found(service, uri).pack()
        else:
            return ResponseFactory.unknown_service(service).pack()

    @apivalidate(
        0,
        (is_service('service'), is_api('service_api'), is_api('caller_api')))
    @overwrite
    def registerService(self, caller_id, service, service_api, caller_api):
        """
        Forked from ROSMasterHandler.
        Register the caller as a provider of the specified service.
        0. If service is `remote-only`, register with ROS Master
        1. If Service is `local-only`, register with current shadow instance.
        2. Otherwise, register with both sides.
        @param caller_id str: ROS caller id
        @type  caller_id: str
        @param service: Fully-qualified name of service
        @type  service: str
        @param service_api: Service URI
        @type  service_api: str
        @param caller_api: XML-RPC URI of caller node
        @type  caller_api: str
        @return: (code, message, ignore)
        @rtype: (int, str, int)
        """
        cfg = swcfg.services.get_config(service)
        if not cfg.is_local_only():
            self._reg_remote_service(caller_id, service, service_api,
                                     caller_api)
        if not cfg.is_remote_only():
            self._reg_local_service(caller_id, service, service_api,
                                    caller_api)
        return ResponseFactory.service_reg(caller_id, service).pack()

    @apivalidate(0, (is_service('service'), is_api('service_api')))
    @overwrite
    def unregisterService(self, caller_id, service, service_api):
        """
        Forked from ROSMasterHandler.
        Unregister the caller as a provider of the specified service.
        @param caller_id str: ROS caller id
        @type  caller_id: str
        @param service: Fully-qualified name of service
        @type  service: str
        @param service_api: API URI of service to unregister. Unregistration will only occur if current
           registration matches.
        @type  service_api: str
        @return: (code, message, numUnregistered). Number of unregistrations (either 0 or 1).
           If this is zero it means that the caller was not registered as a service provider.
           The call still succeeds as the intended final state is reached.
        @rtype: (int, str, int)
        """
        cfg = swcfg.services.get_config(service)
        if not cfg.is_local_only():
            self._unreg_remote_service(caller_id, service, service_api)
        if not cfg.is_remote_only():
            self._unreg_local_service(caller_id, service, service_api)
        return ResponseFactory.service_unreg(caller_id, service).pack()