update trigger control logic

xr_rm_teleop/xr_rm_teleop/fun_peripheral.py

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

xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py

@@ -129,7 +129,8 @@ 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._trigger_tool_open = True
+        self._last_trigger_pressed: 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
@@ -201,7 +202,7 @@ class SingleArmVelocityTeleop(Node):
             topic = f"{self._debug_topic_prefix}/{self._arm_name}/tool_enable"
         self.create_subscription(Bool, topic, self._on_tool_command, 10)
         trigger_text = (
-            f", trigger 需 grip=true 且 >= {self._trigger_close_threshold:.2f} 时闭合"
+            f", trigger >= {self._trigger_close_threshold:.2f} 上升沿切换开合"
             if self._enable_trigger_gripper_control
             else ""
         )
@@ -212,7 +213,7 @@ class SingleArmVelocityTeleop(Node):
 
     def _on_tool_command(self, msg: Bool) -> None:
         open_tool = bool(msg.data)
-        self._last_trigger_tool_open = None
+        self._trigger_tool_open = open_tool
         self._enqueue_tool_command(open_tool, "topic")
 
     def _start_tool_worker(self) -> None:
@@ -292,16 +293,19 @@ class SingleArmVelocityTeleop(Node):
     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:
+
+        trigger_pressed = msg.trigger >= self._trigger_close_threshold
+        if self._last_trigger_pressed is None:
+            self._last_trigger_pressed = trigger_pressed
             return
 
-        should_close = msg.trigger >= self._trigger_close_threshold
-        open_tool = not should_close
-        if self._last_trigger_tool_open == open_tool:
+        rising_edge = trigger_pressed and not self._last_trigger_pressed
+        self._last_trigger_pressed = trigger_pressed
+        if not rising_edge:
             return
 
-        self._last_trigger_tool_open = open_tool
-        self._enqueue_tool_command(open_tool, "trigger")
+        self._trigger_tool_open = not self._trigger_tool_open
+        self._enqueue_tool_command(self._trigger_tool_open, "trigger")
 
     def _control_tick(self) -> None:
         now = self.get_clock().now()