Exemple #1
0
    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)
Exemple #2
0
    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)
Exemple #3
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, 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)
Exemple #5
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 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
Exemple #8
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
Exemple #9
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 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 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)))
Exemple #12
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
Exemple #13
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)))
Exemple #22
0
  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()
Exemple #23
0
#!/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()