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