Enhanced gripper control functionality

README.md

@@ -20,6 +20,7 @@ PICO/XR 双手柄 UDP JSON
 - PICO/XR 手柄 UDP 数据接收，并分发到左右手柄 ROS2 话题。
 - 通过统一的 `arm_debug.launch.py` 支持左臂、右臂、双臂的 mock 调试和真机调试。
 - RM75 真机连接适配，包含 `rm_movep_canfd` 位姿透传、安全速度/加速度配置、可选初始化点位移动。
+- 真机模式下，按住对应手柄 `grip` 时可用 `trigger` 控制对应夹爪：`trigger >= 0.95` 闭合，否则打开。
 - Tkinter 启动面板 `launcher_ui.py`，用于现场快速启动、监控 topic、检查环境和清理进程。
 - 自定义 PICO 4 Ultra UDP Sender Unity 工程，负责发送左右手柄 pose、`grip`、`trigger` 和 pose 诊断字段。
 
@@ -217,6 +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`。
+- `trigger_close_threshold`：夹爪闭合阈值，默认 `0.95`；低于该值且 `grip=true` 时打开。
 - `configure_peripheral_on_connect`：遥操作节点连接真机后是否配置末端外设，默认 `true`；工具控制会复用同一个 RealMan 连接，避免两个进程同时抢占同一机械臂。
 - `move_to_initial_pose_on_connect`：连接后是否执行 `movej`/`movel` 初始化，默认 `false`。
 
@@ -229,7 +232,7 @@ ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false \
 
 `left_arm_rm75.yaml` 和 `right_arm_rm75.yaml` 用于 `arm_debug.launch.py arm:=left/right` 的单臂调试，因为单臂节点名是 `single_arm_velocity_teleop`。
 
-`xr_rm_bringup/config/peripherals_rm75.yaml` 保存末端工具坐标、负载和左右臂外设选择。当前配置为左臂 `scissorgripper=1`、右臂 `scissorgripper=2`，并且只在真机连接阶段初始化外设，不在主遥操作循环里控制开合。
+`xr_rm_bringup/config/peripherals_rm75.yaml` 保存末端工具坐标、负载和左右臂外设选择。当前配置为左臂 `scissorgripper=2`、右臂 `scissorgripper=1`，真机连接阶段会初始化外设，后续开合命令复用同一个 RealMan 连接。
 
 重点控制参数：
 
@@ -257,7 +260,9 @@ ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false \
 
 ## 末端工具开合
 
-真机 launch 默认会在遥操作节点内启用工具控制。启动后可以用 Bool 话题控制开合，`true` 表示打开，`false` 表示闭合：
+真机 launch 默认会在遥操作节点内启用工具控制。按住左/右手柄 `grip` 时，对应手柄 `trigger >= 0.95` 会闭合对应夹爪，`trigger < 0.95` 会打开对应夹爪；`grip=false` 时 trigger 不会改变夹爪状态。
+
+也可以用 Bool 话题手动控制开合，`true` 表示打开，`false` 表示闭合：
 
 ```bash
 ros2 topic pub --once /xr_rm/left_rm75/tool_enable std_msgs/msg/Bool "{data: true}"
@@ -267,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` 和 `Dual Arm` 模式里也有对应的 Tool Open/Close 命令项。
+桌面 UI 的 `Left Arm` 和 `Right Arm` 模式里也有对应的 Tool Open/Close 命令项；`Dual Arm` 真机模式下可直接通过左右手柄 `grip+trigger` 分别控制夹爪。
 
 ## UDP 数据格式
 
@@ -328,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`。当前控制主链路不使用它，预留给后续夹爪集成。
+- `trigger`：扳机值，范围 `0.0-1.0`。真机模式下需要同时按住 `grip` 才会驱动对应夹爪，`>=0.95` 闭合，否则打开。
 - `pos`：手柄位置，长度 3。
 - `quat`：手柄姿态四元数，默认按 `xyzw` 解析。
 - `pose_valid`：姿态是否可信。ROS 接收端看到 `false` 会强制 `grip=false`。
@@ -349,8 +354,9 @@ PICO 4 Ultra 在 Ubuntu 22.04 下配置 Unity、构建 APK、安装到头显并
 3. 单臂启动，`move_to_initial_pose_on_connect:=false`。
 4. 手握急停，按住 `grip` 后只做小幅单轴移动。
 5. 逐个确认上/下、前/后、左/右方向。
-6. 确认松开 `grip` 后机械臂慢停，`/xr_rm/<arm>/cmd_vel` 回到零。
+6. 保持 `grip=true`，轻按/松开对应 `trigger`，确认对应夹爪闭合/打开且左右不串臂。
-7. 左右臂都确认后，再进入双臂模式。
+7. 确认松开 `grip` 后机械臂慢停，`/xr_rm/<arm>/cmd_vel` 回到零；此时 trigger 不应再改变夹爪。
+8. 左右臂都确认后，再进入双臂模式。
 
 当前项目没有双臂碰撞检测。双臂首次联调时，请让两个工作区在物理上分开，低速验证，不要让两臂末端互相靠近。
 
@@ -360,7 +366,7 @@ PICO 4 Ultra 在 Ubuntu 22.04 下配置 Unity、构建 APK、安装到头显并
 
 1. 稳定 PICO 数据链路：利用 `seq`、`source_time`、`pose_valid` 做频率、延迟、丢包和追踪状态统计，记录 `/xr/*_controller`、`/xr_rm/*/raw_target_pose`、`/xr_rm/*/target_pose`、`/xr_rm/*/target_clamped`、`/xr_rm/*/current_pose`。
 2. 提升真机安全性：增加启动前安全检查、软件急停 topic、UI Stop 状态提示、双臂中间区域互斥边界和速度/加速度限幅。
-3. 集成末端执行器：明确夹爪 topic、力控比例、开合方向和安全上限，把 `trigger` 从预留字段变成稳定夹爪输入。
+3. 细化末端执行器：增加夹爪状态反馈、力控比例、安全上限和现场可视化提示。
 4. 接入视觉和数据记录：加入 D405/D435 相机 launch、TF、内外参和 rosbag2 实验记录。
 5. 从遥操作走向半自动：先做目标检测和 3D 定位提示，再做单臂辅助，最后做双臂任务分配和任务级状态机。
 

docs/pico_udp_sender_ubuntu22_setup.md

@@ -262,7 +262,7 @@ sudo ufw status
 - Trigger / A / X：编辑当前行或触发当前行
 - B / Y：保存并应用
 - Menu：在配置面板和运行 HUD 之间切换
-- Grip：ROS 侧遥操作使能；未按下时机械臂停止
+- Grip：ROS 侧遥操作使能；未按下时机械臂停止，且 trigger 不驱动夹爪
 
 验证现象：
 
@@ -271,6 +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` 闭合，否则打开。
 - 松开 `grip` 后，机械臂慢停，`cmd_vel` 应回到零。
 
 ## 9. ROS2 端验证
@@ -309,6 +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。
 - 平移手柄时，`pose.position` 连续变化。
 
 如果收不到包，先抓 UDP：
@@ -426,8 +428,8 @@ Unity APK 每个周期发送一个双手柄 JSON 包：
 | `frame_id` | string | 默认 `xr_world` |
 | `controllers.left` | object | 左手柄 |
 | `controllers.right` | object | 右手柄 |
-| `grip` | bool | ROS 遥操作启停 |
+| `grip` | bool | ROS 遥操作启停；也作为 trigger 夹爪控制使能 |
-| `trigger` | float | `0.0-1.0` |
+| `trigger` | float | `0.0-1.0`；真机模式下 `grip=true` 且 `trigger >= 0.95` 闭合夹爪，否则打开 |
 | `pos` | float[3] | `[x, y, z]` |
 | `quat` | float[4] | `[qx, qy, qz, qw]` |
 | `pose_valid` | bool | 姿态有效性；`false` 时 ROS receiver 强制 `grip=false` |
@@ -449,6 +451,7 @@ project.z = -native.x;
 这条转换只用于 `pose_source=pxr_predict` 且 `Coordinates=Project (+Z back)` 的情况。`Source raw` 模式保留 PXR native 或 Unity XR fallback 的原始坐标，便于现场对照；`unity_xr` fallback 暂不做 PXR native 转换，避免把已经由 Unity XR 提供的坐标二次转换。
 
 禁用发送、App 暂停或退出时，必须额外发送一次左右手柄 `grip=false`，让 ROS 侧安全停止。
+ROS 侧收到 `grip=false` 时不会主动打开或闭合夹爪，夹爪保持上一次状态。
 
 `udp_controller_receiver` 仍兼容调试脚本的旧格式：单个 object 可以直接带 `hand`、`pos`、`quat`；`controllers` 可以是 object 或 list；位置字段也支持 `position`、`p`、`pose.position`，姿态字段支持 `orientation`、`q`。四元数默认按 `xyzw` 解析，也可用 `quat_order:=wxyz` 覆盖。
 
@@ -491,6 +494,7 @@ PXR `pxr_predict` 原始坐标按现场实测通常表现为右移 `native.z+`
 - 机械臂工作区清空。
 - PICO topic 在 mock 下已经稳定。
 - `grip=false` 时机械臂慢停，`/xr_rm/<arm>/cmd_vel` 为 0。
+- 夹爪验证时必须保持对应手柄 `grip=true`，再用 `trigger` 从松开到按下确认打开/闭合。
 - `move_to_initial_pose_on_connect` 保持 `false`。
 
 单臂真机：
@@ -519,6 +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 以上 |
 | 按左手右臂动 | PICO 端 left/right 获取或填充反了 |
 | 松开 grip 后仍有速度 | 确认 PICO 持续发送 `grip=false`，并检查 teleop 超时停止 |
 | 经常超时 | 发送频率太低、网络丢包、PICO 应用后台暂停；提高 `sendHz` 并保持应用前台运行 |

xr_rm_bringup/config/dual_arm_rm75.yaml

@@ -4,7 +4,7 @@
 # 按下 grip 时锁定当前手柄位姿和 TCP 位姿，之后将手柄相对位移映射为目标
 # TCP 位姿，经过工作空间限幅、目标低通和单帧步长限制后，通过 rm_movep_canfd
 # 下发。cmd_vel 仅作为目标位姿变化率调试话题，不是机械臂执行命令。
-# 末端外设由 peripherals_rm75.yaml 配置，launch 只在真机连接阶段初始化。
+# 末端外设由 peripherals_rm75.yaml 配置，真机连接阶段初始化后由遥操作节点复用。
 
 left_arm_teleop:
   ros__parameters:

xr_rm_bringup/config/left_arm_rm75.yaml

@@ -1,5 +1,5 @@
 # 左臂单独调试配置：XR 相对位姿透传控制 RM75 TCP。
-# 末端外设由 peripherals_rm75.yaml 配置，launch 只在真机连接阶段初始化。
+# 末端外设由 peripherals_rm75.yaml 配置，真机连接阶段初始化后由遥操作节点复用。
 
 single_arm_velocity_teleop:
   ros__parameters:

xr_rm_bringup/config/peripherals_rm75.yaml

@@ -2,7 +2,7 @@
 #
 # scissorgripper 与 tools_in_ee 的顺序保持和 acRealman 一致：
 # 0 -> scissor，1 -> omnipic，2 -> minisci，-1 -> no_tool。
-# 当前阶段只做连接后的外设初始化，不在遥操作主循环中控制开合。
+# 当前阶段在连接后初始化外设，开合命令由遥操作节点复用同一个 RealMan 连接执行。
 
 set_initial_tool_state: false
 

xr_rm_bringup/config/right_arm_rm75.yaml

@@ -1,5 +1,5 @@
 # 右臂单独调试配置：XR 相对位姿透传控制 RM75 TCP。
-# 末端外设由 peripherals_rm75.yaml 配置，launch 只在真机连接阶段初始化。
+# 末端外设由 peripherals_rm75.yaml 配置，真机连接阶段初始化后由遥操作节点复用。
 
 single_arm_velocity_teleop:
   ros__parameters:
@@ -13,7 +13,7 @@ single_arm_velocity_teleop:
     target_filter_alpha: 0.65
     target_filter_alpha_fast: 0.9
     target_filter_fast_threshold_m: 0.03
-    max_linear_speed: 0.3
+    max_linear_speed: 0.15
     enable_position_axes: [true, true, true]
     current_pose_poll_hz: 10.0
 

xr_rm_bringup/launch/arm_debug.launch.py

@@ -53,6 +53,8 @@ def _single_arm_node(
     follow: bool,
     configure_safety_limits: bool,
     enable_tool_control: bool,
+    enable_trigger_gripper_control: bool,
+    trigger_close_threshold: float,
     configure_peripheral_on_connect: bool,
 ) -> Node:
     """创建单臂调试节点；左/右臂分别使用独立 YAML，节点名保持单臂默认名。"""
@@ -77,6 +79,8 @@ def _single_arm_node(
                 "configure_safety_limits": configure_safety_limits,
                 "move_to_initial_pose_on_connect": move_to_initial_pose,
                 "enable_tool_control": enable_tool_control,
+                "enable_trigger_gripper_control": enable_trigger_gripper_control,
+                "trigger_close_threshold": trigger_close_threshold,
                 "configure_peripheral_on_connect": configure_peripheral_on_connect,
                 "peripheral_config_file": _config_file("peripherals_rm75.yaml"),
                 "peripheral_arm": arm,
@@ -100,6 +104,8 @@ def _dual_arm_nodes(
     follow: bool,
     configure_safety_limits: bool,
     enable_tool_control: bool,
+    enable_trigger_gripper_control: bool,
+    trigger_close_threshold: float,
     configure_peripheral_on_connect: bool,
 ) -> list[Node]:
     """创建双臂节点；两个节点共用双臂 YAML，但节点名区分左右臂参数命名空间。"""
@@ -123,6 +129,8 @@ def _dual_arm_nodes(
                     "configure_safety_limits": configure_safety_limits,
                     "move_to_initial_pose_on_connect": move_to_initial_pose,
                     "enable_tool_control": enable_tool_control,
+                    "enable_trigger_gripper_control": enable_trigger_gripper_control,
+                    "trigger_close_threshold": trigger_close_threshold,
                     "configure_peripheral_on_connect": configure_peripheral_on_connect,
                     "peripheral_config_file": _config_file("peripherals_rm75.yaml"),
                     "peripheral_arm": "left",
@@ -148,6 +156,8 @@ def _dual_arm_nodes(
                     "configure_safety_limits": configure_safety_limits,
                     "move_to_initial_pose_on_connect": move_to_initial_pose,
                     "enable_tool_control": enable_tool_control,
+                    "enable_trigger_gripper_control": enable_trigger_gripper_control,
+                    "trigger_close_threshold": trigger_close_threshold,
                     "configure_peripheral_on_connect": configure_peripheral_on_connect,
                     "peripheral_config_file": _config_file("peripherals_rm75.yaml"),
                     "peripheral_arm": "right",
@@ -185,6 +195,12 @@ def _launch_setup(context, *args, **kwargs):
     enable_tool_control = _as_bool(
         LaunchConfiguration("enable_tool_control").perform(context)
     )
+    enable_trigger_gripper_control = _as_bool(
+        LaunchConfiguration("enable_trigger_gripper_control").perform(context)
+    )
+    trigger_close_threshold = float(
+        LaunchConfiguration("trigger_close_threshold").perform(context)
+    )
 
     if arm not in ("left", "right", "both"):
         raise ValueError("arm must be one of: left, right, both")
@@ -202,6 +218,8 @@ def _launch_setup(context, *args, **kwargs):
                 follow,
                 configure_safety_limits,
                 enable_tool_control,
+                enable_trigger_gripper_control,
+                trigger_close_threshold,
                 configure_peripheral_on_connect,
             )
         )
@@ -218,6 +236,8 @@ def _launch_setup(context, *args, **kwargs):
                 follow,
                 configure_safety_limits,
                 enable_tool_control,
+                enable_trigger_gripper_control,
+                trigger_close_threshold,
                 configure_peripheral_on_connect,
             )
         )
@@ -252,6 +272,9 @@ def generate_launch_description() -> LaunchDescription:
         DeclareLaunchArgument("configure_safety_limits", default_value="true"),
         # 工具控制通过遥操作节点复用同一个 RealMan 连接，避免两个进程抢同一机械臂连接。
         DeclareLaunchArgument("enable_tool_control", default_value="true"),
+        # 按住 grip 时，trigger >= 阈值闭合夹爪，否则打开；grip=false 时不改变夹爪。
+        DeclareLaunchArgument("enable_trigger_gripper_control", default_value="true"),
+        DeclareLaunchArgument("trigger_close_threshold", default_value="0.95"),
         # 连接成功后是否配置外设；关闭后仅订阅开合话题，但开合前需要另行完成外设配置。
         DeclareLaunchArgument("configure_peripheral_on_connect", default_value="true"),
         # 默认不自动移动到初始点，确认安全区后再显式打开。

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)
+        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._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: {exc}")
+                    self.get_logger().error(
-            return
+                        f"{self._arm_name} tool {action} failed from {source}: {exc}"
-        self.get_logger().info(f"{self._arm_name} tool {action} command sent")
+                    )
+                    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()