update trigger control logic

README.md

@@ -20,7 +20,7 @@ PICO/XR 双手柄 UDP JSON
 - PICO/XR 手柄 UDP 数据接收，并分发到左右手柄 ROS2 话题。
 - 通过统一的 `arm_debug.launch.py` 支持左臂、右臂、双臂的 mock 调试和真机调试。
 - RM75 真机连接适配，包含 `rm_movep_canfd` 位姿透传、安全速度/加速度配置、可选初始化点位移动。
-- 真机模式下，按住对应手柄 `grip` 时可用 `trigger` 控制对应夹爪：`trigger >= 0.95` 闭合，否则打开。
+- 真机模式下，点击对应手柄 `trigger` 可切换并保持对应夹爪开/关状态。
 - Tkinter 启动面板 `launcher_ui.py`，用于现场快速启动、监控 topic、检查环境和清理进程。
 - 自定义 PICO 4 Ultra UDP Sender Unity 工程，负责发送左右手柄 pose、`grip`、`trigger` 和 pose 诊断字段。
 
@@ -218,8 +218,8 @@ ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false \
 - `follow`：传给 `rm_movep_canfd` 的跟随标志，默认 `false`。
 - `configure_safety_limits`：连接真机后是否配置速度/加速度安全参数，默认 `true`。
 - `enable_tool_control`：是否在遥操作节点内启用末端工具控制 topic，默认 `true`。
-- `enable_trigger_gripper_control`：是否允许按住 `grip` 时用 `trigger` 控制对应夹爪，默认 `true`。
+- `enable_trigger_gripper_control`：是否允许用 `trigger` 点击切换对应夹爪状态，默认 `true`。
-- `trigger_close_threshold`：夹爪闭合阈值，默认 `0.95`；低于该值且 `grip=true` 时打开。
+- `trigger_close_threshold`：trigger 点击判定阈值，默认 `0.95`。
 - `configure_peripheral_on_connect`：遥操作节点连接真机后是否配置末端外设，默认 `true`；工具控制会复用同一个 RealMan 连接，避免两个进程同时抢占同一机械臂。
 - `move_to_initial_pose_on_connect`：连接后是否执行 `movej`/`movel` 初始化，默认 `false`。
 
@@ -260,7 +260,7 @@ ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false \
 
 ## 末端工具开合
 
-真机 launch 默认会在遥操作节点内启用工具控制。按住左/右手柄 `grip` 时，对应手柄 `trigger >= 0.95` 会闭合对应夹爪，`trigger < 0.95` 会打开对应夹爪；`grip=false` 时 trigger 不会改变夹爪状态。
+真机 launch 默认会在遥操作节点内启用工具控制。左/右手柄 `trigger` 从低于阈值按到 `>= 0.95` 时，会切换一次对应夹爪开/关状态，并保持到下一次点击。`grip` 仍只控制机械臂运动，不影响夹爪 trigger 切换。
 
 也可以用 Bool 话题手动控制开合，`true` 表示打开，`false` 表示闭合：
 
@@ -272,7 +272,7 @@ ros2 topic pub --once /xr_rm/right_rm75/tool_enable std_msgs/msg/Bool "{data: tr
 ros2 topic pub --once /xr_rm/right_rm75/tool_enable std_msgs/msg/Bool "{data: false}"
 ```
 
-桌面 UI 的 `Left Arm` 和 `Right Arm` 模式里也有对应的 Tool Open/Close 命令项；`Dual Arm` 真机模式下可直接通过左右手柄 `grip+trigger` 分别控制夹爪。
+桌面 UI 的 `Left Arm` 和 `Right Arm` 模式里也有对应的 Tool Open/Close 命令项；`Dual Arm` 真机模式下可直接通过左右手柄 `trigger` 分别切换夹爪。
 
 ## UDP 数据格式
 
@@ -333,7 +333,7 @@ ros2 topic pub --once /xr_rm/right_rm75/tool_enable std_msgs/msg/Bool "{data: fa
 - `seq`：Unity 端递增包序号，用于后续丢包分析。
 - `frame_id`：默认 `xr_world`，会写入 `XrController.header.frame_id`。
 - `grip`：运动使能。`true` 时进入相对位移控制，`false` 时停止。
-- `trigger`：扳机值，范围 `0.0-1.0`。真机模式下需要同时按住 `grip` 才会驱动对应夹爪，`>=0.95` 闭合，否则打开。
+- `trigger`：扳机值，范围 `0.0-1.0`。真机模式下跨过 `0.95` 的上升沿会切换对应夹爪开/关状态。
 - `pos`：手柄位置，长度 3。
 - `quat`：手柄姿态四元数，默认按 `xyzw` 解析。
 - `pose_valid`：姿态是否可信。ROS 接收端看到 `false` 会强制 `grip=false`。
@@ -354,8 +354,8 @@ PICO 4 Ultra 在 Ubuntu 22.04 下配置 Unity、构建 APK、安装到头显并
 3. 单臂启动，`move_to_initial_pose_on_connect:=false`。
 4. 手握急停，按住 `grip` 后只做小幅单轴移动。
 5. 逐个确认上/下、前/后、左/右方向。
-6. 保持 `grip=true`，轻按/松开对应 `trigger`，确认对应夹爪闭合/打开且左右不串臂。
+6. 点击对应 `trigger`，确认每次点击都会切换对应夹爪状态，松开 trigger 后状态保持且左右不串臂。
-7. 确认松开 `grip` 后机械臂慢停，`/xr_rm/<arm>/cmd_vel` 回到零；此时 trigger 不应再改变夹爪。
+7. 确认松开 `grip` 后机械臂慢停，`/xr_rm/<arm>/cmd_vel` 回到零；trigger 仍只影响夹爪，不影响机械臂运动门控。
 8. 左右臂都确认后，再进入双臂模式。
 
 当前项目没有双臂碰撞检测。双臂首次联调时，请让两个工作区在物理上分开，低速验证，不要让两臂末端互相靠近。

docs/pico_udp_sender_ubuntu22_setup.md

@@ -262,7 +262,7 @@ sudo ufw status
 - Trigger / A / X：编辑当前行或触发当前行
 - B / Y：保存并应用
 - Menu：在配置面板和运行 HUD 之间切换
-- Grip：ROS 侧遥操作使能；未按下时机械臂停止，且 trigger 不驱动夹爪
+- Grip：ROS 侧遥操作使能；未按下时机械臂停止
 
 验证现象：
 
@@ -271,7 +271,7 @@ sudo ufw status
 - HUD 显示包计数、发送端点、左右姿态状态和 KeepAwake 状态。
 - `pose_valid=false` 时，ROS 接收端会强制该手柄 `grip=false`，即使 PICO 端按下了 Grip。
 - 按住 `grip` 并移动手柄时，mock 模式下 `/xr_rm/*/target_pose` 应连续变化，`/xr_rm/*/cmd_vel` 会显示目标位姿变化率。
-- 真机模式下，必须按住对应手柄 `grip` 才能用 `trigger` 控制对应夹爪；`trigger >= 0.95` 闭合，否则打开。
+- 真机模式下，点击对应手柄 `trigger` 会切换并保持对应夹爪开/关状态。
 - 松开 `grip` 后，机械臂慢停，`cmd_vel` 应回到零。
 
 ## 9. ROS2 端验证
@@ -310,7 +310,7 @@ ros2 topic hz /xr/right_controller
 - 按左手 `grip` 时，`/xr/left_controller` 的 `grip` 变成 `true`。
 - 按右手 `grip` 时，`/xr/right_controller` 的 `grip` 变成 `true`。
 - 扳机从松开到按下时，`trigger` 从 `0.0` 附近变到 `1.0` 附近。
-- 单臂或双臂真机 launch 已启动时，只有 `grip=true` 的那只手柄会让对应夹爪响应 trigger。
+- 单臂或双臂真机 launch 已启动时，对应手柄每次点击 `trigger` 都会切换并保持对应夹爪状态。
 - 平移手柄时，`pose.position` 连续变化。
 
 如果收不到包，先抓 UDP：
@@ -428,8 +428,8 @@ Unity APK 每个周期发送一个双手柄 JSON 包：
 | `frame_id` | string | 默认 `xr_world` |
 | `controllers.left` | object | 左手柄 |
 | `controllers.right` | object | 右手柄 |
-| `grip` | bool | ROS 遥操作启停；也作为 trigger 夹爪控制使能 |
+| `grip` | bool | ROS 遥操作启停 |
-| `trigger` | float | `0.0-1.0`；真机模式下 `grip=true` 且 `trigger >= 0.95` 闭合夹爪，否则打开 |
+| `trigger` | float | `0.0-1.0`；真机模式下跨过 `0.95` 的上升沿切换夹爪开/关状态 |
 | `pos` | float[3] | `[x, y, z]` |
 | `quat` | float[4] | `[qx, qy, qz, qw]` |
 | `pose_valid` | bool | 姿态有效性；`false` 时 ROS receiver 强制 `grip=false` |
@@ -494,7 +494,7 @@ PXR `pxr_predict` 原始坐标按现场实测通常表现为右移 `native.z+`
 - 机械臂工作区清空。
 - PICO topic 在 mock 下已经稳定。
 - `grip=false` 时机械臂慢停，`/xr_rm/<arm>/cmd_vel` 为 0。
-- 夹爪验证时必须保持对应手柄 `grip=true`，再用 `trigger` 从松开到按下确认打开/闭合。
+- 夹爪验证时点击对应手柄 `trigger`，确认每次点击都会切换并保持开/关状态。
 - `move_to_initial_pose_on_connect` 保持 `false`。
 
 单臂真机：
@@ -523,7 +523,7 @@ ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false \
 | ROS2 收不到 UDP | 检查同一 Wi-Fi、Target IP、Target Port、UFW、`UDP Sending ON`、左右手柄是否 `valid pxr/unity` |
 | tcpdump 有包但 topic 没数据 | JSON 字段不对，确认包含 `controllers.left/right`，且 `pos` 长度 3、`quat` 长度 4 |
 | 有 topic 但机械臂不动 | 检查 `grip`、`pose_valid` 日志、teleop 节点订阅话题、工作空间限幅、UDP 超时 |
-| trigger 不控制夹爪 | 确认是真机 launch、`enable_tool_control=true`、已按住对应 `grip`，且 `trigger` 能到 0.95 以上 |
+| trigger 不控制夹爪 | 确认是真机 launch、`enable_tool_control=true`，且 `trigger` 能从低值跨到 0.95 以上 |
 | 按左手右臂动 | PICO 端 left/right 获取或填充反了 |
 | 松开 grip 后仍有速度 | 确认 PICO 持续发送 `grip=false`，并检查 teleop 超时停止 |
 | 经常超时 | 发送频率太低、网络丢包、PICO 应用后台暂停；提高 `sendHz` 并保持应用前台运行 |

xr_rm_bringup/launch/arm_debug.launch.py

@@ -272,7 +272,7 @@ def generate_launch_description() -> LaunchDescription:
         DeclareLaunchArgument("configure_safety_limits", default_value="true"),
         # 工具控制通过遥操作节点复用同一个 RealMan 连接，避免两个进程抢同一机械臂连接。
         DeclareLaunchArgument("enable_tool_control", default_value="true"),
-        # 按住 grip 时，trigger >= 阈值闭合夹爪，否则打开；grip=false 时不改变夹爪。
+        # trigger 上升沿切换夹爪开/关；grip 仍只控制机械臂运动。
         DeclareLaunchArgument("enable_trigger_gripper_control", default_value="true"),
         DeclareLaunchArgument("trigger_close_threshold", default_value="0.95"),
         # 连接成功后是否配置外设；关闭后仅订阅开合话题，但开合前需要另行完成外设配置。

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
+        rising_edge = trigger_pressed and not self._last_trigger_pressed
-        open_tool = not should_close
+        self._last_trigger_pressed = trigger_pressed
-        if self._last_trigger_tool_open == open_tool:
+        if not rising_edge:
             return
 
-        self._last_trigger_tool_open = open_tool
+        self._trigger_tool_open = not self._trigger_tool_open
-        self._enqueue_tool_command(open_tool, "trigger")
+        self._enqueue_tool_command(self._trigger_tool_open, "trigger")
 
     def _control_tick(self) -> None:
         now = self.get_clock().now()