diff --git a/README.md b/README.md index 72f7d46..4ed320a 100755 --- a/README.md +++ b/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//cmd_vel` 回到零。 -7. 左右臂都确认后，再进入双臂模式。 +6. 保持 `grip=true`，轻按/松开对应 `trigger`，确认对应夹爪闭合/打开且左右不串臂。 +7. 确认松开 `grip` 后机械臂慢停，`/xr_rm//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 定位提示，再做单臂辅助，最后做双臂任务分配和任务级状态机。 diff --git a/docs/pico_udp_sender_ubuntu22_setup.md b/docs/pico_udp_sender_ubuntu22_setup.md index 8e094b5..c3ef7cc 100644 --- a/docs/pico_udp_sender_ubuntu22_setup.md +++ b/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 遥操作启停 | -| `trigger` | float | `0.0-1.0` | +| `grip` | bool | ROS 遥操作启停；也作为 trigger 夹爪控制使能 | +| `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//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` 并保持应用前台运行 | diff --git a/docs/xr_rm75_midterm_report.docx b/docs/xr_rm75_midterm_report.docx index 5af8227..9ce15cd 100644 Binary files a/docs/xr_rm75_midterm_report.docx and b/docs/xr_rm75_midterm_report.docx differ diff --git a/docs/xr_rm75_progress_report.pptx b/docs/xr_rm75_progress_report.pptx index f2993c9..9064122 100644 Binary files a/docs/xr_rm75_progress_report.pptx and b/docs/xr_rm75_progress_report.pptx differ diff --git a/xr_rm_bringup/config/dual_arm_rm75.yaml b/xr_rm_bringup/config/dual_arm_rm75.yaml index 5ea3879..254a6f2 100755 --- a/xr_rm_bringup/config/dual_arm_rm75.yaml +++ b/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: diff --git a/xr_rm_bringup/config/left_arm_rm75.yaml b/xr_rm_bringup/config/left_arm_rm75.yaml index c37dabf..8f3a4c6 100644 --- a/xr_rm_bringup/config/left_arm_rm75.yaml +++ b/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: diff --git a/xr_rm_bringup/config/peripherals_rm75.yaml b/xr_rm_bringup/config/peripherals_rm75.yaml index 59f9eaa..ac42c8d 100644 --- a/xr_rm_bringup/config/peripherals_rm75.yaml +++ b/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 diff --git a/xr_rm_bringup/config/right_arm_rm75.yaml b/xr_rm_bringup/config/right_arm_rm75.yaml index 1b80670..d7eafac 100644 --- a/xr_rm_bringup/config/right_arm_rm75.yaml +++ b/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 diff --git a/xr_rm_bringup/launch/arm_debug.launch.py b/xr_rm_bringup/launch/arm_debug.launch.py index e60cb31..a99b3ce 100644 --- a/xr_rm_bringup/launch/arm_debug.launch.py +++ b/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"), # 默认不自动移动到初始点，确认安全区后再显式打开。 diff --git a/xr_rm_teleop/xr_rm_teleop/fun_peripheral.py b/xr_rm_teleop/xr_rm_teleop/fun_peripheral.py index 2462fd9..6ce1d1e 100644 --- a/xr_rm_teleop/xr_rm_teleop/fun_peripheral.py +++ b/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 diff --git a/xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py b/xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py index af13548..7863a97 100755 --- a/xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py +++ b/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()