Refactor and enhance XR-RM teleoperation functionality

README.md

@@ -7,11 +7,11 @@ PICO/XR 双手柄 UDP JSON
   -> xr_rm_input/udp_controller_receiver
   -> /xr/left_controller 与 /xr/right_controller
   -> xr_rm_teleop/single_arm_velocity_teleop
-  -> 左右 RM75 笛卡尔相对位移速度控制
-  -> /xr_rm/<arm_name>/current_pose、target_pose、cmd_vel 调试话题
+  -> 左右 RM75 笛卡尔相对位姿透传控制
+  -> /xr_rm/<arm_name>/current_pose、raw_target_pose、target_pose、cmd_vel、target_clamped 调试话题
 ```
 
-当前控制方式是“手柄相对位移”遥操作：按住 `grip` 时锁定当前手柄位置和机械臂 TCP 位置，之后根据手柄相对位移生成目标 TCP，再用速度命令跟随。松开 `grip`、UDP 超时或节点退出时会发送零速度。
+当前控制方式是“手柄相对位姿透传”遥操作：按住 `grip` 时锁定当前手柄位置和机械臂 TCP 位置，之后根据手柄相对位移生成目标 TCP，经过工作空间限幅、目标低通和单帧步长限制后，通过 `rm_movep_canfd` 下发目标位姿。松开 `grip`、UDP 超时或节点退出时会请求机械臂慢停。
 
 ## 当前范围
 
@@ -19,7 +19,7 @@ PICO/XR 双手柄 UDP JSON
 
 - PICO/XR 手柄 UDP 数据接收，并分发到左右手柄 ROS2 话题。
 - 通过统一的 `arm_debug.launch.py` 支持左臂、右臂、双臂的 mock 调试和真机调试。
-- RM75 真机连接适配，包含速度透传初始化、安全速度/加速度配置、可选初始化点位移动。
+- RM75 真机连接适配，包含 `rm_movep_canfd` 位姿透传、安全速度/加速度配置、可选初始化点位移动。
 - Tkinter 启动面板 `launcher_ui.py`，用于现场快速启动、监控 topic、检查环境和清理进程。
 - 自定义 PICO 4 Ultra UDP Sender Unity 工程，负责发送左右手柄 pose、`grip` 和 `trigger`。
 
@@ -126,7 +126,7 @@ ros2 run xr_rm_bringup launcher_ui
 每个模式都会附带三个监控入口：
 
 - `Open Controller Topic Monitor`：同时查看 `/xr/left_controller` 和 `/xr/right_controller`。
-- `Open Cmd Vel Topic Monitor`：同时查看 `/xr_rm/left_rm75/cmd_vel` 和 `/xr_rm/right_rm75/cmd_vel`。
+- `Open Target Velocity Monitor`：同时查看 `/xr_rm/left_rm75/cmd_vel` 和 `/xr_rm/right_rm75/cmd_vel`；该话题表示目标位姿变化率，仅用于调试。
 - `Open ROS Topic/Node List Monitor`：每秒刷新 `ros2 topic list` 和 `ros2 node list`。
 
 分屏监控依赖 `x-terminal-emulator` 指向 Terminator。若提示不支持，可安装并切换：
@@ -156,6 +156,8 @@ ros2 run xr_rm_input sample_udp_sender --hand both --host 127.0.0.1 --port 15000
 ```bash
 ros2 topic echo /xr/left_controller
 ros2 topic echo /xr/right_controller
+ros2 topic echo /xr_rm/left_rm75/target_pose
+ros2 topic echo /xr_rm/right_rm75/target_pose
 ros2 topic echo /xr_rm/left_rm75/cmd_vel
 ros2 topic echo /xr_rm/right_rm75/cmd_vel
 ```
@@ -194,9 +196,12 @@ ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false \
 - `use_mock`：`true` 不连接真机，`false` 连接 RM75。
 - `udp_host`：UDP 监听地址，默认 `0.0.0.0`。
 - `udp_port`：UDP 监听端口，默认 `15000`。
+- `udp_timer_hz`：UDP receiver 轮询频率，默认 `200.0`。
 - `left_robot_ip`：左臂 IP，默认 `192.168.192.18`。
 - `right_robot_ip`：右臂 IP，默认 `192.168.192.19`。
 - `robot_port`：RM75 TCP 端口，默认 `8080`。
+- `control_rate_hz`：`rm_movep_canfd` 目标位姿发送频率，默认 `90.0`。
+- `follow`：传给 `rm_movep_canfd` 的跟随标志，默认 `false`。
 - `enable_tool_control`：是否在遥操作节点内启用末端工具控制 topic，默认 `true`。
 - `configure_peripheral_on_connect`：遥操作节点连接真机后是否配置末端外设，默认 `true`；工具控制会复用同一个 RealMan 连接，避免两个进程同时抢占同一机械臂。
 - `move_to_initial_pose_on_connect`：连接后是否执行 `movej`/`movel` 初始化，默认 `false`。
@@ -216,11 +221,13 @@ ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false \
 
 - `controller_topic`：订阅的手柄话题。
 - `scale`：手柄位移到 TCP 位移的比例。
-- `kp_linear`：位置误差到速度命令的比例增益。
-- `max_linear_speed`：软件侧最大线速度。
+- `target_filter_alpha` / `target_filter_alpha_fast`：目标 TCP 低通滤波系数，快速移动时自动使用更大的系数。
+- `target_filter_fast_threshold_m`：进入快速滤波区间的目标变化阈值。
+- `max_linear_speed`：目标位姿单帧步长限制对应的最大线速度。
 - `workspace_min` / `workspace_max`：笛卡尔工作空间边界。
 - `cyl_radius_limit`：基座圆柱半径限制。
 - `xr_to_robot_matrix`：PICO/OpenXR 位移到 RM75 base 坐标的映射矩阵。
+- `current_pose_poll_hz`：低频读取真机当前 TCP 的频率；控制中不再每帧阻塞读取状态。
 - `mock_initial_pose`：mock 模式初始 TCP 位姿。
 - `initial_joint_pose` / `initial_tcp_pose`：可选真机初始化点。
 
@@ -300,7 +307,7 @@ 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` 后机械臂慢停，`/xr_rm/<arm>/cmd_vel` 回到零。
 7. 左右臂都确认后，再进入双臂模式。
 
 当前项目没有双臂碰撞检测。双臂首次联调时，请让两个工作区在物理上分开，低速验证，不要让两臂末端互相靠近。
@@ -309,7 +316,7 @@ PICO 4 Ultra 在 Ubuntu 22.04 下配置 Unity、构建 APK、安装到头显并
 
 为了达到“稳定可用的双臂 XR 遥操作/采摘平台”，建议按下面顺序推进：
 
-1. 稳定 PICO 数据链路：固定 UDP JSON 协议和坐标系，增加频率、延迟、丢包统计，记录 `/xr/*_controller`、`/xr_rm/*/cmd_vel`、`/xr_rm/*/current_pose`。
+1. 稳定 PICO 数据链路：固定 UDP JSON 协议和坐标系，增加频率、延迟、丢包统计，记录 `/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` 从预留字段变成稳定夹爪输入。
 4. 接入视觉和数据记录：加入 D405/D435 相机 launch、TF、内外参和 rosbag2 实验记录。
@@ -341,6 +348,6 @@ Controller topic 没有数据：
 机械臂不动：
 
 - 确认 `grip=true`。
-- 确认 `/xr_rm/<arm>/cmd_vel` 是否有非零速度。
-- 确认目标 TCP 没有被工作空间边界或圆柱半径限制夹住。
+- 确认 `/xr_rm/<arm>/raw_target_pose` 与 `/xr_rm/<arm>/target_pose` 是否在变化。
+- 确认 `/xr_rm/<arm>/target_clamped` 是否持续为 `true`，如果是，目标 TCP 可能被工作空间、圆柱半径或单帧步长限制夹住。
 - 确认真机 SDK 连接成功，且 RM75 没有报警或急停。