def test_TCPROSTransport(self):
import rospy.impl.tcpros_base
from rospy.impl.tcpros_base import TCPROSTransport, TCPROSTransportProtocol
from rospy.impl.transport import OUTBOUND
p = TCPROSTransportProtocol('Bob', rospy.AnyMsg)
p.direction = OUTBOUND
try:
TCPROSTransport(p, '')
self.fail("TCPROSTransport should not accept bad name")
except rospy.impl.tcpros_base.TransportInitError: pass
t = TCPROSTransport(p, 'transport-name')
self.assert_(t.socket is None)
self.assert_(t.md5sum is None)
self.assert_(t.type is None)
self.assertEquals(p, t.protocol)
self.assertEquals('TCPROS', t.transport_type)
self.assertEquals(OUTBOUND, t.direction)
self.assertEquals('unknown', t.endpoint_id)
self.assertEquals('', t.read_buff.getvalue())
self.assertEquals('', t.write_buff.getvalue())
s = MockSock('12345')
t.set_socket(s, 'new_endpoint_id')
self.assertEquals('new_endpoint_id', t.endpoint_id)
self.assertEquals(s, t.socket)
t.close()
self.assert_(t.socket is None)
self.assert_(t.read_buff is None)
self.assert_(t.write_buff is None)
self.assert_(t.protocol is None)
def test_TCPROSTransport(self):
import rospy.impl.tcpros_base
from rospy.impl.tcpros_base import TCPROSTransport, TCPROSTransportProtocol
from rospy.impl.transport import OUTBOUND
p = TCPROSTransportProtocol('Bob', rospy.AnyMsg)
p.direction = OUTBOUND
try:
TCPROSTransport(p, '')
self.fail("TCPROSTransport should not accept bad name")
except rospy.impl.tcpros_base.TransportInitError: pass
t = TCPROSTransport(p, 'transport-name')
self.assert_(t.socket is None)
self.assert_(t.md5sum is None)
self.assert_(t.type is None)
self.assertEquals(p, t.protocol)
self.assertEquals('TCPROS', t.transport_type)
self.assertEquals(OUTBOUND, t.direction)
self.assertEquals('unknown', t.endpoint_id)
self.assertEquals('', t.read_buff.getvalue())
self.assertEquals('', t.write_buff.getvalue())
s = MockSock('12345')
t.set_socket(s, 'new_endpoint_id')
self.assertEquals('new_endpoint_id', t.endpoint_id)
self.assertEquals(s, t.socket)
t.close()
self.assert_(t.socket is None)
self.assert_(t.read_buff is None)
self.assert_(t.write_buff is None)
self.assert_(t.protocol is None)
def call(self, *args, **kwds):
"""
Call the service. This accepts either a request message instance,
or you can call directly with arguments to create a new request instance. e.g.::
add_two_ints(AddTwoIntsRequest(1, 2))
add_two_ints(1, 2)
add_two_ints(a=1, b=2)
@raise TypeError: if request is not of the valid type (Message)
@raise ServiceException: if communication with remote service fails
@raise ROSInterruptException: if node shutdown (e.g. ctrl-C) interrupts service call
@raise ROSSerializationException: If unable to serialize
message. This is usually a type error with one of the fields.
"""
# convert args/kwds to request message class
request = rospy.msg.args_kwds_to_message(self.request_class, args, kwds)
# initialize transport
if self.transport is None:
service_uri = self._get_service_uri(request)
dest_addr, dest_port = rospy.core.parse_rosrpc_uri(service_uri)
# connect to service
transport = TCPROSTransport(self.protocol, self.resolved_name)
transport.buff_size = self.buff_size
try:
transport.connect(dest_addr, dest_port, service_uri)
except TransportInitError as e:
# can be a connection or md5sum mismatch
raise ServiceException("unable to connect to service: %s"%e)
self.transport = transport
else:
transport = self.transport
# send the actual request message
self.seq += 1
transport.send_message(request, self.seq)
try:
responses = transport.receive_once()
if len(responses) == 0:
raise ServiceException("service [%s] returned no response"%self.resolved_name)
elif len(responses) > 1:
raise ServiceException("service [%s] returned multiple responses: %s"%(self.resolved_name, len(responses)))
except rospy.exceptions.TransportException as e:
# convert lower-level exception to exposed type
if rospy.core.is_shutdown():
raise rospy.exceptions.ROSInterruptException("node shutdown interrupted service call")
else:
raise ServiceException("transport error completing service call: %s"%(str(e)))
finally:
if not self.persistent:
transport.close()
self.transport = None
return responses[0]
def call(self, *args, **kwds):
"""
Call the service. This accepts either a request message instance,
or you can call directly with arguments to create a new request instance. e.g.::
add_two_ints(AddTwoIntsRequest(1, 2))
add_two_ints(1, 2)
add_two_ints(a=1, b=2)
@raise TypeError: if request is not of the valid type (Message)
@raise ServiceException: if communication with remote service fails
@raise ROSInterruptException: if node shutdown (e.g. ctrl-C) interrupts service call
@raise ROSSerializationException: If unable to serialize
message. This is usually a type error with one of the fields.
"""
# convert args/kwds to request message class
request = rospy.msg.args_kwds_to_message(self.request_class, args, kwds)
# initialize transport
if self.transport is None:
service_uri = self._get_service_uri(request)
dest_addr, dest_port = rospy.core.parse_rosrpc_uri(service_uri)
# connect to service
transport = TCPROSTransport(self.protocol, self.resolved_name)
transport.buff_size = self.buff_size
try:
transport.connect(dest_addr, dest_port, service_uri)
except TransportInitError as e:
# can be a connection or md5sum mismatch
raise ServiceException("unable to connect to service: %s"%e)
self.transport = transport
else:
transport = self.transport
# send the actual request message
self.seq += 1
transport.send_message(request, self.seq)
try:
responses = transport.receive_once()
if len(responses) == 0:
raise ServiceException("service [%s] returned no response"%self.resolved_name)
elif len(responses) > 1:
raise ServiceException("service [%s] returned multiple responses: %s"%(self.resolved_name, len(responses)))
except rospy.exceptions.TransportException as e:
# convert lower-level exception to exposed type
if rospy.core.is_shutdown():
raise rospy.exceptions.ROSInterruptException("node shutdown interrupted service call")
else:
raise ServiceException("transport error completing service call: %s"%(str(e)))
finally:
if not self.persistent:
transport.close()
self.transport = None
return responses[0]
def create_transport(self, resolved_name, pub_uri, protocol_params):
"""
Connect to topic resolved_name on Publisher pub_uri using TCPROS.
@param resolved_name str: resolved topic name
@type resolved_name: str
@param pub_uri: XML-RPC URI of publisher
@type pub_uri: str
@param protocol_params: protocol parameters to use for connecting
@type protocol_params: [XmlRpcLegal]
@return: code, message, debug
@rtype: (int, str, int)
"""
#Validate protocol params = [TCPROS, address, port]
if type(protocol_params) != list or len(protocol_params) != 3:
return 0, "ERROR: invalid TCPROS parameters", 0
if protocol_params[0] != TCPROS:
return 0, "INTERNAL ERROR: protocol id is not TCPROS: %s" % protocol_params[
0], 0
id, dest_addr, dest_port = protocol_params
sub = rospy.impl.registration.get_topic_manager().get_subscriber_impl(
resolved_name)
#Create connection
protocol = TCPROSSub(resolved_name, sub.data_class, \
queue_size=sub.queue_size, buff_size=sub.buff_size,
tcp_nodelay=sub.tcp_nodelay)
conn = TCPROSTransport(protocol, resolved_name)
conn.set_endpoint_id(pub_uri)
t = threading.Thread(name=resolved_name,
target=robust_connect_subscriber,
args=(conn, dest_addr, dest_port, pub_uri,
sub.receive_callback, resolved_name))
# don't enable this just yet, need to work on this logic
#rospy.core._add_shutdown_thread(t)
# Attach connection to _SubscriberImpl
if sub.add_connection(conn): #pass tcp connection to handler
# since the thread might cause the connection to close
# it should only be started after the connection has been added to the subscriber
# https://github.com/ros/ros_comm/issues/544
t.start()
return 1, "Connected topic[%s]. Transport impl[%s]" % (
resolved_name, conn.__class__.__name__), dest_port
else:
# _SubscriberImpl already closed or duplicate subscriber created
conn.close()
return 0, "ERROR: Race condition failure creating topic subscriber [%s]" % (
resolved_name), 0
def create_transport(self, resolved_name, pub_uri, protocol_params):
"""
Connect to topic resolved_name on Publisher pub_uri using TCPROS.
@param resolved_name str: resolved topic name
@type resolved_name: str
@param pub_uri: XML-RPC URI of publisher
@type pub_uri: str
@param protocol_params: protocol parameters to use for connecting
@type protocol_params: [XmlRpcLegal]
@return: code, message, debug
@rtype: (int, str, int)
"""
#Validate protocol params = [TCPROS, address, port]
if type(protocol_params) != list or len(protocol_params) != 3:
return 0, "ERROR: invalid TCPROS parameters", 0
if protocol_params[0] != TCPROS:
return 0, "INTERNAL ERROR: protocol id is not TCPROS: %s" % id, 0
id, dest_addr, dest_port = protocol_params
sub = rospy.impl.registration.get_topic_manager().get_subscriber_impl(
resolved_name)
#Create connection
try:
protocol = TCPROSSub(resolved_name, sub.data_class, \
queue_size=sub.queue_size, buff_size=sub.buff_size,
tcp_nodelay=sub.tcp_nodelay)
conn = TCPROSTransport(protocol, resolved_name)
# timeout is really generous. for now just choosing one that is large but not infinite
bool = conn.connect(dest_addr, dest_port, pub_uri, timeout=60.)
t = threading.Thread(name=resolved_name,
target=conn.receive_loop,
args=(sub.receive_callback, ))
# don't enable this just yet, need to work on this logic
#rospy.core._add_shutdown_thread(t)
t.start()
except rospy.exceptions.TransportInitError as e:
rospyerr("unable to create subscriber transport: %s", e)
return 0, "Internal error creating inbound TCP connection for [%s]: %s" % (
resolved_name, e), -1
# Attach connection to _SubscriberImpl
if sub.add_connection(conn): #pass tcp connection to handler
return 1, "Connected topic[%s]. Transport impl[%s]" % (
resolved_name, conn.__class__.__name__), dest_port
else:
conn.close()
return 0, "ERROR: Race condition failure: duplicate topic subscriber [%s] was created" % (
resolved_name), 0
def create_transport(self, resolved_name, pub_uri, protocol_params):
"""
Connect to topic resolved_name on Publisher pub_uri using TCPROS.
@param resolved_name str: resolved topic name
@type resolved_name: str
@param pub_uri: XML-RPC URI of publisher
@type pub_uri: str
@param protocol_params: protocol parameters to use for connecting
@type protocol_params: [XmlRpcLegal]
@return: code, message, debug
@rtype: (int, str, int)
"""
# Validate protocol params = [TCPROS, address, port]
if type(protocol_params) != list or len(protocol_params) != 3:
return 0, "ERROR: invalid TCPROS parameters", 0
if protocol_params[0] != TCPROS:
return 0, "INTERNAL ERROR: protocol id is not TCPROS: %s" % id, 0
id, dest_addr, dest_port = protocol_params
sub = rospy.impl.registration.get_topic_manager().get_subscriber_impl(resolved_name)
# Create connection
try:
protocol = TCPROSSub(
resolved_name,
sub.data_class,
queue_size=sub.queue_size,
buff_size=sub.buff_size,
tcp_nodelay=sub.tcp_nodelay,
)
conn = TCPROSTransport(protocol, resolved_name)
# timeout is really generous. for now just choosing one that is large but not infinite
bool = conn.connect(dest_addr, dest_port, pub_uri, timeout=60.0)
t = threading.Thread(name=resolved_name, target=conn.receive_loop, args=(sub.receive_callback,))
# don't enable this just yet, need to work on this logic
# rospy.core._add_shutdown_thread(t)
t.start()
except rospy.exceptions.TransportInitError as e:
rospyerr("unable to create subscriber transport: %s", e)
return 0, "Internal error creating inbound TCP connection for [%s]: %s" % (resolved_name, e), -1
# Attach connection to _SubscriberImpl
if sub.add_connection(conn): # pass tcp connection to handler
return 1, "Connected topic[%s]. Transport impl[%s]" % (resolved_name, conn.__class__.__name__), dest_port
else:
conn.close()
return 0, "ERROR: Race condition failure: duplicate topic subscriber [%s] was created" % (resolved_name), 0
def create_transport(self, resolved_name, pub_uri, protocol_params):
"""
Connect to topic resolved_name on Publisher pub_uri using TCPROS.
@param resolved_name str: resolved topic name
@type resolved_name: str
@param pub_uri: XML-RPC URI of publisher
@type pub_uri: str
@param protocol_params: protocol parameters to use for connecting
@type protocol_params: [XmlRpcLegal]
@return: code, message, debug
@rtype: (int, str, int)
"""
#Validate protocol params = [TCPROS, address, port]
if type(protocol_params) != list or len(protocol_params) != 3:
return 0, "ERROR: invalid TCPROS parameters", 0
if protocol_params[0] != TCPROS:
return 0, "INTERNAL ERROR: protocol id is not TCPROS: %s"%id, 0
id, dest_addr, dest_port = protocol_params
sub = rospy.impl.registration.get_topic_manager().get_subscriber_impl(resolved_name)
#Create connection
protocol = TCPROSSub(resolved_name, sub.data_class, \
queue_size=sub.queue_size, buff_size=sub.buff_size,
tcp_nodelay=sub.tcp_nodelay)
conn = TCPROSTransport(protocol, resolved_name)
conn.set_endpoint_id(pub_uri);
t = threading.Thread(name=resolved_name, target=robust_connect_subscriber, args=(conn, dest_addr, dest_port, pub_uri, sub.receive_callback,resolved_name))
# don't enable this just yet, need to work on this logic
#rospy.core._add_shutdown_thread(t)
# Attach connection to _SubscriberImpl
if sub.add_connection(conn): #pass tcp connection to handler
# since the thread might cause the connection to close
# it should only be started after the connection has been added to the subscriber
# https://github.com/ros/ros_comm/issues/544
t.start()
return 1, "Connected topic[%s]. Transport impl[%s]"%(resolved_name, conn.__class__.__name__), dest_port
else:
# _SubscriberImpl already closed or duplicate subscriber created
conn.close()
return 0, "ERROR: Race condition failure creating topic subscriber [%s]"%(resolved_name), 0
def callService(self, service_uri, service, service_type, service_args=[]):
'''
Calls the service and return the response.
To call the service the ServiceProxy can't be used, because it uses
environment variables to determine the URI of the running service. In our
case this service can be running using another ROS master. The changes on the
environment variables is not thread safe.
So the source code of the rospy.SerivceProxy (tcpros_service.py) was modified.
:param str service_uri: the URI of the service
:param str service: full service name (with name space)
:param service_type: service class
:type service_type: ServiceDefinition: service class
:param service_args: arguments
:return: the tuple of request and response.
:rtype: (request object, response object)
:raise StartException: on error
:see: rospy.SerivceProxy<http://docs.ros.org/kinetic/api/rospy/html/rospy.impl.tcpros_service.ServiceProxy-class.html>
'''
service = str(service)
rospy.loginfo("Call service %s[%s]: %s, %s", utf8(service),
utf8(service_uri), utf8(service_type),
utf8(service_args))
from rospy.core import parse_rosrpc_uri, is_shutdown
# from rospy.msg import args_kwds_to_message
from rospy.exceptions import TransportInitError, TransportException
from rospy.impl.tcpros_base import TCPROSTransport, DEFAULT_BUFF_SIZE # ,TCPROSTransportProtocol
from rospy.impl.tcpros_service import TCPROSServiceClient
from rospy.service import ServiceException
request = service_type._request_class()
import genpy
try:
now = rospy.get_rostime()
import std_msgs.msg
keys = {'now': now, 'auto': std_msgs.msg.Header(stamp=now)}
genpy.message.fill_message_args(request, service_args, keys)
except genpy.MessageException as e:
def argsummary(args):
if type(args) in [tuple, list]:
return '\n'.join([
' * %s (type %s)' % (a, type(a).__name__) for a in args
])
else:
return ' * %s (type %s)' % (args, type(args).__name__)
raise StartException(
"Incompatible arguments to call service:\n%s\nProvided arguments are:\n%s\n\nService arguments are: [%s]"
% (e, argsummary(service_args),
genpy.message.get_printable_message_args(request)))
# request = args_kwds_to_message(type._request_class, args, kwds)
protocol = TCPROSServiceClient(service, service_type, headers={})
transport = TCPROSTransport(protocol, service)
# initialize transport
dest_addr, dest_port = parse_rosrpc_uri(service_uri)
# connect to service
transport.buff_size = DEFAULT_BUFF_SIZE
try:
transport.connect(dest_addr, dest_port, service_uri, timeout=5)
except TransportInitError as e:
# can be a connection or md5sum mismatch
raise StartException(''.join(
["unable to connect to service: ",
utf8(e)]))
transport.send_message(request, 0)
try:
responses = transport.receive_once()
if len(responses) == 0:
raise StartException("service [%s] returned no response" %
service)
elif len(responses) > 1:
raise StartException(
"service [%s] returned multiple responses: %s" %
(service, len(responses)))
except TransportException as e:
# convert lower-level exception to exposed type
if is_shutdown():
raise StartException("node shutdown interrupted service call")
else:
raise StartException(
"transport error completing service call: %s" % (utf8(e)))
except ServiceException as e:
raise StartException("Service error: %s" % (utf8(e)))
finally:
transport.close()
transport = None
return request, responses[0] if len(responses) > 0 else None
