feat: Enhance single arm velocity teleoperation with zero velocity handling and new parameters

README.md

@@ -112,9 +112,10 @@ ros2 run xr_rm_bringup launcher_ui
 - `Check Env`：检查 ROS2 Humble、工作空间 build、终端、核心 ROS 包、睿尔曼 API2。
 - `Stop All`：结束由本工作空间启动的 launch、sample sender、topic monitor、相关 ROS 节点和终端窗口。
 
-每个模式都会附带两个监控入口：
+每个模式都会附带三个监控入口：
 
 - `Open Controller Topic Monitor`：用 Terminator 分屏同时查看 `/xr/left_controller` 和 `/xr/right_controller`。
+- `Open Cmd Vel Topic Monitor`：用 Terminator 分屏同时查看 `/xr_rm/left_rm75/cmd_vel` 和 `/xr_rm/right_rm75/cmd_vel`。
 - `Open ROS Topic/Node List Monitor`：用 Terminator 分屏每秒刷新 `ros2 topic list` 和 `ros2 node list`。
 
 分屏监控依赖 `x-terminal-emulator` 指向 Terminator。若提示不支持，可安装并切换：

docs/pico4_ultra_udp_teleop_setup.md

@@ -0,0 +1,490 @@
+# PICO 4 Ultra UDP 手柄接入与调试教程
+
+本文档用于把 PICO 4 Ultra 左右手柄接入当前 XR-RM75 ROS2 工作空间。目标是让 PICO 端发送符合本项目约定的 UDP JSON，ROS 端发布：
+
+```text
+/xr/left_controller
+/xr/right_controller
+```
+
+然后由 `single_arm_velocity_teleop` 把手柄相对位移转换成 RM75 TCP 运动。
+
+## 1. 当前项目约定
+
+当前链路如下：
+
+```text
+PICO 4 Ultra Unity 应用
+  -> UDP JSON, 默认端口 15000
+  -> xr_rm_input/udp_controller_receiver
+  -> /xr/left_controller 与 /xr/right_controller
+  -> xr_rm_teleop/single_arm_velocity_teleop
+  -> RM75 笛卡尔相对位移控制
+```
+
+ROS 端默认参数：
+
+| 项目 | 默认值 |
+| --- | --- |
+| UDP 监听地址 | `0.0.0.0` |
+| UDP 端口 | `15000` |
+| 左手柄话题 | `/xr/left_controller` |
+| 右手柄话题 | `/xr/right_controller` |
+| 四元数顺序 | `xyzw` |
+| 默认坐标 | PICO/OpenXR: `+X` 向右, `+Y` 向上, `+Z` 向后 |
+
+控制语义：
+
+- `grip=false`: 机械臂停止，退出相对位移遥操作。
+- `grip=true`: 第一帧锁定手柄起点和当前 TCP 起点，之后跟随手柄相对位移。
+- `trigger`: 当前主运动链路不使用，范围 `0.0-1.0`，预留给夹爪。
+- UDP 超过 `command_timeout_sec=0.12` 秒未更新时，机械臂停止。
+
+因此 PICO 端建议稳定发送 `60 Hz` 或 `90 Hz`。不要低于 `20 Hz`。
+
+## 2. UDP JSON 协议
+
+推荐 PICO 端每包同时发送左右手柄：
+
+```json
+{
+  "t": 12.345,
+  "frame_id": "xr_world",
+  "controllers": {
+    "left": {
+      "grip": true,
+      "trigger": 0.0,
+      "pos": [-0.12, 1.05, 0.30],
+      "quat": [0.0, 0.0, 0.0, 1.0]
+    },
+    "right": {
+      "grip": true,
+      "trigger": 0.4,
+      "pos": [0.12, 1.05, 0.30],
+      "quat": [0.0, 0.0, 0.0, 1.0]
+    }
+  }
+}
+```
+
+也兼容单手柄包：
+
+```json
+{
+  "hand": "right",
+  "grip": true,
+  "trigger": 0.2,
+  "pos": [0.12, 1.05, 0.30],
+  "quat": [0.0, 0.0, 0.0, 1.0]
+}
+```
+
+字段要求：
+
+| 字段 | 类型 | 说明 |
+| --- | --- | --- |
+| `hand` | string | 单手柄包使用，`left` 或 `right` |
+| `controllers.left` | object | 左手柄数据 |
+| `controllers.right` | object | 右手柄数据 |
+| `grip` | bool | 运动使能 |
+| `trigger` | float | `0.0-1.0` |
+| `pos` | float[3] | 手柄位置 `[x, y, z]` |
+| `quat` | float[4] | 手柄姿态 `[qx, qy, qz, qw]` |
+| `frame_id` | string | 可选，默认 `xr_world` |
+
+## 3. PICO 端准备
+
+### 3.1 硬件与网络
+
+1. PICO 4 Ultra 和 Ubuntu ROS 主机连接到同一个局域网。
+2. 在 Ubuntu 上查看主机 IP：
+
+```bash
+hostname -I
+```
+
+假设输出里有 `192.168.1.42`，Unity 脚本里的 `host` 就填 `192.168.1.42`。
+
+3. 如果 Ubuntu 开了防火墙，放行 UDP 端口：
+
+```bash
+sudo ufw allow 15000/udp
+```
+
+4. PICO 上打开开发者模式和 USB 调试，方便 Unity 直接 Build And Run。
+
+### 3.2 Unity 工程
+
+推荐使用 Unity LTS 版本，并安装 Android Build Support、Android SDK/NDK、OpenJDK。
+
+工程设置建议：
+
+1. `File -> Build Settings -> Android -> Switch Platform`。
+2. `Player Settings -> Other Settings` 中设置包名，例如 `com.local.xr_rm_udp_sender`。
+3. `Player Settings -> Other Settings -> Internet Access` 设为 `Require`。UDP 发送需要 Android `INTERNET` 权限。
+4. 导入 PICO Unity Integration SDK。
+5. 按 PICO 官方输入映射文档，手柄输入通过 Unity XR Input System 的 `CommonUsages` 读取。
+
+如果使用 Unity OpenXR 输入路径，常用映射是：
+
+| 功能 | Unity XR `CommonUsages` | OpenXR 语义 |
+| --- | --- | --- |
+| 手柄位置 | `devicePosition` | `/input/grip/pose` position |
+| 手柄姿态 | `deviceRotation` | `/input/grip/pose` rotation |
+| 扳机按钮 | `triggerButton` | `/input/trigger/click` |
+| 扳机模拟量 | `trigger` | `/input/trigger/value` |
+| 抓握按钮 | `gripButton` | squeeze click/button |
+| 抓握模拟量 | `grip` | `/input/squeeze/value` |
+
+## 4. Unity 最小 UDP 发送脚本
+
+在 Unity 中创建 `PicoControllerUdpSender.cs`，挂到一个常驻 GameObject 上。把 Inspector 里的 `Host` 改成 Ubuntu ROS 主机 IP，`Port` 保持 `15000`。
+
+```csharp
+using System;
+using System.Net;
+using System.Net.Sockets;
+using System.Text;
+using UnityEngine;
+using UnityEngine.XR;
+
+public class PicoControllerUdpSender : MonoBehaviour
+{
+    [Header("ROS UDP Target")]
+    public string host = "192.168.1.42";
+    public int port = 15000;
+
+    [Header("Send")]
+    public float sendHz = 60.0f;
+    public bool convertUnityToProjectCoordinates = true;
+
+    private UdpClient client;
+    private IPEndPoint endPoint;
+    private float nextSendTime;
+    private readonly Packet packet = new Packet();
+
+    [Serializable]
+    private class Packet
+    {
+        public double t;
+        public string frame_id = "xr_world";
+        public Controllers controllers = new Controllers();
+    }
+
+    [Serializable]
+    private class Controllers
+    {
+        public ControllerPayload left = new ControllerPayload();
+        public ControllerPayload right = new ControllerPayload();
+    }
+
+    [Serializable]
+    private class ControllerPayload
+    {
+        public bool grip;
+        public float trigger;
+        public float[] pos = new float[] { 0.0f, 1.0f, 0.0f };
+        public float[] quat = new float[] { 0.0f, 0.0f, 0.0f, 1.0f };
+    }
+
+    private void OnEnable()
+    {
+        client = new UdpClient();
+        endPoint = new IPEndPoint(IPAddress.Parse(host), port);
+        nextSendTime = 0.0f;
+    }
+
+    private void OnDisable()
+    {
+        packet.controllers.left.grip = false;
+        packet.controllers.right.grip = false;
+        SendPacket();
+        client?.Close();
+        client = null;
+    }
+
+    private void Update()
+    {
+        if (Time.unscaledTime < nextSendTime)
+        {
+            return;
+        }
+
+        nextSendTime = Time.unscaledTime + 1.0f / Mathf.Max(sendHz, 1.0f);
+        packet.t = Time.realtimeSinceStartupAsDouble;
+
+        FillController(XRNode.LeftHand, packet.controllers.left);
+        FillController(XRNode.RightHand, packet.controllers.right);
+        SendPacket();
+    }
+
+    private void FillController(XRNode node, ControllerPayload payload)
+    {
+        InputDevice device = InputDevices.GetDeviceAtXRNode(node);
+        if (!device.isValid)
+        {
+            payload.grip = false;
+            payload.trigger = 0.0f;
+            return;
+        }
+
+        Vector3 position = Vector3.zero;
+        Quaternion rotation = Quaternion.identity;
+        float trigger = 0.0f;
+        float gripValue = 0.0f;
+        bool gripButton = false;
+
+        device.TryGetFeatureValue(CommonUsages.devicePosition, out position);
+        device.TryGetFeatureValue(CommonUsages.deviceRotation, out rotation);
+        device.TryGetFeatureValue(CommonUsages.trigger, out trigger);
+        device.TryGetFeatureValue(CommonUsages.grip, out gripValue);
+        device.TryGetFeatureValue(CommonUsages.gripButton, out gripButton);
+
+        payload.grip = gripButton || gripValue > 0.5f;
+        payload.trigger = Mathf.Clamp01(trigger);
+
+        if (convertUnityToProjectCoordinates)
+        {
+            // Unity scene coordinates are commonly +Z forward. This project expects
+            // OpenXR-style controller positions with +Z backward.
+            payload.pos[0] = position.x;
+            payload.pos[1] = position.y;
+            payload.pos[2] = -position.z;
+
+            payload.quat[0] = -rotation.x;
+            payload.quat[1] = -rotation.y;
+            payload.quat[2] = rotation.z;
+            payload.quat[3] = rotation.w;
+        }
+        else
+        {
+            payload.pos[0] = position.x;
+            payload.pos[1] = position.y;
+            payload.pos[2] = position.z;
+
+            payload.quat[0] = rotation.x;
+            payload.quat[1] = rotation.y;
+            payload.quat[2] = rotation.z;
+            payload.quat[3] = rotation.w;
+        }
+    }
+
+    private void SendPacket()
+    {
+        if (client == null || endPoint == null)
+        {
+            return;
+        }
+
+        string json = JsonUtility.ToJson(packet);
+        byte[] bytes = Encoding.UTF8.GetBytes(json);
+        client.Send(bytes, bytes.Length, endPoint);
+    }
+}
+```
+
+说明：
+
+- 如果 `/xr/left_controller` 和 `/xr/right_controller` 里 `pos.z` 与实际前后方向相反，优先切换 `convertUnityToProjectCoordinates` 后再验证。
+- 当前项目的主控制只用位置相对位移，姿态 `quat` 会发布出来，但暂不参与机械臂姿态控制。
+- `OnDisable()` 会补发一次 `grip=false`，避免退出 PICO 应用时机械臂保持 active 状态。
+
+## 5. ROS 端先做低层 UDP 验证
+
+在工作空间根目录 `/home/robot/WS_xr` 执行：
+
+```bash
+source /opt/ros/humble/setup.bash
+source install/setup.bash
+ros2 launch xr_rm_input udp_receiver.launch.py udp_port:=15000
+```
+
+另开终端查看左右手柄：
+
+```bash
+source /opt/ros/humble/setup.bash
+source install/setup.bash
+ros2 topic echo /xr/left_controller
+```
+
+```bash
+source /opt/ros/humble/setup.bash
+source install/setup.bash
+ros2 topic echo /xr/right_controller
+```
+
+再查看频率：
+
+```bash
+ros2 topic hz /xr/left_controller
+ros2 topic hz /xr/right_controller
+```
+
+期望现象：
+
+- PICO 应用启动后，两个 topic 都持续刷新。
+- 按下左手抓握键时，`/xr/left_controller` 的 `grip` 变成 `true`。
+- 按下右手抓握键时，`/xr/right_controller` 的 `grip` 变成 `true`。
+- 扳机从松开到按下时，`trigger` 从 `0.0` 附近变到 `1.0` 附近。
+- 平移手柄时，`pose.position` 连续变化。
+
+如果收不到包，先在 Ubuntu 上抓 UDP：
+
+```bash
+sudo tcpdump -ni any udp port 15000
+```
+
+能看到 UDP 但 ROS topic 没数据，说明 JSON 字段不符合协议。看 `udp_controller_receiver` 终端里的 `XR 数据包格式错误` 或 `XR 手柄字段错误`。
+
+## 6. 用 sample_udp_sender 排除 ROS 端问题
+
+在接 PICO 前，先确认 ROS 端链路是通的。
+
+终端 1：
+
+```bash
+source /opt/ros/humble/setup.bash
+source install/setup.bash
+ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=true
+```
+
+终端 2：
+
+```bash
+source /opt/ros/humble/setup.bash
+source install/setup.bash
+ros2 run xr_rm_input sample_udp_sender --hand both --host 127.0.0.1 --port 15000 --seconds 20
+```
+
+终端 3：
+
+```bash
+ros2 topic echo /xr/left_controller
+ros2 topic echo /xr/right_controller
+ros2 topic echo /xr_rm/left_rm75/cmd_vel
+ros2 topic echo /xr_rm/right_rm75/cmd_vel
+```
+
+如果 sample sender 正常，而 PICO 不正常，问题在 PICO 端 IP、端口、权限、JSON 或坐标转换。
+
+## 7. PICO 端 mock 闭环调试流程
+
+先不要连接真机，使用 mock 模式验证完整控制链。
+
+1. 启动双臂 mock：
+
+```bash
+ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=true udp_port:=15000
+```
+
+2. 启动 PICO Unity 应用。
+3. 确认 `/xr/left_controller` 和 `/xr/right_controller` 正常刷新。
+4. 左手按住 `grip`，只移动左手一小段，观察：
+
+```bash
+ros2 topic echo /xr_rm/left_rm75/cmd_vel
+```
+
+5. 松开左手 `grip`，确认 `cmd_vel` 回到 0。
+6. 右手重复同样流程。
+7. 用 `ros2 topic hz` 确认频率稳定，建议 `50 Hz` 以上。
+
+## 8. 坐标方向检查
+
+当前配置使用的 PICO/OpenXR 位置坐标：
+
+```text
++X: 向右
++Y: 向上
++Z: 向后
+```
+
+双臂配置中的映射关系：
+
+```text
+左臂机器人位移增量 = [-手柄y, -手柄z,  手柄x]
+右臂机器人位移增量 = [ 手柄y, -手柄z, -手柄x]
+```
+
+建议现场按下面顺序验证：
+
+1. 只启动 `use_mock:=true`。
+2. 按住左手 `grip`，沿 PICO 的 `+X/-X`、`+Y/-Y`、`+Z/-Z` 每次只动一个轴。
+3. 记录 `/xr/left_controller.pose.position` 的变化方向。
+4. 记录 `/xr_rm/left_rm75/cmd_vel` 的方向。
+5. 右手重复。
+
+如果两个手柄在 ROS topic 里的某个轴都反了，优先检查 Unity 的坐标转换。
+
+如果 ROS topic 正确，但某一只机械臂运动方向不符合现场坐标，优先只改对应 YAML 的 `xr_to_robot_matrix`，不要同时改 Unity 坐标和机器人映射。
+
+## 9. 单臂真机小幅调试
+
+真机前检查：
+
+- 急停可用。
+- 机械臂工作区清空。
+- PICO topic 在 mock 下已经稳定。
+- `grip=false` 时 `/xr_rm/<arm>/cmd_vel` 为 0。
+- `move_to_initial_pose_on_connect` 保持 `false`。
+
+左臂：
+
+```bash
+ros2 launch xr_rm_bringup arm_debug.launch.py arm:=left use_mock:=false udp_port:=15000
+```
+
+右臂：
+
+```bash
+ros2 launch xr_rm_bringup arm_debug.launch.py arm:=right use_mock:=false udp_port:=15000
+```
+
+调试动作：
+
+1. 手柄保持静止。
+2. 按住 `grip`，第一帧只锁定起点，机械臂不应突然运动。
+3. 单轴移动手柄 `2-3 cm`。
+4. 松开 `grip`，确认机械臂停止。
+5. 每次只验证一个方向。
+
+## 10. 双臂真机调试
+
+只有在左右单臂都通过后，再启动双臂：
+
+```bash
+ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false \
+  left_robot_ip:=192.168.192.18 \
+  right_robot_ip:=192.168.192.19 \
+  udp_port:=15000
+```
+
+双臂第一轮建议：
+
+1. 左右手 `grip` 都不按，确认双臂静止。
+2. 只按左手 `grip`，确认只有左臂响应。
+3. 只按右手 `grip`，确认只有右臂响应。
+4. 左右同时按住 `grip`，做小幅、慢速、单轴运动。
+
+## 11. 常见问题
+
+| 现象 | 排查 |
+| --- | --- |
+| PICO 启动后 ROS topic 没数据 | 检查 Unity `host` 是否是 Ubuntu IP；检查 PICO 和 Ubuntu 是否同网段；检查 `sudo tcpdump -ni any udp port 15000` |
+| tcpdump 有包但 topic 没数据 | JSON 字段不对；确认包含 `controllers.left/right` 或单包 `hand`；确认 `pos` 长度 3、`quat` 长度 4 |
+| topic 有数据但机械臂不动 | 检查 `grip` 是否为 `true`；检查 teleop 节点是否订阅对应话题；检查是否超过工作空间限幅 |
+| 按左手右臂动 | PICO 端 left/right 填反，或 Unity `XRNode.LeftHand/RightHand` 获取错误 |
+| 松开 grip 后仍有速度 | 确认 PICO 持续发送 `grip=false`，并检查 teleop 终端是否收到超时停止 |
+| 经常提示手柄数据超时 | 发送频率太低、网络丢包、PICO 应用后台暂停；提高 `sendHz`，保持应用前台运行 |
+| 前后方向反了 | 先切换 Unity 脚本里的 `convertUnityToProjectCoordinates`，再验证 topic 方向 |
+| 某一只臂方向反了 | 修改对应 YAML 的 `xr_to_robot_matrix` 符号 |
+| 抖动明显 | 降低 `scale` 或 `kp_linear`，提高 `deadband_m`，保持 `low_pass_alpha` 不要过大 |
+
+## 12. 参考入口
+
+- PICO Controller & HMD input mapping: https://developer-cn.picoxr.com/en/document/unity/input-mapping/
+- PICO Unity Integration SDK: https://github.com/Pico-Developer/PICO-Unity-Integration-SDK
+- PICO `PXR_Input` API: https://developer-cn.picoxr.com/en/reference/unity/client-api/PXR_Input/
+- Unity XR `InputDevice.TryGetFeatureValue`: https://docs.unity3d.com/ScriptReference/XR.InputDevice.TryGetFeatureValue.html
+- Unity XR `CommonUsages`: https://docs.unity3d.com/ScriptReference/XR.CommonUsages.html
+- Unity OpenXR input: https://docs.unity.cn/Packages/com.unity.xr.openxr%401.9/manual/input.html

xr_rm_bringup/config/dual_arm_rm75.yaml

@@ -23,7 +23,7 @@ left_arm_teleop:
     kp_linear: 1.8
     deadband_m: 0.002
     low_pass_alpha: 0.35
-    max_linear_speed: 0.12
+    max_linear_speed: 0.2
     enable_position_axes: [true, true, true]
 
     # 来自 acDual-arm 的 bounds_p[left]：
@@ -36,10 +36,11 @@ left_arm_teleop:
 
     # PICO/OpenXR 位置坐标：+X 向右，+Y 向上，+Z 向后。
     # 左臂 base 坐标：+X 向下，+Z 向右，前方工作区对应 -Y。
-    # 映射关系：机器人位移增量 = [-手柄y, -手柄z, 手柄x]。
+    # 现场模拟 XR 调试确认 X/Z 轴符号与原标定相反。
+    # 映射关系：机器人位移增量 = [-手柄y, 手柄z, -手柄x]。
     xr_to_robot_matrix: [0.0, -1.0, 0.0,
-                         0.0, 0.0, -1.0,
+                         0.0, 0.0, 1.0,
-                         1.0, 0.0, 0.0]
+                         -1.0, 0.0, 0.0]
 
     use_mock: false
     mock_initial_pose: [-0.2562, -0.2765, 0.1489, -3.0190, -0.1010, 3.1400]
@@ -76,23 +77,24 @@ right_arm_teleop:
     kp_linear: 1.8
     deadband_m: 0.002
     low_pass_alpha: 0.35
-    max_linear_speed: 0.12
+    max_linear_speed: 0.2
     enable_position_axes: [true, true, true]
 
     # 来自 acDual-arm 的 bounds_p[right]：
     # x[-0.70, 0.50]，y[-0.60, 0.40]，z[0.10, 0.70]。
     workspace_min: [-0.70, -0.60, 0.10]
-    workspace_max: [0.50, 0.40, 0.70]
+    workspace_max: [0.70, 0.40, 0.70]
     cyl_radius_limit: [0.20, 0.60]
     low_z_threshold: 0.20
     low_z_min_radius: 0.21
 
     # PICO/OpenXR 位置坐标：+X 向右，+Y 向上，+Z 向后。
     # 右臂 base 坐标：+X 向上，+Z 向左，前方工作区对应 -Y。
-    # 映射关系：机器人位移增量 = [手柄y, -手柄z, -手柄x]。
+    # 现场模拟 XR 调试确认 X/Z 轴符号与原标定相反。
+    # 映射关系：机器人位移增量 = [手柄y, 手柄z, 手柄x]。
     xr_to_robot_matrix: [0.0, 1.0, 0.0,
-                         0.0, 0.0, -1.0,
+                         0.0, 0.0, 1.0,
-                         -1.0, 0.0, 0.0]
+                         1.0, 0.0, 0.0]
 
     use_mock: false
     mock_initial_pose: [0.2663, -0.2606, 0.1027, 3.0330, 0.0000, 1.0910]

xr_rm_bringup/config/left_arm_rm75.yaml

@@ -20,9 +20,10 @@ single_arm_velocity_teleop:
     low_z_threshold: 0.20
     low_z_min_radius: 0.21
 
+    # 现场模拟 XR 调试确认 X/Z 轴符号与原标定相反，因此翻转手柄 X、Z 两列。
     xr_to_robot_matrix: [0.0, -1.0, 0.0,
-                         0.0, 0.0, -1.0,
+                         0.0, 0.0, 1.0,
-                         1.0, 0.0, 0.0]
+                         -1.0, 0.0, 0.0]
 
     use_mock: false
     mock_initial_pose: [-0.2562, -0.2765, 0.1489, -3.0190, -0.1010, 3.1400]

xr_rm_bringup/config/right_arm_rm75.yaml

@@ -20,9 +20,10 @@ single_arm_velocity_teleop:
     low_z_threshold: 0.20
     low_z_min_radius: 0.21
 
+    # 现场模拟 XR 调试确认 X/Z 轴符号与原标定相反，因此翻转手柄 X、Z 两列。
     xr_to_robot_matrix: [0.0, 1.0, 0.0,
-                         0.0, 0.0, -1.0,
+                         0.0, 0.0, 1.0,
-                         -1.0, 0.0, 0.0]
+                         1.0, 0.0, 0.0]
 
     use_mock: false
     mock_initial_pose: [0.2663, -0.2606, 0.1027, 3.0330, 0.0000, 1.0910]

xr_rm_bringup/tools/launcher_ui.py

@@ -41,6 +41,8 @@ SAMPLE_SENDER_ARGS = (
 TERMINAL_TITLE_PREFIX = "XR-RM Terminal - "
 TOPIC_MONITOR_TITLE = "XR-RM Topic Monitor"
 TOPIC_MONITOR_ACTION = "__xr_rm_topic_monitor__"
+CMD_VEL_MONITOR_TITLE = "XR-RM Cmd Vel Monitor"
+CMD_VEL_MONITOR_ACTION = "__xr_rm_cmd_vel_monitor__"
 ROS_GRAPH_MONITOR_TITLE = "XR-RM ROS Graph Monitor"
 ROS_GRAPH_MONITOR_ACTION = "__xr_rm_ros_graph_monitor__"
 
@@ -49,6 +51,11 @@ TOPIC_MONITORS = [
     ("Right Controller", "/xr/right_controller"),
 ]
 
+CMD_VEL_MONITORS = [
+    ("Left Cmd Vel", "/xr_rm/left_rm75/cmd_vel"),
+    ("Right Cmd Vel", "/xr_rm/right_rm75/cmd_vel"),
+]
+
 ROS_GRAPH_MONITORS = [
     ("ROS Topic List", "ros2 topic list"),
     ("ROS Node List", "ros2 node list"),
@@ -153,11 +160,40 @@ def _topic_monitor_item() -> tuple[str, str]:
     return ("Open Controller Topic Monitor", TOPIC_MONITOR_ACTION)
 
 
+def _cmd_vel_monitor_item() -> tuple[str, str]:
+    return ("Open Cmd Vel Topic Monitor", CMD_VEL_MONITOR_ACTION)
+
+
+def _is_topic_monitor_action(action: str) -> bool:
+    return action in (TOPIC_MONITOR_ACTION, CMD_VEL_MONITOR_ACTION)
+
+
+def _topic_monitor_spec(action: str) -> tuple[str, list[tuple[str, str]], str, str, str]:
+    if action == CMD_VEL_MONITOR_ACTION:
+        return (
+            CMD_VEL_MONITOR_TITLE,
+            CMD_VEL_MONITORS,
+            "xr_rm_cmd_vel_monitor",
+            "xr_rm_cmd_vel_monitor_",
+            "cmd_vel topic",
+        )
+    return (
+        TOPIC_MONITOR_TITLE,
+        TOPIC_MONITORS,
+        "xr_rm_topic_monitor",
+        "xr_rm_topic_monitor_",
+        "controller topic",
+    )
+
+
 def _finalize_items(
     items: list[tuple[str, str]],
     one_click: tuple[str, str] | None = None,
 ) -> list[tuple[str, str]]:
-    final_items = items + _diagnostic_commands() + [_topic_monitor_item()]
+    final_items = items + _diagnostic_commands() + [
+        _topic_monitor_item(),
+        _cmd_vel_monitor_item(),
+    ]
     if one_click is not None:
         final_items.append(one_click)
     return _with_index(final_items)
@@ -344,6 +380,7 @@ class LauncherApp:
         self.status = tk.Label(root, text="Ready", bd=1, relief=tk.SUNKEN, anchor=tk.W)
         self.status.grid(row=1, column=0, sticky="ew")
 
+        self.root.protocol("WM_DELETE_WINDOW", self.on_close_requested)
         self.refresh_command_list()
 
     def refresh_command_list(self) -> None:
@@ -371,9 +408,10 @@ class LauncherApp:
     def update_preview(self) -> None:
         selected = self.selected_command()
         text = selected[1] if selected else ""
-        if text == TOPIC_MONITOR_ACTION:
+        if _is_topic_monitor_action(text):
-            topics = ", ".join(topic for _title, topic in TOPIC_MONITORS)
+            _window_title, monitors, _layout_name, _config_prefix, _label = _topic_monitor_spec(text)
-            text = f"Open one Terminator window split into controller topic panes:\n{topics}"
+            topics = ", ".join(topic for _title, topic in monitors)
+            text = f"Open one Terminator window split into topic panes:\n{topics}"
         elif text == ROS_GRAPH_MONITOR_ACTION:
             text = "Open one Terminator window split into 2 horizontal panes:\nros2 topic list | ros2 node list"
         self.preview.configure(state=tk.NORMAL)
@@ -417,8 +455,8 @@ class LauncherApp:
             return
 
         name, command = selected
-        if command == TOPIC_MONITOR_ACTION:
+        if _is_topic_monitor_action(command):
-            self.launch_topic_monitor()
+            self.launch_topic_monitor(command)
             return
         if command == ROS_GRAPH_MONITOR_ACTION:
             self.launch_ros_graph_monitor()
@@ -574,8 +612,8 @@ class LauncherApp:
         except OSError:
             return Path(terminal).name
 
-    def topic_pane_command(self, title: str, topic: str) -> str:
+    def topic_pane_command(self, title: str, topic: str, window_title: str = TOPIC_MONITOR_TITLE) -> str:
-        pane_title = f"{TOPIC_MONITOR_TITLE} - {title}"
+        pane_title = f"{window_title} - {title}"
         lines = _source_lines(self.workspace_root)
         lines.extend([
             "export XR_RM_LAUNCHER_SESSION=1",
@@ -610,16 +648,22 @@ class LauncherApp:
         ])
         return "\n".join(lines)
 
-    def write_topic_monitor_script(self, title: str, topic: str) -> Path:
+    def write_topic_monitor_script(
+        self,
+        title: str,
+        topic: str,
+        window_title: str = TOPIC_MONITOR_TITLE,
+        config_prefix: str = "xr_rm_topic_monitor_",
+    ) -> Path:
         script_file = tempfile.NamedTemporaryFile(
             mode="w",
-            prefix="xr_rm_topic_monitor_",
+            prefix=config_prefix,
             suffix=".sh",
             delete=False,
             encoding="utf-8",
         )
         with script_file:
-            script_file.write(self.topic_pane_command(title, topic))
+            script_file.write(self.topic_pane_command(title, topic, window_title))
             script_file.write("\n")
         os.chmod(script_file.name, 0o755)
         return Path(script_file.name)
@@ -931,13 +975,17 @@ class LauncherApp:
         event_mask = (1 << 19) | (1 << 20)  # SubstructureNotifyMask | SubstructureRedirectMask
         xlib.XSendEvent(display, root, False, event_mask, ctypes.byref(event))
 
-    def write_topic_monitor_config(self, target_rect: tuple[int, int, int, int] | None = None) -> Path:
+    def write_topic_monitor_config(
+        self,
+        target_rect: tuple[int, int, int, int] | None = None,
+        action: str = TOPIC_MONITOR_ACTION,
+    ) -> Path:
+        window_title, monitors, layout_name, config_prefix, _label = _topic_monitor_spec(action)
         width, height, left, top = target_rect or self.topic_monitor_rect()
         left_script, right_script = [
-            self.write_topic_monitor_script(title, topic)
+            self.write_topic_monitor_script(title, topic, window_title, config_prefix)
-            for title, topic in TOPIC_MONITORS
+            for title, topic in monitors
         ]
-        layout_name = "xr_rm_topic_monitor"
         config_text = "\n".join([
             "[global_config]",
             "  geometry_hinting = False",
@@ -952,7 +1000,7 @@ class LauncherApp:
             "    [[[window0]]]",
             "      type = Window",
             "      parent = \"\"",
-            f"      title = {_config_quote(TOPIC_MONITOR_TITLE)}",
+            f"      title = {_config_quote(window_title)}",
             f"      position = {_config_quote(f'{left}:{top}')}",
             f"      size = {width}, {height}",
             "      maximised = False",
@@ -977,7 +1025,7 @@ class LauncherApp:
         ])
         config_file = tempfile.NamedTemporaryFile(
             mode="w",
-            prefix="xr_rm_topic_monitor_",
+            prefix=config_prefix,
             suffix=".conf",
             delete=False,
             encoding="utf-8",
@@ -1041,7 +1089,8 @@ class LauncherApp:
             config_file.write(config_text)
         return Path(config_file.name)
 
-    def launch_topic_monitor(self) -> None:
+    def launch_topic_monitor(self, action: str = TOPIC_MONITOR_ACTION) -> None:
+        window_title, _monitors, layout_name, _config_prefix, label = _topic_monitor_spec(action)
         terminal = shutil.which("x-terminal-emulator")
         if terminal is None:
             messagebox.showerror(
@@ -1053,22 +1102,22 @@ class LauncherApp:
         if self.x_terminal_target() != "terminator":
             messagebox.showwarning(
                 "Topic Monitor",
-                "The controller topic monitor uses Terminator's split-layout support. "
+                f"The {label} monitor uses Terminator's split-layout support. "
                 "Please set x-terminal-emulator to Terminator.",
             )
             return
 
         target_rect = self.topic_monitor_rect()
-        config_path = self.write_topic_monitor_config(target_rect)
+        config_path = self.write_topic_monitor_config(target_rect, action)
         command = [
             terminal,
             "--no-dbus",
             "--title",
-            TOPIC_MONITOR_TITLE,
+            window_title,
             "--config",
             str(config_path),
             "--layout",
-            "xr_rm_topic_monitor",
+            layout_name,
         ]
         try:
             subprocess.Popen(
@@ -1077,8 +1126,8 @@ class LauncherApp:
                 stdout=subprocess.DEVNULL,
                 stderr=subprocess.DEVNULL,
             )
-            self.root.after(60, lambda: self.force_monitor_window_geometry(TOPIC_MONITOR_TITLE, target_rect))
+            self.root.after(60, lambda: self.force_monitor_window_geometry(window_title, target_rect))
-            self.status.config(text="Opened Terminator controller topic monitor.")
+            self.status.config(text=f"Opened Terminator {label} monitor.")
         except Exception as exc:
             messagebox.showerror("Topic Monitor Failed", f"Failed to launch topic monitor:\n{exc}")
             self.status.config(text="Topic monitor launch failed")
@@ -1126,7 +1175,12 @@ class LauncherApp:
             self.status.config(text="ROS graph monitor launch failed")
 
     def close_related_terminal_windows(self) -> int:
-        title_patterns = (TERMINAL_TITLE_PREFIX, TOPIC_MONITOR_TITLE, ROS_GRAPH_MONITOR_TITLE)
+        title_patterns = (
+            TERMINAL_TITLE_PREFIX,
+            TOPIC_MONITOR_TITLE,
+            CMD_VEL_MONITOR_TITLE,
+            ROS_GRAPH_MONITOR_TITLE,
+        )
         closed = 0
 
         if shutil.which("wmctrl"):
@@ -1157,13 +1211,19 @@ class LauncherApp:
 
         return closed
 
+    def on_close_requested(self) -> None:
+        if self.stop_launched_processes(confirm=True, notify=False):
+            self.root.destroy()
+
     def kill_launched_processes(self) -> None:
-        confirm = messagebox.askyesno(
+        self.stop_launched_processes(confirm=True, notify=True)
+
+    def stop_launched_processes(self, *, confirm: bool, notify: bool) -> bool:
+        if confirm and not messagebox.askyesno(
             "Confirm Stop",
             "Stop XR-RM launcher terminals, topic monitors, and ROS nodes started from this workspace?",
-        )
+        ):
-        if not confirm:
+            return False
-            return
 
         # 只清理由启动器常用命令创建的 ROS/监控进程，并保护当前 UI 进程。
         patterns = [
@@ -1173,14 +1233,18 @@ class LauncherApp:
             "XR_RM_LAUNCHER_SESSION=1",
             TERMINAL_TITLE_PREFIX,
             TOPIC_MONITOR_TITLE,
+            CMD_VEL_MONITOR_TITLE,
             ROS_GRAPH_MONITOR_TITLE,
             "xr_rm_topic_monitor_",
+            "xr_rm_cmd_vel_monitor_",
             "xr_rm_ros_graph_monitor_",
             "udp_controller_receiver",
             "sample_udp_sender",
             "single_arm_velocity_teleop",
             "ros2 topic echo /xr/left_controller",
             "ros2 topic echo /xr/right_controller",
+            "ros2 topic echo /xr_rm/left_rm75/cmd_vel",
+            "ros2 topic echo /xr_rm/right_rm75/cmd_vel",
             "ros2 topic list",
             "ros2 node list",
         ]
@@ -1214,12 +1278,14 @@ class LauncherApp:
         self.status.config(
             text=f"Stop requested: {len(killed)} matching processes, {closed_windows} terminal windows."
         )
-        messagebox.showinfo(
+        if notify:
-            "Stop Complete",
+            messagebox.showinfo(
-            "Stop requested for:\n"
+                "Stop Complete",
-            f"- {len(killed)} matching processes, including launcher terminal wrappers\n"
+                "Stop requested for:\n"
-            f"- {closed_windows} related terminal windows",
+                f"- {len(killed)} matching processes, including launcher terminal wrappers\n"
-        )
+                f"- {closed_windows} related terminal windows",
+            )
+        return True
 
 
 def main() -> None:

xr_rm_bringup/tools/realman_dual_arm_state_monitor.py

@@ -12,12 +12,42 @@ from __future__ import annotations
 import argparse
 import time
 from dataclasses import dataclass
-from typing import Optional
+from numbers import Number
+from pathlib import Path
+from typing import Any
 
 import numpy as np
+import yaml
 
-from realman_pkg.Robotic_Arm.rm_robot_interface import RoboticArm, rm_thread_mode_e
+
-from realman_pkg.core.arg_cfg import host_ip, ip_l, ip_r, port_l, port_r
+DEFAULT_HOST_IP = "0.0.0.0"
+DEFAULT_LEFT_IP = "192.168.192.18"
+DEFAULT_RIGHT_IP = "192.168.192.19"
+DEFAULT_ROBOT_PORT = 8080
+
+
+def default_config_path() -> Path:
+    """同时兼容源码目录和 colcon install 后的配置文件路径。"""
+    script_path = Path(__file__).resolve()
+    candidates = [
+        script_path.parents[1] / "config" / "dual_arm_rm75.yaml",
+        script_path.parents[2] / "share" / "xr_rm_bringup" / "config" / "dual_arm_rm75.yaml",
+    ]
+
+    try:
+        from ament_index_python.packages import get_package_share_directory
+
+        candidates.append(Path(get_package_share_directory("xr_rm_bringup")) / "config" / "dual_arm_rm75.yaml")
+    except Exception:
+        pass
+
+    for candidate in candidates:
+        if candidate.is_file():
+            return candidate
+    return candidates[0]
+
+
+DEFAULT_CONFIG_PATH = default_config_path()
 
 
 @dataclass
@@ -32,6 +62,245 @@ class ArmSnapshot:
     err_text: str
 
 
+@dataclass
+class MonitorDefaults:
+    """从 YAML 配置中读取到的监控默认参数。"""
+
+    config_path: Path
+    left_ip: str = DEFAULT_LEFT_IP
+    right_ip: str = DEFAULT_RIGHT_IP
+    left_port: int = DEFAULT_ROBOT_PORT
+    right_port: int = DEFAULT_ROBOT_PORT
+    host_ip: str = DEFAULT_HOST_IP
+
+
+def _load_yaml(path: Path) -> dict[str, Any]:
+    """读取 ROS 参数 YAML，返回顶层 dict。"""
+    if not path.is_file():
+        raise FileNotFoundError(f"配置文件不存在: {path}")
+
+    with path.open("r", encoding="utf-8") as f:
+        data = yaml.safe_load(f) or {}
+
+    if not isinstance(data, dict):
+        raise ValueError(f"配置文件格式错误，顶层应为 dict: {path}")
+    return data
+
+
+def _node_parameters(config: dict[str, Any], node_name: str) -> dict[str, Any]:
+    node_config = config.get(node_name, {})
+    if not isinstance(node_config, dict):
+        return {}
+    params = node_config.get("ros__parameters", {})
+    return params if isinstance(params, dict) else {}
+
+
+def load_monitor_defaults(config_path: Path) -> MonitorDefaults:
+    """从双臂配置读取左右臂连接参数。"""
+    config = _load_yaml(config_path)
+    left_params = _node_parameters(config, "left_arm_teleop")
+    right_params = _node_parameters(config, "right_arm_teleop")
+
+    return MonitorDefaults(
+        config_path=config_path,
+        left_ip=str(left_params.get("robot_ip", DEFAULT_LEFT_IP)),
+        right_ip=str(right_params.get("robot_ip", DEFAULT_RIGHT_IP)),
+        left_port=int(left_params.get("robot_port", DEFAULT_ROBOT_PORT)),
+        right_port=int(right_params.get("robot_port", DEFAULT_ROBOT_PORT)),
+    )
+
+
+def load_realman_sdk() -> tuple[Any, Any]:
+    """延迟导入睿尔曼 SDK，让 --help 和参数解析不依赖现场环境。"""
+    try:
+        from Robotic_Arm.rm_robot_interface import RoboticArm, rm_thread_mode_e
+    except ImportError as exc:
+        raise RuntimeError(
+            "未安装睿尔曼 Python API2，无法连接真机。请先安装厂商 SDK，"
+            "确认可以导入 Robotic_Arm.rm_robot_interface。"
+        ) from exc
+    return RoboticArm, rm_thread_mode_e
+
+
+def resolve_thread_mode(rm_thread_mode_e: Any, mode: int) -> Any:
+    """兼容不同 SDK 版本的线程模式枚举写法。"""
+    try:
+        return rm_thread_mode_e(mode)
+    except Exception:
+        pass
+
+    mode_names = {
+        0: "RM_SINGLE_MODE_E",
+        1: "RM_DUAL_MODE_E",
+        2: "RM_TRIPLE_MODE_E",
+    }
+    mode_name = mode_names.get(int(mode))
+    if mode_name and hasattr(rm_thread_mode_e, mode_name):
+        return getattr(rm_thread_mode_e, mode_name)
+    return mode
+
+
+def create_robot_arm(robot: Any, robot_ip: str, robot_port: int, level: int) -> Any:
+    """兼容 rm_create_robot_arm(ip, port) 和 rm_create_robot_arm(ip, port, level)。"""
+    try:
+        return robot.rm_create_robot_arm(robot_ip, robot_port, level)
+    except TypeError:
+        return robot.rm_create_robot_arm(robot_ip, robot_port)
+
+
+def robot_handle_id(handle: Any) -> int | None:
+    """从 SDK handle 中取 id，兼容对象和整数返回值。"""
+    if isinstance(handle, int):
+        return handle
+    return getattr(handle, "id", None)
+
+
+def return_code(obj: Any) -> int:
+    """从不同 SDK 返回结构中解析 ret/code。"""
+    if isinstance(obj, tuple) and obj and isinstance(obj[0], int):
+        return obj[0]
+    if isinstance(obj, dict):
+        for key in ("ret", "code", "err_code", "error_code"):
+            value = obj.get(key)
+            if isinstance(value, int):
+                return value
+    return 0
+
+
+def payload(obj: Any) -> Any:
+    """从 (ret, data) 风格返回值中取状态主体。"""
+    if isinstance(obj, tuple) and len(obj) >= 2 and isinstance(obj[0], int):
+        return obj[1]
+    return obj
+
+
+def _as_number_list(value: Any, expected_len: int) -> list[float] | None:
+    if isinstance(value, np.ndarray):
+        value = value.tolist()
+    if isinstance(value, (list, tuple)) and len(value) >= expected_len:
+        if all(isinstance(item, Number) for item in value[:expected_len]):
+            return [float(item) for item in value[:expected_len]]
+    return None
+
+
+def _as_pose(value: Any) -> list[float] | None:
+    pose = _as_number_list(value, 6)
+    if pose is not None:
+        return pose
+
+    if isinstance(value, dict):
+        position = value.get("position")
+        euler = value.get("euler")
+        if isinstance(position, dict) and isinstance(euler, dict):
+            xyz_keys = ("x", "y", "z")
+            rpy_keys = ("rx", "ry", "rz")
+            if all(key in position for key in xyz_keys) and all(key in euler for key in rpy_keys):
+                return [
+                    float(position["x"]),
+                    float(position["y"]),
+                    float(position["z"]),
+                    float(euler["rx"]),
+                    float(euler["ry"]),
+                    float(euler["rz"]),
+                ]
+
+    if all(hasattr(value, attr) for attr in ("position", "euler")):
+        position = getattr(value, "position")
+        euler = getattr(value, "euler")
+        if all(hasattr(position, key) for key in ("x", "y", "z")) and all(
+            hasattr(euler, key) for key in ("rx", "ry", "rz")
+        ):
+            return [
+                float(position.x),
+                float(position.y),
+                float(position.z),
+                float(euler.rx),
+                float(euler.ry),
+                float(euler.rz),
+            ]
+    return None
+
+
+def find_pose(obj: Any) -> list[float] | None:
+    """递归查找常见 TCP 位姿字段。"""
+    pose = _as_pose(obj)
+    if pose is not None:
+        return pose
+
+    if isinstance(obj, dict):
+        for key in ("pose", "tool_pose", "tcp_pose", "current_pose"):
+            pose = _as_pose(obj.get(key))
+            if pose is not None:
+                return pose
+        for value in obj.values():
+            pose = find_pose(value)
+            if pose is not None:
+                return pose
+    elif isinstance(obj, (list, tuple)):
+        for value in obj:
+            pose = find_pose(value)
+            if pose is not None:
+                return pose
+    elif hasattr(obj, "to_dictionary"):
+        try:
+            return find_pose(obj.to_dictionary(7))
+        except TypeError:
+            return find_pose(obj.to_dictionary())
+    elif hasattr(obj, "to_dict"):
+        return find_pose(obj.to_dict())
+    else:
+        for key in ("pose", "tool_pose", "tcp_pose", "current_pose"):
+            if hasattr(obj, key):
+                pose = _as_pose(getattr(obj, key))
+                if pose is not None:
+                    return pose
+    return None
+
+
+def find_joint(obj: Any) -> list[float] | None:
+    """递归查找常见关节角字段。"""
+    joint = _as_number_list(obj, 7)
+    if joint is not None:
+        return joint
+
+    if isinstance(obj, dict):
+        for key in ("joint", "joints", "joint_deg", "joint_position", "joint_positions"):
+            joint = _as_number_list(obj.get(key), 7)
+            if joint is not None:
+                return joint
+        for value in obj.values():
+            joint = find_joint(value)
+            if joint is not None:
+                return joint
+    elif isinstance(obj, (list, tuple)):
+        for value in obj:
+            joint = find_joint(value)
+            if joint is not None:
+                return joint
+    elif hasattr(obj, "to_dictionary"):
+        try:
+            return find_joint(obj.to_dictionary(7))
+        except TypeError:
+            return find_joint(obj.to_dictionary())
+    elif hasattr(obj, "to_dict"):
+        return find_joint(obj.to_dict())
+    else:
+        for key in ("joint", "joints", "joint_deg", "joint_position", "joint_positions"):
+            if hasattr(obj, key):
+                joint = _as_number_list(getattr(obj, key), 7)
+                if joint is not None:
+                    return joint
+    return None
+
+
+def find_error_text(obj: Any) -> str:
+    if isinstance(obj, dict):
+        for key in ("err", "error", "error_msg", "err_msg"):
+            if key in obj:
+                return str(obj[key])
+    return "ok"
+
+
 class ArmStatePoller:
     """单臂状态轮询器。"""
 
@@ -49,7 +318,7 @@ class ArmStatePoller:
         self.level = int(level)
         self.mode = int(mode)
 
-        self.robot: Optional[RoboticArm] = None
+        self.robot: Any | None = None
         self.robot_handle = None
         self.connected = False
 
@@ -67,9 +336,11 @@ class ArmStatePoller:
     def _connect(self):
         """连接机械臂。"""
         try:
-            self.robot = RoboticArm(rm_thread_mode_e(self.mode))
+            RoboticArm, rm_thread_mode_e = load_realman_sdk()
-            self.robot_handle = self.robot.rm_create_robot_arm(self.robot_ip, self.robot_port, self.level)
+            self.robot = RoboticArm(resolve_thread_mode(rm_thread_mode_e, self.mode))
-            if self.robot_handle.id == -1:
+            self.robot_handle = create_robot_arm(self.robot, self.robot_ip, self.robot_port, self.level)
+            handle_id = robot_handle_id(self.robot_handle)
+            if self.robot_handle is None or handle_id == -1:
                 self.connected = False
                 self.last_snapshot.connected = False
                 self.last_snapshot.err_text = f"connect failed ({self.robot_ip}:{self.robot_port})"
@@ -79,7 +350,7 @@ class ArmStatePoller:
             self.connected = True
             self.last_snapshot.connected = True
             self.last_snapshot.err_text = "ok"
-            print(f"[{self.name}] 已连接, handle_id={self.robot_handle.id}, ip={self.robot_ip}, port={self.robot_port}")
+            print(f"[{self.name}] 已连接, handle_id={handle_id}, ip={self.robot_ip}, port={self.robot_port}")
         except Exception as exc:
             self.connected = False
             self.last_snapshot.connected = False
@@ -92,7 +363,9 @@ class ArmStatePoller:
             return self.last_snapshot
 
         try:
-            ret, state = self.robot.rm_get_current_arm_state()
+            raw_state = self.robot.rm_get_current_arm_state()
+            ret = return_code(raw_state)
+            state = payload(raw_state)
             now_t = time.time()
 
             if ret != 0:
@@ -100,9 +373,9 @@ class ArmStatePoller:
                 self.last_snapshot.err_text = f"rm_get_current_arm_state ret={ret}"
                 return self.last_snapshot
 
-            joint = list(state.get("joint", [0.0] * 7))
+            joint = find_joint(state) or [0.0] * 7
-            pose = list(state.get("pose", [0.0] * 6))
+            pose = find_pose(state) or [0.0] * 6
-            err = state.get("err", {})
+            err = find_error_text(state)
 
             if len(joint) < 7:
                 joint.extend([0.0] * (7 - len(joint)))
@@ -156,22 +429,32 @@ def format_arm_block(name: str, snap: ArmSnapshot, now_t: float) -> str:
 
 
 def build_arg_parser() -> argparse.ArgumentParser:
+    bootstrap = argparse.ArgumentParser(add_help=False)
+    bootstrap.add_argument("--config", type=Path, default=DEFAULT_CONFIG_PATH)
+    known_args, _ = bootstrap.parse_known_args()
+    defaults = load_monitor_defaults(known_args.config)
+
     parser = argparse.ArgumentParser(description="Realman 双臂状态监控脚本（轮询版）")
-    parser.add_argument("--left-ip", default=ip_l, help="左臂 IP")
+    parser.add_argument("--config", type=Path, default=defaults.config_path, help="双臂 YAML 配置文件")
-    parser.add_argument("--right-ip", default=ip_r, help="右臂 IP")
+    parser.add_argument("--left-ip", default=defaults.left_ip, help="左臂 IP")
-    parser.add_argument("--left-port", type=int, default=port_l, help="左臂 TCP 端口")
+    parser.add_argument("--right-ip", default=defaults.right_ip, help="右臂 IP")
-    parser.add_argument("--right-port", type=int, default=port_r, help="右臂 TCP 端口")
+    parser.add_argument("--left-port", type=int, default=defaults.left_port, help="左臂 TCP 端口")
+    parser.add_argument("--right-port", type=int, default=defaults.right_port, help="右臂 TCP 端口")
     parser.add_argument("--refresh", type=float, default=0.5, help="刷新周期（秒）")
     parser.add_argument("--level", type=int, default=3, help="连接级别")
     parser.add_argument("--mode", type=int, default=2, help="线程模式 0/1/2")
-    parser.add_argument("--host-ip", default=host_ip, help="仅展示当前配置用，轮询版不使用")
+    parser.add_argument("--host-ip", default=defaults.host_ip, help="仅展示当前配置用，轮询版不使用")
     return parser
 
 
 def main():
     args = build_arg_parser().parse_args()
 
-    print(f"配置: left={args.left_ip}:{args.left_port}, right={args.right_ip}:{args.right_port}, host_ip={args.host_ip}")
+    print(
+        f"配置: config={args.config}, "
+        f"left={args.left_ip}:{args.left_port}, "
+        f"right={args.right_ip}:{args.right_port}, host_ip={args.host_ip}"
+    )
 
     left = ArmStatePoller(
         name="LEFT",

xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py

@@ -40,6 +40,7 @@ class SingleArmVelocityTeleop(Node):
         self.declare_parameter("kp_linear", 2.0)
         self.declare_parameter("deadband_m", 0.002)
         self.declare_parameter("low_pass_alpha", 0.35)
+        self.declare_parameter("velocity_zero_epsilon", 1e-6)
         self.declare_parameter("max_linear_speed", 0.05)
         self.declare_parameter("enable_position_axes", [True, True, True])
         self.declare_parameter("workspace_min", [0.20, -0.35, 0.10])
@@ -82,6 +83,7 @@ class SingleArmVelocityTeleop(Node):
         self._kp_linear = float(self.get_parameter("kp_linear").value)
         self._deadband_m = float(self.get_parameter("deadband_m").value)
         self._low_pass_alpha = float(self.get_parameter("low_pass_alpha").value)
+        self._velocity_zero_epsilon = float(self.get_parameter("velocity_zero_epsilon").value)
         self._max_linear_speed = float(self.get_parameter("max_linear_speed").value)
         self._enable_position_axes = self._bool_list_parameter("enable_position_axes", 3)
         self._workspace_min = self._float_list_parameter("workspace_min", 3)
@@ -217,6 +219,7 @@ class SingleArmVelocityTeleop(Node):
             alpha * velocity[i] + (1.0 - alpha) * self._filtered_velocity[i]
             for i in range(3)
         ]
+        self._filtered_velocity = self._zero_tiny_velocity(self._filtered_velocity)
         cartesian_velocity = self._filtered_velocity + [0.0, 0.0, 0.0]
         self._publish_debug(robot_pose, target, cartesian_velocity)
         if self._adapter_uses_pose_targets():
@@ -290,10 +293,18 @@ class SingleArmVelocityTeleop(Node):
     def _safe_stop(self) -> None:
         # 所有断连、松手、退出路径都走这里，确保发送零速度。
         self._filtered_velocity = [0.0, 0.0, 0.0]
+        self._publish_stop_debug()
         if self._adapter_uses_pose_targets():
             return
         self._send_cartesian_velocity([0.0] * 6)
 
+    def _publish_stop_debug(self) -> None:
+        try:
+            current_pose = self._adapter.get_current_pose()
+        except Exception:
+            return
+        self._publish_debug(current_pose, current_pose.xyz(), [0.0] * 6)
+
     def _send_cartesian_velocity(self, velocity: list[float]) -> None:
         try:
             self._adapter.send_cartesian_velocity(velocity, self._follow)
@@ -318,6 +329,14 @@ class SingleArmVelocityTeleop(Node):
         uses_pose_targets = getattr(self._adapter, "uses_pose_targets", None)
         return bool(uses_pose_targets is not None and uses_pose_targets())
 
+    def _zero_tiny_velocity(self, velocity: list[float]) -> list[float]:
+        if self._velocity_zero_epsilon <= 0.0:
+            return velocity
+        return [
+            0.0 if abs(value) < self._velocity_zero_epsilon else value
+            for value in velocity
+        ]
+
     def _publish_debug(self, current_pose: ArmPose, target_xyz: list[float], velocity: list[float]) -> None:
         stamp = self.get_clock().now().to_msg()
         current_msg = self._pose_msg(stamp, current_pose)
@@ -332,6 +351,7 @@ class SingleArmVelocityTeleop(Node):
                 rz=current_pose.rz,
             ),
         )
+        velocity = self._zero_tiny_velocity(velocity)
         velocity_msg = TwistStamped()
         velocity_msg.header.stamp = stamp
         velocity_msg.header.frame_id = "rm_base"
@@ -406,6 +426,8 @@ class SingleArmVelocityTeleop(Node):
             raise ValueError("deadband_m must be >= 0")
         if not 0.0 <= self._low_pass_alpha <= 1.0:
             raise ValueError("low_pass_alpha must be in [0, 1]")
+        if self._velocity_zero_epsilon < 0.0:
+            raise ValueError("velocity_zero_epsilon must be >= 0")
         if self._max_linear_speed < 0.0:
             raise ValueError("max_linear_speed must be >= 0")
         for axis, (low, high) in enumerate(zip(self._workspace_min, self._workspace_max)):