def __init__(self):

        self.services = {}

        self.service_manager = ServiceManager(
            registration_listeners=RegistrationListeners())
        self.topic_manager = rospy.topics._TopicManager()

        self.subs_internal = {}

        self.external_node = None
        self.external_tcpros = None
    def __init__(self):
        
        self.services = {}
        
        self.service_manager = ServiceManager(registration_listeners=RegistrationListeners())
        self.topic_manager = rospy.topics._TopicManager()

        self.subs_internal = {}
        
        self.external_node = None
        self.external_tcpros = None
Exemple #3
0
    def test_ServiceManager(self):
        class MockService(rospy.service._Service):
            def __init__(self, name, service_class, uri):
                rospy.service._Service.__init__(self, name, service_class)
                self.uri = uri

        from rospy.service import ServiceManager
        sm = ServiceManager()
        self.assertEquals({}, sm.map)
        try:
            sm.lock.acquire()
        finally:
            sm.lock.release()
        self.assertEquals([], sm.get_services())
        sm.unregister_all()
        self.assertEquals([], sm.get_services())

        # test actual registration
        mock = MockService('/serv', MockServiceClass, "rosrpc://uri:1")
        mock2 = MockService('/serv', MockServiceClass, "rosrpc://uri:2")

        sm.register('/serv', mock)
        self.assertEquals(mock, sm.get_service('/serv'))
        self.assertEquals([('/serv', mock.uri)], sm.get_services())
        try:
            sm.register('/serv', mock2)
            self.fail("duplicate reg should fail")
        except rospy.service.ServiceException:
            pass

        sm.unregister_all()
        self.assertEquals([], sm.get_services())

        #  - register two services
        sm.register('/serv', mock)
        sm.unregister('/serv', mock2)
        self.assertEquals(mock, sm.get_service('/serv'))

        sm.register('/serv2', mock2)
        self.assertEquals(mock, sm.get_service('/serv'))
        self.assertEquals(mock2, sm.get_service('/serv2'))
        self.assert_(('/serv', mock.uri) in sm.get_services())
        self.assert_(('/serv2', mock2.uri) in sm.get_services())

        sm.unregister('/serv', mock)
        self.assertEquals([('/serv2', mock2.uri)], sm.get_services())
        sm.unregister('/serv2', mock2)
        self.assertEquals([], sm.get_services())
    def test_ServiceManager(self):
        class MockService(rospy.service._Service):
            def __init__(self, name, service_class, uri):
                rospy.service._Service.__init__(self, name, service_class)
                self.uri = uri

        from rospy.service import ServiceManager
        sm = ServiceManager()
        self.assertEquals({}, sm.map)
        try:
            sm.lock.acquire()
        finally:
            sm.lock.release()
        self.assertEquals([], sm.get_services())
        sm.unregister_all()
        self.assertEquals([], sm.get_services())

        # test actual registration
        mock = MockService('/serv', MockServiceClass, "rosrpc://uri:1")
        mock2 = MockService('/serv', MockServiceClass, "rosrpc://uri:2")        

        sm.register('/serv', mock)
        self.assertEquals(mock, sm.get_service('/serv'))
        self.assertEquals([('/serv', mock.uri)], sm.get_services())
        try:
            sm.register('/serv', mock2)
            self.fail("duplicate reg should fail")
        except rospy.service.ServiceException: pass
        
        sm.unregister_all()
        self.assertEquals([], sm.get_services())
        
        #  - register two services
        sm.register('/serv', mock)
        sm.unregister('/serv', mock2)
        self.assertEquals(mock, sm.get_service('/serv'))

        sm.register('/serv2', mock2)
        self.assertEquals(mock, sm.get_service('/serv'))
        self.assertEquals(mock2, sm.get_service('/serv2'))
        self.assert_(('/serv', mock.uri) in sm.get_services())
        self.assert_(('/serv2', mock2.uri) in sm.get_services())        
        
        sm.unregister('/serv', mock)
        self.assertEquals([('/serv2', mock2.uri)], sm.get_services())
        sm.unregister('/serv2', mock2)
        self.assertEquals([], sm.get_services())
class Proxy(object):
    
    def __init__(self):
        
        self.services = {}
        
        self.service_manager = ServiceManager(registration_listeners=RegistrationListeners())
        self.topic_manager = rospy.topics._TopicManager()

        self.subs_internal = {}
        
        self.external_node = None
        self.external_tcpros = None

    def connect(self):
        self.configure_internal()
        self.configure_external()
        
    def _configure_proxy_services(self, external_tcpros):
        external_tcpros.service_connection_handler = self.service_connection_handler

        # override the registration listeners to do nothing so we don't appear to be actually registering this listeners
        # create proxy handlers for each service
        i = 0
        for name, proxy in self.services.iteritems():
            
            i += 1
            c = proxy.service_class
            service = ServiceImpl(name, proxy.service_class, proxy)
            
            rospy.loginfo("registering proxied service %s"%(name))
            self.service_manager.register(name, service)
        
    def _configure_proxy_topics(self, external_tcpros, tcpros_handler):
        external_tcpros.topic_connection_handler = tcpros_handler.topic_connection_handler

        import rospy.topics
        
        # create publishers for each of the topics we internally subscribe to
        subs_initialized = {}
        for resolved_name, sub in self.subs_internal.iteritems():
            # create a pub handle
            rospy.loginfo("create external publisher [%s]", resolved_name)
            pub = self.topic_manager.acquire_impl(Registration.PUB, resolved_name, sub.data_class)
            # clone the subscriber handle, this time with a callback
            sub_ex = rospy.Subscriber(resolved_name, sub.data_class, pub_forwarder, pub)
            subs_initialized[resolved_name] = sub_ex
            # decrement the ref count of the old handle
            #sub.unregister()
            
        # throw away old subs_internal dictionary and replace it with
        # the initialized objects
        del self.subs_internal
        self.subs_internal = subs_initialized

        
    def configure_external(self):

        
        # Start another TCPROSServer to handle requests on the external port
        #  - TODO: allow handlers these to be passed into TCPROSServer constructor
        self.external_tcpros = external_tcpros = TCPROSServer()
        tcpros_handler = rosproxy.tcpros.ProxyTCPROSHandler(self.topic_manager, external_tcpros)

        self._configure_proxy_services(external_tcpros)
        self._configure_proxy_topics(external_tcpros, tcpros_handler)

        tcpros_port = rospy.get_param('~tcpros_port', 11312)
        xmlrpc_port = rospy.get_param('~xmlrpc_port', 11313)

        rospy.loginfo("reading port configuration: TCPROS[%s] XMLRPC[%s]"%(tcpros_port, xmlrpc_port))
        
        external_tcpros.start_server(port=tcpros_port)

        # TODO: this may report the address of the wrong interface
        rospy.loginfo("ROSRPC URI is rosrpc://%s:%s"%(rosgraph.network.get_local_address(), tcpros_port))

        # Startup XMLRPC interface so we can do pub/sub
        master_uri = rosgraph.get_master_uri()
        name = 'proxy-proxy'

        
        protocol_handlers = [tcpros_handler]
        rpc_handler = rosproxy.handler.ProxyHandler(name, master_uri, self.topic_manager, protocol_handlers)
        self.external_node = external_node= XmlRpcNode(xmlrpc_port, rpc_handler)

        # - start the node and wait for init
        external_node.start()
        import time
        timeout_t = time.time() + 10.
        while time.time() < timeout_t and external_node.uri is None:
            time.sleep(0.01)

        rospy.loginfo("XMLRPC interface is up %s"%self.external_node.uri)
        

    def _configure_internal_services(self, service_names):
        """
        Create rospy handles to all of the services that we are
        configured to be able to call.
        """
        
        i = 0
        for name in service_names:
            rospy.loginfo("configuring service %s", name)
            resolved_name = rospy.resolve_name(name)
            rospy.wait_for_service(name, timeout=60.)
            type_ = rosservice.get_service_type(resolved_name)
            if type_ is None:
                raise rospy.ROSInitException("cannot proxy service [%s]: unknown type"%resolved_name)

            i += 1

            # for efficiency, we generate a passthrough service
            # definition that does not do any serializatoin on the
            # request or response. This requires more work because the
            # instantiated class has to have the correct properties.
            real_service_class = roslib.message.get_service_class(type_)
            real_req = real_service_class._request_class
            real_resp = real_service_class._response_class
            request_d = {
                '_type': real_req._type,
                '_md5sum': real_req._md5sum,   
                '_full_text': real_req._full_text,   
                }
            response_d = {
                '_type': real_resp._type,
                '_md5sum': real_resp._md5sum,   
                '_full_text': real_resp._full_text,   
                }
            service_class = classobj('s_passthrough_%s'%i, (object,), {
                    '_type' : real_service_class._type,
                    '_md5sum' : real_service_class._md5sum,
                    '_request_class' : classobj('passthrough_request_%s'%i, (PassthroughServiceMessage, ), request_d),
                    '_response_class' : classobj('passthrough_response_%s'%i, (PassthroughServiceMessage,), response_d),
                    })
            
            self.services[resolved_name] = rospy.ServiceProxy(name, service_class, persistent=True)

            rospy.loginfo("proxying %s", resolved_name)
        
    def _configure_internal_topics(self, pub_names):
        """
        Create subscriptions to all the topics that we externally publish.
        """
        
        i = 0
        for name in pub_names:
            resolved_name = rospy.resolve_name(name)
            rospy.loginfo("configuring internal subscriber [%s]", resolved_name)            

            try:
                real_msg_class, _, _ = rostopic.get_topic_class(resolved_name)
            except rostopic.ROSTopicException:
                raise rospy.ROSInitException("cannot proxy subscription [%s]: unknown topic type"%resolved_name)

            i += 1
            topic_class = classobj('t_passthrough_%s'%i, (rospy.msg.AnyMsg,), {
                    '_type' : real_msg_class._type,
                    '_md5sum' : real_msg_class._md5sum,
                    })
            self.subs_internal[resolved_name] = rospy.Subscriber(name, topic_class)

            rospy.loginfo("proxying %s", resolved_name)
        
        
    def configure_internal(self):
        """
        Bring up connections to internal ROS graph
        """
        
        rospy.init_node('proxy')

        # fetch all the parameters for our node
    
        service_names = rospy.get_param('~services', {})
        self._configure_internal_services(service_names)

        pub_names = rospy.get_param('~pubs', {})
        self._configure_internal_topics(pub_names)
        

    def service_connection_handler(self, sock, client_addr, header):
        """
        @param sock: socket connection
        @type  sock: socket
        @param client_addr: client address
        @type  client_addr: (str, int)
        @param header: key/value pairs from handshake header
        @type  header: dict
        @return: error string or None 
        @rtype: str
        """

        # This is a cturtle hack. rospy's service_connection_handler
        # is wired to the ServiceManager singleton. If we replace the
        # singleton with something more configurable, then we simply
        # have to run our own ServiceManager to handle the forwarding
        # behavior.

        for required in ['service', 'md5sum', 'callerid']:
            if not required in header:
                return "Missing required '%s' field"%required
        else:
            #logger.debug("connection from %s:%s", client_addr[0], client_addr[1])
            service_name = header['service']

            sm = self.service_manager
            md5sum = header['md5sum']
            service = sm.get_service(service_name)
            if not service:
                return "[%s] is not a provider of  [%s]"%(rospy.names.get_caller_id(), service_name)
            elif md5sum != rospy.names.SERVICE_ANYTYPE and md5sum != service.service_class._md5sum:
                return "request from [%s]: md5sums do not match: [%s] vs. [%s]"%(header['callerid'], md5sum, service.service_class._md5sum)
            else:
                transport = TCPROSTransport(service.protocol, service_name, header=header)
                transport.set_socket(sock, header['callerid'])
                transport.write_header()
                thread.start_new_thread(service.handle, (transport, header))
class Proxy(object):
    def __init__(self):

        self.services = {}

        self.service_manager = ServiceManager(
            registration_listeners=RegistrationListeners())
        self.topic_manager = rospy.topics._TopicManager()

        self.subs_internal = {}

        self.external_node = None
        self.external_tcpros = None

    def connect(self):
        self.configure_internal()
        self.configure_external()

    def _configure_proxy_services(self, external_tcpros):
        external_tcpros.service_connection_handler = self.service_connection_handler

        # override the registration listeners to do nothing so we don't appear to be actually registering this listeners
        # create proxy handlers for each service
        i = 0
        for name, proxy in self.services.iteritems():

            i += 1
            c = proxy.service_class
            service = ServiceImpl(name, proxy.service_class, proxy)

            rospy.loginfo("registering proxied service %s" % (name))
            self.service_manager.register(name, service)

    def _configure_proxy_topics(self, external_tcpros, tcpros_handler):
        external_tcpros.topic_connection_handler = tcpros_handler.topic_connection_handler

        import rospy.topics

        # create publishers for each of the topics we internally subscribe to
        subs_initialized = {}
        for resolved_name, sub in self.subs_internal.iteritems():
            # create a pub handle
            rospy.loginfo("create external publisher [%s]", resolved_name)
            pub = self.topic_manager.acquire_impl(Registration.PUB,
                                                  resolved_name,
                                                  sub.data_class)
            # clone the subscriber handle, this time with a callback
            sub_ex = rospy.Subscriber(resolved_name, sub.data_class,
                                      pub_forwarder, pub)
            subs_initialized[resolved_name] = sub_ex
            # decrement the ref count of the old handle
            #sub.unregister()

        # throw away old subs_internal dictionary and replace it with
        # the initialized objects
        del self.subs_internal
        self.subs_internal = subs_initialized

    def configure_external(self):

        # Start another TCPROSServer to handle requests on the external port
        #  - TODO: allow handlers these to be passed into TCPROSServer constructor
        self.external_tcpros = external_tcpros = TCPROSServer()
        tcpros_handler = rosproxy.tcpros.ProxyTCPROSHandler(
            self.topic_manager, external_tcpros)

        self._configure_proxy_services(external_tcpros)
        self._configure_proxy_topics(external_tcpros, tcpros_handler)

        tcpros_port = rospy.get_param('~tcpros_port', 11312)
        xmlrpc_port = rospy.get_param('~xmlrpc_port', 11313)

        rospy.loginfo("reading port configuration: TCPROS[%s] XMLRPC[%s]" %
                      (tcpros_port, xmlrpc_port))

        external_tcpros.start_server(port=tcpros_port)

        # TODO: this may report the address of the wrong interface
        rospy.loginfo("ROSRPC URI is rosrpc://%s:%s" %
                      (rosgraph.network.get_local_address(), tcpros_port))

        # Startup XMLRPC interface so we can do pub/sub
        master_uri = rosgraph.get_master_uri()
        name = 'proxy-proxy'

        protocol_handlers = [tcpros_handler]
        rpc_handler = rosproxy.handler.ProxyHandler(name, master_uri,
                                                    self.topic_manager,
                                                    protocol_handlers)
        self.external_node = external_node = XmlRpcNode(
            xmlrpc_port, rpc_handler)

        # - start the node and wait for init
        external_node.start()
        import time
        timeout_t = time.time() + 10.
        while time.time() < timeout_t and external_node.uri is None:
            time.sleep(0.01)

        rospy.loginfo("XMLRPC interface is up %s" % self.external_node.uri)

    def _configure_internal_services(self, service_names):
        """
        Create rospy handles to all of the services that we are
        configured to be able to call.
        """

        i = 0
        for name in service_names:
            rospy.loginfo("configuring service %s", name)
            resolved_name = rospy.resolve_name(name)
            rospy.wait_for_service(name, timeout=60.)
            type_ = rosservice.get_service_type(resolved_name)
            if type_ is None:
                raise rospy.ROSInitException(
                    "cannot proxy service [%s]: unknown type" % resolved_name)

            i += 1

            # for efficiency, we generate a passthrough service
            # definition that does not do any serializatoin on the
            # request or response. This requires more work because the
            # instantiated class has to have the correct properties.
            real_service_class = roslib.message.get_service_class(type_)
            real_req = real_service_class._request_class
            real_resp = real_service_class._response_class
            request_d = {
                '_type': real_req._type,
                '_md5sum': real_req._md5sum,
                '_full_text': real_req._full_text,
            }
            response_d = {
                '_type': real_resp._type,
                '_md5sum': real_resp._md5sum,
                '_full_text': real_resp._full_text,
            }
            service_class = classobj(
                's_passthrough_%s' % i, (object, ), {
                    '_type':
                    real_service_class._type,
                    '_md5sum':
                    real_service_class._md5sum,
                    '_request_class':
                    classobj('passthrough_request_%s' % i,
                             (PassthroughServiceMessage, ), request_d),
                    '_response_class':
                    classobj('passthrough_response_%s' % i,
                             (PassthroughServiceMessage, ), response_d),
                })

            self.services[resolved_name] = rospy.ServiceProxy(name,
                                                              service_class,
                                                              persistent=True)

            rospy.loginfo("proxying %s", resolved_name)

    def _configure_internal_topics(self, pub_names):
        """
        Create subscriptions to all the topics that we externally publish.
        """

        i = 0
        for name in pub_names:
            resolved_name = rospy.resolve_name(name)
            rospy.loginfo("configuring internal subscriber [%s]",
                          resolved_name)

            try:
                real_msg_class, _, _ = rostopic.get_topic_class(resolved_name)
            except rostopic.ROSTopicException:
                raise rospy.ROSInitException(
                    "cannot proxy subscription [%s]: unknown topic type" %
                    resolved_name)

            i += 1
            topic_class = classobj('t_passthrough_%s' % i,
                                   (rospy.msg.AnyMsg, ), {
                                       '_type': real_msg_class._type,
                                       '_md5sum': real_msg_class._md5sum,
                                   })
            self.subs_internal[resolved_name] = rospy.Subscriber(
                name, topic_class)

            rospy.loginfo("proxying %s", resolved_name)

    def configure_internal(self):
        """
        Bring up connections to internal ROS graph
        """

        rospy.init_node('proxy')

        # fetch all the parameters for our node

        service_names = rospy.get_param('~services', {})
        self._configure_internal_services(service_names)

        pub_names = rospy.get_param('~pubs', {})
        self._configure_internal_topics(pub_names)

    def service_connection_handler(self, sock, client_addr, header):
        """
        @param sock: socket connection
        @type  sock: socket
        @param client_addr: client address
        @type  client_addr: (str, int)
        @param header: key/value pairs from handshake header
        @type  header: dict
        @return: error string or None 
        @rtype: str
        """

        # This is a cturtle hack. rospy's service_connection_handler
        # is wired to the ServiceManager singleton. If we replace the
        # singleton with something more configurable, then we simply
        # have to run our own ServiceManager to handle the forwarding
        # behavior.

        for required in ['service', 'md5sum', 'callerid']:
            if not required in header:
                return "Missing required '%s' field" % required
        else:
            #logger.debug("connection from %s:%s", client_addr[0], client_addr[1])
            service_name = header['service']

            sm = self.service_manager
            md5sum = header['md5sum']
            service = sm.get_service(service_name)
            if not service:
                return "[%s] is not a provider of  [%s]" % (
                    rospy.names.get_caller_id(), service_name)
            elif md5sum != rospy.names.SERVICE_ANYTYPE and md5sum != service.service_class._md5sum:
                return "request from [%s]: md5sums do not match: [%s] vs. [%s]" % (
                    header['callerid'], md5sum, service.service_class._md5sum)
            else:
                transport = TCPROSTransport(service.protocol,
                                            service_name,
                                            header=header)
                transport.set_socket(sock, header['callerid'])
                transport.write_header()
                thread.start_new_thread(service.handle, (transport, header))