feat: 长保持连接，控制端可重启

ros-control-py/udp_teleop_bridge/udp_teleop_bridge/udp_cmd_vel_receiver.py

@@ -81,12 +81,7 @@ class UdpCmdVelReceiver(Node):
 
             self._msg_type_binary = MSG_TYPE_BINARY
             self._msg_type_error = MSG_TYPE_ERROR
-            self._transport = OmniTransport(
-                transport=self._transport_name,
-                server_addr=self._server_addr,
-                relay_via=self._relay_via,
-                peer_id=self._peer_id,
-            )
+            self._transport = self._create_transport()
 
         self._lock = threading.Lock()
         self._last_log_times: Dict[str, float] = {}
@@ -151,6 +146,61 @@ class UdpCmdVelReceiver(Node):
         self._unix_socket.bind(self._local_socket_path)
         self._unix_socket.settimeout(0.1)
 
+    def _close_unix_socket(self) -> None:
+        if self._unix_socket is not None:
+            try:
+                self._unix_socket.close()
+            except OSError:
+                pass
+            self._unix_socket = None
+
+    def _create_transport(self):
+        from .omni_transport import OmniTransport
+
+        return OmniTransport(
+            transport=self._transport_name,
+            server_addr=self._server_addr,
+            relay_via=self._relay_via,
+            peer_id=self._peer_id,
+        )
+
+    def _reconnect_transport(self) -> bool:
+        while not self._closing.is_set() and rclpy.ok():
+            current_transport = self._transport
+            if current_transport is not None:
+                try:
+                    current_transport.close()
+                except OSError:
+                    pass
+            try:
+                self._transport = self._create_transport()
+                if self._should_log('transport_reconnected', 1.0):
+                    self.get_logger().info(
+                        'Reconnected OmniSocket transport %s://%s as %s'
+                        % (self._transport_name, self._server_addr, self._peer_id)
+                    )
+                return True
+            except OSError as exc:
+                self._transport = None
+                if self._should_log('transport_reconnect_error', 2.0):
+                    self.get_logger().error(f'Failed to reconnect OmniSocket transport: {exc}')
+                time.sleep(0.5)
+        return False
+
+    def _rebind_unix_socket(self) -> bool:
+        while not self._closing.is_set() and rclpy.ok():
+            self._close_unix_socket()
+            try:
+                self._setup_unix_socket()
+                if self._should_log('unix_rebound', 1.0):
+                    self.get_logger().info(f'Rebound unix datagram socket at {self._local_socket_path}')
+                return True
+            except OSError as exc:
+                if self._should_log('unix_rebind_error', 2.0):
+                    self.get_logger().error(f'Failed to rebind unix datagram socket: {exc}')
+                time.sleep(0.5)
+        return False
+
     def _should_log(self, key: str, throttle_sec: float) -> bool:
         now = time.monotonic()
         previous = self._last_log_times.get(key)
@@ -189,7 +239,9 @@ class UdpCmdVelReceiver(Node):
             except OSError as exc:
                 if not self._closing.is_set() and self._should_log('recv_error', 2.0):
                     self.get_logger().error(f'OmniSocket receive loop stopped: {exc}')
-                return
+                if not self._reconnect_transport():
+                    return
+                continue
 
             if meta is None:
                 continue
@@ -244,11 +296,20 @@ class UdpCmdVelReceiver(Node):
             try:
                 payload = self._unix_socket.recv(DEFAULT_RECV_BUFFER_BYTES)
             except socket.timeout:
+                if not os.path.exists(self._local_socket_path):
+                    if self._should_log('unix_socket_missing', 2.0):
+                        self.get_logger().warning(
+                            f'Unix datagram socket path disappeared, rebinding {self._local_socket_path}'
+                        )
+                    if not self._rebind_unix_socket():
+                        return
                 continue
             except OSError as exc:
                 if not self._closing.is_set() and self._should_log('unix_recv_error', 2.0):
                     self.get_logger().error(f'Unix datagram receive loop stopped: {exc}')
-                return
+                if not self._rebind_unix_socket():
+                    return
+                continue
 
             if len(payload) != PACKET_SIZE:
                 if self._should_log('unix_packet_size', 2.0):
@@ -305,11 +366,10 @@ class UdpCmdVelReceiver(Node):
             self._transport = None
         if self._unix_socket is not None:
             try:
-                self._unix_socket.close()
+                self._close_unix_socket()
             except OSError as exc:
                 if self._should_log('unix_close_error', 2.0):
                     self.get_logger().warning(f'Closing unix socket failed: {exc}')
-            self._unix_socket = None
             try:
                 os.unlink(self._local_socket_path)
             except FileNotFoundError: