def topic_connection_handler(self, sock, client_addr, header): """ Process incoming topic connection. Reads in topic name from handshake and creates the appropriate L{TCPROSPub} handler for the connection. @param sock: socket connection @type sock: socket.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 """ if rospy.core.is_shutdown_requested(): return "Node is shutting down" for required in ['topic', 'md5sum', 'callerid']: if not required in header: return "Missing required '%s' field"%required else: resolved_topic_name = header['topic'] md5sum = header['md5sum'] tm = rospy.impl.registration.get_topic_manager() topic = tm.get_publisher_impl(resolved_topic_name) if not topic: return "[%s] is not a publisher of [%s]. Topics are %s"%(rospy.names.get_caller_id(), resolved_topic_name, tm.get_publications()) elif not topic.data_class or topic.closed: return "Internal error processing topic [%s]"%(resolved_topic_name) elif md5sum != rospy.names.TOPIC_ANYTYPE and md5sum != topic.data_class._md5sum: data_class = topic.data_class actual_type = data_class._type # check to see if subscriber sent 'type' header. If they did, check that # types are same first as this provides a better debugging message if 'type' in header: requested_type = header['type'] if requested_type != actual_type: return "topic types do not match: [%s] vs. [%s]"%(requested_type, actual_type) else: # defaults to actual type requested_type = actual_type return "Client [%s] wants topic [%s] to have datatype/md5sum [%s/%s], but our version has [%s/%s] Dropping connection."%(header['callerid'], resolved_topic_name, requested_type, md5sum, actual_type, data_class._md5sum) else: #TODO:POLLING if polling header is present, have to spin up receive loop as well # #1334: tcp_nodelay support from subscriber option if 'tcp_nodelay' in header: tcp_nodelay = True if header['tcp_nodelay'].strip() == '1' else False else: tcp_nodelay = self.tcp_nodelay_map.get(resolved_topic_name, False) _configure_pub_socket(sock, tcp_nodelay) protocol = TCPROSPub(resolved_topic_name, topic.data_class, is_latch=topic.is_latch, headers=topic.headers) transport = TCPROSTransport(protocol, resolved_topic_name) transport.set_socket(sock, header['callerid']) transport.remote_endpoint = client_addr transport.write_header() topic.add_connection(transport)
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, e: # can be a connection or md5sum mismatch raise ServiceException("unable to connect to service: %s" % e) self.transport = transport
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" % 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 listen_to(self, service_name): rospy.loginfo("Tapping service: {}".format(service_name)) # block until the service is available rospy.wait_for_service(service_name) # determine which node provides the given service server = rosservice.get_service_node(service_name) assert not server is None # get the class used by this service service_cls = rosservice.get_service_class_by_name(service_name) # create a persistent proxy to that service # inject a persistent connection into the proxy, so that when we replace # the original service, we can still forward messages onto the old one proxy = rospy.ServiceProxy(service_name, service_cls, persistent=True) # TODO: listen for failures # http://docs.ros.org/jade/api/rospy/html/rospy.impl.tcpros_service-pysrc.html#ServiceProxy service_uri = self.master.lookupService(proxy.resolved_name) (dest_addr, dest_port) = rospy.core.parse_rosrpc_uri(service_uri) proxy.transport = TCPROSTransport(proxy.protocol, proxy.resolved_name) proxy.transport.buff_size = proxy.buff_size proxy.transport.connect(dest_addr, dest_port, service_uri) # record the URI of the original service, so we can restore it later self.tapped[service_name] = service_uri # create a new, tapped service, with the same name tap = lambda r: self.__handler(server, service_name, proxy, r) rospy.Service(service_name, service_cls, tap) rospy.loginfo("Tapped service: {}".format(service_name))
def callService(self, service_uri, service, type, *args, **kwds): ''' 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 service_uri: the URI of the service @type service_uri: C{str} @param service: full service name (with name space) @type service: C{str} @param type: service class @type type: ServiceDefinition: service class @param args: arguments to remote service @param kwds: message keyword arguments @return: the tuple of request and response. @rtype: C{(request object, response object)} @raise StartException: on error @see: L{rospy.SerivceProxy} ''' 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, TCPROSTransportProtocol, DEFAULT_BUFF_SIZE from rospy.impl.tcpros_service import TCPROSServiceClient from rospy.service import ServiceException request = args_kwds_to_message(type._request_class, args, kwds) transport = None protocol = TCPROSServiceClient(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) except TransportInitError as e: # can be a connection or md5sum mismatch raise StartException("unable to connect to service: %s"%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"%(str(e))) except ServiceException, e: raise StartException("Service error: %s"%(str(e)))
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 inbound_handler(sock, addr): print "%s connected." % (str(addr)) t = TCPROSTransport(TestServerProtocol(), "testservice") t.set_socket(sock, "client") t.write_header() t.read_header() inputs.append(sock)
def service_connection_handler(sock, client_addr, header): """ Process incoming service connection. For use with TCPROSServer. Reads in service name from handshake and creates the appropriate service handler for the connection. @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 """ 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'] #TODO: make service manager configurable. I think the right #thing to do is to make these singletons private members of a #Node instance and enable rospy to have multiple node #instances. sm = get_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() # using threadpool reduced performance by an order of # magnitude, need to investigate better t = threading.Thread(target=service.handle, args=(transport, header)) t.setDaemon(True) t.start()
def service_connection_handler(sock, client_addr, header): """ Process incoming service connection. For use with TCPROSServer. Reads in service name from handshake and creates the appropriate service handler for the connection. @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 """ 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'] """ check again if the client request is authorized """ auth_logger.info( "received service connection for %s from %s (%s:%s)" % (service_name, header["callerid"], client_addr[0], client_addr[1])) client_ip_address = client_addr[0] if not is_requester_authorized(service_name, client_ip_address): auth_logger.warn( "service connection for %s from %s (%s) not authorized" % (service_name, header["callerid"], client_ip_address)) return "Client [%s] wants service connection for %s, but %s is not authorized" % ( header['callerid'], service_name, client_ip_address) auth_logger.info("service connection for %s from %s (%s) OK" % (service_name, header["callerid"], client_ip_address)) #TODO: make service manager configurable. I think the right #thing to do is to make these singletons private members of a #Node instance and enable rospy to have multiple node #instances. sm = get_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() # using threadpool reduced performance by an order of # magnitude, need to investigate better t = threading.Thread(target=service.handle, args=(transport, header)) t.setDaemon(True) t.start()
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))
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 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))
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
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 service_uri: the URI of the service @type service_uri: C{str} @param service: full service name (with name space) @type service: C{str} @param service_type: service class @type service_type: ServiceDefinition: service class @param service_args: arguments @return: the tuple of request and response. @rtype: C{(request object, response object)} @raise StartException: on error @see: U{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, e: raise StartException("Service error: %s" % (utf8(e)))
def callService(self, service_uri, service, type, *args, **kwds): ''' 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 service_uri: the URI of the service @type service_uri: C{str} @param service: full service name (with name space) @type service: C{str} @param type: service class @type type: ServiceDefinition: service class @param args: arguments to remote service @param kwds: message keyword arguments @return: the tuple of request and response. @rtype: C{(request object, response object)} @raise StartException: on error @see: L{rospy.SerivceProxy} ''' 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, TCPROSTransportProtocol, DEFAULT_BUFF_SIZE from rospy.impl.tcpros_service import TCPROSServiceClient from rospy.service import ServiceException request = args_kwds_to_message(type._request_class, args, kwds) transport = None protocol = TCPROSServiceClient(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) except TransportInitError as e: # can be a connection or md5sum mismatch raise StartException("unable to connect to service: %s" % 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" % (str(e))) except ServiceException, e: raise StartException("Service error: %s" % (str(e)))
t.set_socket(sock, "client") t.write_header() t.read_header() inputs.append(sock) if __name__ == '__main__': s = TCPServer(inbound_handler, PORT) s.start() while True: inputready,outputready,exceptready = select.select(inputs, [], [], 1) for s in inputready: # s is really just a python socket, but let's use # ROS to parse the message and write a response t = TCPROSTransport(TestServerProtocol(), "testservice") t.set_socket(s, "client") try: data = t.receive_once() # Echo all messages seq = 0 for msg in data: msg.data = "This is a reply of the message '%s'." % (msg.data) t.send_message(msg, seq) seq += 1 print "Echoed data" except rospy.exceptions.TransportTerminated: print "Closing connection" s.close()
#!/usr/bin/env python import roslib; roslib.load_manifest('testserver') import rospy from std_msgs.msg import String from rospy.impl.tcpros_base import TCPROSTransport from rospy.impl.tcpros_base import TCPROSTransportProtocol from prot import TestServerProtocol import sys PORT = 2048 if __name__ == "__main__": t = TCPROSTransport(TestServerProtocol(), "testservice") t.connect("127.0.0.1", PORT, "testservice_connection") msg = String() seq = 0 while True: # Send line from stdin line = sys.stdin.readline() if not line: break; msg.data = line[:-1] t.send_message(msg, seq) seq+=1 # Print response print t.receive_once()