Exemple #1
0
    def _tcp_server_callback(self, sock, client_addr):
        """
        TCPServer callback: detects incoming topic or service connection and passes connection accordingly
    
        @param sock: socket connection
        @type  sock: socket.socket
        @param client_addr: client address
        @type  client_addr: (str, int)
        @raise TransportInitError: If transport cannot be succesfully initialized
        """
        #TODOXXX:rewrite this logic so it is possible to create TCPROSTransport object first, set its protocol,
        #and then use that to do the writing
        try:
            buff_size = 4096  # size of read buffer
            if python3 == 0:
                #initialize read_ros_handshake_header with BytesIO for Python 3 (instead of bytesarray())
                header = read_ros_handshake_header(sock, StringIO(), buff_size)
            else:
                header = read_ros_handshake_header(sock, BytesIO(), buff_size)

            if 'topic' in header:
                err_msg = self.topic_connection_handler(
                    sock, client_addr, header)
            elif 'service' in header:
                err_msg = self.service_connection_handler(
                    sock, client_addr, header)
            else:
                err_msg = 'no topic or service name detected'
            if err_msg:
                # shutdown race condition: nodes that come up and down
                # quickly can receive connections during teardown.

                # We use is_shutdown_requested() because we can get
                # into bad connection states during client shutdown
                # hooks.
                if not rospy.core.is_shutdown_requested():
                    write_ros_handshake_header(sock, {'error': err_msg})
                    raise TransportInitError(
                        "Could not process inbound connection: " + err_msg +
                        str(header))
                else:
                    write_ros_handshake_header(sock,
                                               {'error': 'node shutting down'})
                    return
        except rospy.exceptions.TransportInitError as e:
            logwarn(str(e))
            if sock is not None:
                sock.close()
        except Exception as e:
            # collect stack trace separately in local log file
            if not rospy.core.is_shutdown_requested():
                logwarn("Inbound TCP/IP connection failed from '%s': %s",
                        str(client_addr), e)
                rospyerr("Inbound TCP/IP connection failed from '%s':\n%s",
                         str(client_addr), traceback.format_exc())
            if sock is not None:
                sock.close()
Exemple #2
0
 def _tcp_server_callback(self, sock, client_addr):
     """
     TCPServer callback: detects incoming topic or service connection and passes connection accordingly
 
     @param sock: socket connection
     @type  sock: socket.socket
     @param client_addr: client address
     @type  client_addr: (str, int)
     @raise TransportInitError: If transport cannot be succesfully initialized
     """
     #TODOXXX:rewrite this logic so it is possible to create TCPROSTransport object first, set its protocol,
     #and then use that to do the writing
     try:
         buff_size = 4096 # size of read buffer
         if python3 == 0:
             #initialize read_ros_handshake_header with BytesIO for Python 3 (instead of bytesarray())    
             header = read_ros_handshake_header(sock, StringIO(), buff_size)
         else:
             header = read_ros_handshake_header(sock, BytesIO(), buff_size)
         
         if 'topic' in header:
             err_msg = self.topic_connection_handler(sock, client_addr, header)
         elif 'service' in header:
             err_msg = self.service_connection_handler(sock, client_addr, header)
         else:
             err_msg = 'no topic or service name detected'
         if err_msg:
             # shutdown race condition: nodes that come up and down
             # quickly can receive connections during teardown.
             
             # We use is_shutdown_requested() because we can get
             # into bad connection states during client shutdown
             # hooks.
             if not rospy.core.is_shutdown_requested():
                 write_ros_handshake_header(sock, {'error' : err_msg})
                 raise TransportInitError("Could not process inbound connection: "+err_msg+str(header))
             else:
                 write_ros_handshake_header(sock, {'error' : 'node shutting down'})
                 return
     except rospy.exceptions.TransportInitError as e:
         logwarn(str(e))
         if sock is not None:
             sock.close()
     except Exception as e:
         # collect stack trace separately in local log file
         if not rospy.core.is_shutdown_requested():
             logwarn("Inbound TCP/IP connection failed: %s", e)
             rospyerr("Inbound TCP/IP connection failed:\n%s", traceback.format_exc())
         if sock is not None:
             sock.close()
Exemple #3
0
    def run(self):
        while not self._shutdownflag:
            try:
                (client_sock, _) = self._server_sock.accept()
                self.addSubscriberLink(client_sock)

                if sys.version_info[0] == 3:
                    header = read_ros_handshake_header(client_sock, BytesIO(),
                                                       65536)
                else:
                    header = read_ros_handshake_header(client_sock, StringIO(),
                                                       65536)

                fileno = client_sock.fileno()
                poller = None
                if hasattr(select, 'poll'):
                    ready = False
                    poller = select.poll()
                    poller.register(fileno, select.POLLOUT)
                    while not ready:
                        events = poller.poll()
                        for _, flag in events:
                            if flag & select.POLLOUT:
                                ready = True
                else:
                    ready = None
                    while not ready:
                        try:
                            _, ready, _ = select.select([], [fileno], [])
                        except ValueError:
                            print("ValueError")
                            return
                client_sock.setblocking(1)

                guard_pub = OpenRTM_aist.Guard.ScopedLock(self._pub_mutex)
                for publisher in self._publishers:
                    publisher.connect(client_sock, header)
                del guard_pub

                if poller:
                    poller.unregister(fileno)

            except rosgraph.network.ROSHandshakeException:
                print("read ROS handshake exception")
            except BaseException:
                print(OpenRTM_aist.Logger.print_exception())
Exemple #4
0
 def read_header(self):
     """
     Read TCPROS header from active socket
     @raise TransportInitError if header fails to validate
     """
     sock = self.socket
     if sock is None:
         return
     sock.setblocking(1)
     self._validate_header(read_ros_handshake_header(sock, self.read_buff, self.protocol.buff_size))
Exemple #5
0
 def read_header(self):
     """
     Read TCPROS header from active socket
     @raise TransportInitError if header fails to validate
     """
     sock = self.socket
     if sock is None:
         return
     sock.setblocking(1)
     # TODO: add bytes received to self.stat_bytes
     self._validate_header(read_ros_handshake_header(sock, self.read_buff, self.protocol.buff_size))
Exemple #6
0
    def connect(self, caller_id, topic, publishers):
        self._rtcout.RTC_VERBOSE("connect()")
        if topic != self._topic:
            self._rtcout.RTC_WARN(
                "Topic name is not match(%s:%s)", (topic, self._topic))
            return

        for uri in publishers:
            if uri in self._tcp_connecters:
                continue
            self._rtcout.RTC_PARANOID(
                "connectTCP(%s, %s, %s)", (caller_id, topic, uri))
            try:
                pub = xmlrpclib.ServerProxy(uri)
                ret, message, result = pub.requestTopic(
                    self._callerid, topic, [['TCPROS']])
            except BaseException:
                self._rtcout.RTC_ERROR("Failed connect %s", uri)
                continue

            if ret == -1:
                self._rtcout.RTC_WARN("requestTopic error: %s", message)
                continue
            elif ret == 0:
                self._rtcout.RTC_WARN("requestTopic error: %s", message)
                continue
            else:
                _, dest_addr, dest_port = result
                sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
                sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
                sock.setsockopt(socket.SOL_SOCKET, socket.SO_KEEPALIVE, 1)
                sock.setsockopt(socket.SOL_TCP, socket.TCP_KEEPCNT, 9)
                sock.setsockopt(socket.SOL_TCP, socket.TCP_KEEPIDLE, 60)
                sock.setsockopt(socket.SOL_TCP, socket.TCP_KEEPINTVL, 10)
                sock.setsockopt(socket.IPPROTO_TCP, socket.TCP_NODELAY, 1)
                sock.settimeout(60.0)
                sock.connect((dest_addr, dest_port))

                fileno = sock.fileno()
                if hasattr(select, 'poll'):
                    poller = select.poll()
                    poller.register(fileno, select.POLLOUT)
                    ready = False

                    while not ready:
                        events = poller.poll()
                        for _, flag in events:
                            if flag & select.POLLOUT:
                                ready = True
                else:
                    ready = None
                    while not ready:
                        try:
                            _, ready, _ = select.select([], [fileno], [])
                        except ValueError:
                            self._rtcout.RTC_ERROR("ValueError")
                            return

                factory = ROSMessageInfo.ROSMessageInfoList.instance()
                info = factory.getInfo(self._messageType)
                if(info):
                    info_type = info.datatype()
                    info_md5sum = info.md5sum()
                    info_message_definition = info.message_definition()
                else:
                    self._rtcout.RTC_ERROR(
                        "Can not found %s", self._messageType)

                sock.setblocking(1)
                fields = {'topic': topic,
                          'message_definition': info_message_definition,
                          'tcp_nodelay': '0',
                          'md5sum': info_md5sum,
                          'type': info_type,
                          'callerid': self._callerid}

                try:
                    write_ros_handshake_header(sock, fields)
                except rosgraph.network.ROSHandshakeException:
                    self._rtcout.RTC_ERROR("write ROS handshake header")
                    continue
                if sys.version_info[0] == 3:
                    read_buff = BytesIO()
                else:
                    read_buff = StringIO()
                sock.setblocking(1)

                try:
                    read_ros_handshake_header(sock, read_buff, 65536)
                except rosgraph.network.ROSHandshakeException:
                    self._rtcout.RTC_ERROR("read ROS handshake header")
                    continue

                listener = SubListener(self, sock, uri)

                self._rtcout.RTC_VERBOSE("Subscriber Listener thread start")
                task = threading.Thread(target=listener.recieve, args=())
                task.start()

                self._tcp_connecters[uri] = {
                    "socket": sock,
                    "listener": listener,
                    "thread": task,
                    "id": self._pubnum}
                self._pubnum += 1
    def connect(self, client_sock, addr):
        self._rtcout.RTC_PARANOID("connect()")
        if addr in self._tcp_connecters:
            self._rtcout.RTC_DEBUG("%s already exist", addr)
            return

        try:
            if sys.version_info[0] == 3:
                header = read_ros_handshake_header(client_sock, BytesIO(),
                                                   65536)
            else:
                header = read_ros_handshake_header(client_sock, StringIO(),
                                                   65536)
        except rosgraph.network.ROSHandshakeException:
            self._rtcout.RTC_DEBUG("read ROS handshake exception")
            return
        except BaseException:
            self._rtcout.RTC_ERROR(OpenRTM_aist.Logger.print_exception())

        topic_name = header['topic']
        md5sum = header['md5sum']
        type_name = header['type']

        self._rtcout.RTC_VERBOSE("Topic:%s", topic_name)
        self._rtcout.RTC_VERBOSE("MD5sum:%s", md5sum)
        self._rtcout.RTC_VERBOSE("Type:%s", type_name)

        factory = ROSMessageInfo.ROSMessageInfoList.instance()
        info = factory.getInfo(self._messageType)

        if info:
            info_type = info.datatype()
            info_md5sum = info.md5sum()
            info_message_definition = info.message_definition()
        else:
            self._rtcout.RTC_ERROR("can not found %s", self._messageType)
            return

        if info_type != type_name:
            self._rtcout.RTC_WARN("topic name in not match(%s:%s)",
                                  (info_type, type_name))
            return
        if info_md5sum != md5sum:
            self._rtcout.RTC_WARN("MD5sum in not match(%s:%s)",
                                  (info_md5sum, md5sum))
            return

        fileno = client_sock.fileno()
        poller = None
        if hasattr(select, 'poll'):
            ready = False
            poller = select.poll()
            poller.register(fileno, select.POLLOUT)
            while not ready:
                events = poller.poll()
                for _, flag in events:
                    if flag & select.POLLOUT:
                        ready = True
        else:
            ready = None
            while not ready:
                try:
                    _, ready, _ = select.select([], [fileno], [])
                except ValueError:
                    self._rtcout.RTC_ERROR("ValueError")
                    return
        client_sock.setblocking(1)
        fields = {
            'topic': topic_name,
            'message_definition': info_message_definition,
            'tcp_nodelay': '0',
            'md5sum': info_md5sum,
            'type': info_type,
            'callerid': self._callerid
        }

        try:
            write_ros_handshake_header(client_sock, fields)
        except rosgraph.network.ROSHandshakeException:
            self._rtcout.RTC_ERROR("write ROS handshake exception")
            return
        if poller:
            poller.unregister(fileno)

        self._tcp_connecters[addr] = {
            "socket": client_sock,
            "id": self._subnum,
            "node": header['callerid']
        }
        self._subnum += 1