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
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))
