Enhanced gripper control functionality

xr_rm_teleop/xr_rm_teleop/fun_peripheral.py

@@ -5,7 +5,8 @@ scissorgripper 参数约定：
 1：Modbus 控制的电动夹爪
 2：控制器 DO3/DO4 控制的小型剪刀夹爪
 
-当前 XR 工程只在真机连接阶段做外设初始化；主遥操作流程不调用开合命令。
+当前 XR 工程在真机连接阶段初始化外设，遥操作节点随后复用同一个
+RealMan 连接执行手动 topic 或 grip-gated trigger 夹爪开合命令。
 """
 
 from __future__ import annotations

xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py

@@ -7,6 +7,8 @@
 from __future__ import annotations
 
 import math
+import queue
+import threading
 from typing import Iterable
 
 import rclpy
@@ -76,6 +78,8 @@ class SingleArmVelocityTeleop(Node):
         self.declare_parameter("canfd_trajectory_mode", 2)
         self.declare_parameter("canfd_radio", 0)
         self.declare_parameter("enable_tool_control", True)
+        self.declare_parameter("enable_trigger_gripper_control", True)
+        self.declare_parameter("trigger_close_threshold", 0.95)
         self.declare_parameter("configure_peripheral_on_connect", True)
         self.declare_parameter("peripheral_config_file", "")
         self.declare_parameter("peripheral_arm", "")
@@ -106,6 +110,9 @@ class SingleArmVelocityTeleop(Node):
         self._xr_to_robot_matrix = self._float_list_parameter("xr_to_robot_matrix", 9)
         self._current_pose_poll_hz = float(self.get_parameter("current_pose_poll_hz").value)
         self._follow = self._bool_parameter("follow")
+        self._enable_tool_control = self._bool_parameter("enable_tool_control")
+        self._enable_trigger_gripper_control = self._bool_parameter("enable_trigger_gripper_control")
+        self._trigger_close_threshold = float(self.get_parameter("trigger_close_threshold").value)
         self._debug_topic_prefix = str(self.get_parameter("debug_topic_prefix").value).rstrip("/")
         if not self._debug_topic_prefix:
             self._debug_topic_prefix = "/xr_rm"
@@ -122,6 +129,10 @@ class SingleArmVelocityTeleop(Node):
         self._last_current_pose: ArmPose | None = None
         self._last_current_pose_time: Time | None = None
         self._stop_sent = True
+        self._last_trigger_tool_open: bool | None = None
+        self._tool_command_queue: queue.Queue[tuple[bool, str] | None] | None = None
+        self._tool_worker_stop = threading.Event()
+        self._tool_worker_thread: threading.Thread | None = None
 
         self._adapter = self._make_adapter()
         self._adapter.connect()
@@ -171,7 +182,11 @@ class SingleArmVelocityTeleop(Node):
         )
 
     def _setup_tool_control(self) -> None:
-        if not self._bool_parameter("enable_tool_control"):
+        if not self._enable_tool_control:
+            if self._enable_trigger_gripper_control:
+                self.get_logger().warn(
+                    f"{self._arm_name} trigger 夹爪控制已请求，但 enable_tool_control=false，已禁用。"
+                )
             return
 
         peripheral_arm = self._peripheral_arm_name()
@@ -179,21 +194,84 @@ class SingleArmVelocityTeleop(Node):
         if self._bool_parameter("configure_peripheral_on_connect"):
             self._adapter.configure_peripheral(config_file, peripheral_arm)
 
+        self._start_tool_worker()
+
         topic = str(self.get_parameter("tool_command_topic").value).strip()
         if not topic:
             topic = f"{self._debug_topic_prefix}/{self._arm_name}/tool_enable"
         self.create_subscription(Bool, topic, self._on_tool_command, 10)
-        self.get_logger().info(f"{self._arm_name} 工具控制已启用，监听：{topic} (true=open, false=close)")
+        trigger_text = (
+            f", trigger 需 grip=true 且 >= {self._trigger_close_threshold:.2f} 时闭合"
+            if self._enable_trigger_gripper_control
+            else ""
+        )
+        self.get_logger().info(
+            f"{self._arm_name} 工具控制已启用，监听：{topic} "
+            f"(true=open, false=close{trigger_text})"
+        )
 
     def _on_tool_command(self, msg: Bool) -> None:
         open_tool = bool(msg.data)
-        action = "open" if open_tool else "close"
-        try:
-            self._adapter.set_tool_enabled(open_tool)
-        except Exception as exc:
-            self.get_logger().error(f"{self._arm_name} tool {action} failed: {exc}")
+        self._last_trigger_tool_open = None
+        self._enqueue_tool_command(open_tool, "topic")
+
+    def _start_tool_worker(self) -> None:
+        if self._tool_worker_thread is not None:
             return
-        self.get_logger().info(f"{self._arm_name} tool {action} command sent")
+        self._tool_command_queue = queue.Queue(maxsize=1)
+        self._tool_worker_stop.clear()
+        self._tool_worker_thread = threading.Thread(
+            target=self._tool_worker_loop,
+            name=f"{self._arm_name}_tool_worker",
+            daemon=True,
+        )
+        self._tool_worker_thread.start()
+
+    def _enqueue_tool_command(self, open_tool: bool, source: str) -> None:
+        if self._tool_command_queue is None:
+            self.get_logger().warn(
+                f"{self._arm_name} 工具控制未启用，忽略 {source} 夹爪命令。",
+                throttle_duration_sec=1.0,
+            )
+            return
+
+        item = (open_tool, source)
+        while True:
+            try:
+                self._tool_command_queue.put_nowait(item)
+                return
+            except queue.Full:
+                try:
+                    self._tool_command_queue.get_nowait()
+                    self._tool_command_queue.task_done()
+                except queue.Empty:
+                    continue
+
+    def _tool_worker_loop(self) -> None:
+        assert self._tool_command_queue is not None
+        while not self._tool_worker_stop.is_set():
+            try:
+                item = self._tool_command_queue.get(timeout=0.1)
+            except queue.Empty:
+                continue
+
+            try:
+                if item is None:
+                    return
+                open_tool, source = item
+                action = "open" if open_tool else "close"
+                try:
+                    self._adapter.set_tool_enabled(open_tool)
+                except Exception as exc:
+                    self.get_logger().error(
+                        f"{self._arm_name} tool {action} failed from {source}: {exc}"
+                    )
+                    continue
+                self.get_logger().info(
+                    f"{self._arm_name} tool {action} command sent from {source}"
+                )
+            finally:
+                self._tool_command_queue.task_done()
 
     def _peripheral_arm_name(self) -> str:
         configured = str(self.get_parameter("peripheral_arm").value).strip().lower()
@@ -209,6 +287,21 @@ class SingleArmVelocityTeleop(Node):
     def _on_controller(self, msg: XrController) -> None:
         self._last_msg = msg
         self._last_msg_time = self.get_clock().now()
+        self._handle_trigger_gripper(msg)
+
+    def _handle_trigger_gripper(self, msg: XrController) -> None:
+        if not self._enable_tool_control or not self._enable_trigger_gripper_control:
+            return
+        if not msg.grip:
+            return
+
+        should_close = msg.trigger >= self._trigger_close_threshold
+        open_tool = not should_close
+        if self._last_trigger_tool_open == open_tool:
+            return
+
+        self._last_trigger_tool_open = open_tool
+        self._enqueue_tool_command(open_tool, "trigger")
 
     def _control_tick(self) -> None:
         now = self.get_clock().now()
@@ -599,6 +692,8 @@ class SingleArmVelocityTeleop(Node):
             raise ValueError("max_linear_speed must be > 0")
         if self._current_pose_poll_hz < 0.0:
             raise ValueError("current_pose_poll_hz must be >= 0")
+        if not 0.0 <= self._trigger_close_threshold <= 1.0:
+            raise ValueError("trigger_close_threshold must be in [0, 1]")
         for axis, (low, high) in enumerate(zip(self._workspace_min, self._workspace_max)):
             if low >= high:
                 raise ValueError(f"workspace_min[{axis}] must be smaller than workspace_max[{axis}]")
@@ -610,7 +705,28 @@ class SingleArmVelocityTeleop(Node):
         if self._low_z_min_radius < 0.0:
             raise ValueError("low_z_min_radius must be >= 0")
 
+    def _shutdown_tool_worker(self) -> None:
+        if self._tool_worker_thread is None or self._tool_command_queue is None:
+            return
+        self._tool_worker_stop.set()
+        while True:
+            try:
+                self._tool_command_queue.put_nowait(None)
+                break
+            except queue.Full:
+                try:
+                    self._tool_command_queue.get_nowait()
+                    self._tool_command_queue.task_done()
+                except queue.Empty:
+                    continue
+        self._tool_worker_thread.join(timeout=3.0)
+        if self._tool_worker_thread.is_alive():
+            self.get_logger().warn(f"{self._arm_name} 工具控制后台线程未在超时时间内退出。")
+        self._tool_worker_thread = None
+        self._tool_command_queue = None
+
     def destroy_node(self) -> bool:
+        self._shutdown_tool_worker()
         self._adapter.close()
         return super().destroy_node()