Refactor and enhance XR-RM teleoperation functionality

docs/pico_udp_sender_ubuntu22_setup.md

@@ -265,8 +265,8 @@ sudo ufw status
 - 面板中 `L ok / R ok` 表示 Unity 能读到左右手柄。
 - `UDP Sending ON` 后，ROS2 的 `/xr/left_controller` 与 `/xr/right_controller` 应持续刷新。
 - HUD 显示包计数、追踪状态、grip 和 KeepAwake 状态。
-- 按住 `grip` 并移动手柄时，mock 模式下 `/xr_rm/*/cmd_vel` 应出现非零速度。
-- 松开 `grip` 后，`cmd_vel` 应回到零速度。
+- 按住 `grip` 并移动手柄时，mock 模式下 `/xr_rm/*/target_pose` 应连续变化，`/xr_rm/*/cmd_vel` 会显示目标位姿变化率。
+- 松开 `grip` 后，机械臂慢停，`cmd_vel` 应回到零。
 
 ## 9. ROS2 端验证
 
@@ -335,16 +335,19 @@ source /opt/ros/humble/setup.bash
 source install/setup.bash
 ros2 topic echo /xr_rm/left_rm75/cmd_vel
 ros2 topic echo /xr_rm/right_rm75/cmd_vel
+# 需要看实际位姿目标时另开终端：
+ros2 topic echo /xr_rm/left_rm75/target_pose
+ros2 topic echo /xr_rm/right_rm75/target_pose
 ```
 
 流程：
 
 1. 启动 PICO Unity 应用。
 2. 确认 `/xr/left_controller` 和 `/xr/right_controller` 正常刷新。
-3. 左手按住 `grip`，只移动左手一小段，观察 `/xr_rm/left_rm75/cmd_vel`。
+3. 左手按住 `grip`，只移动左手一小段，观察 `/xr_rm/left_rm75/target_pose` 和 `/xr_rm/left_rm75/cmd_vel`。
 4. 松开左手 `grip`，确认 `cmd_vel` 回到 0。
 5. 右手重复同样流程。
-6. 用 `ros2 topic hz` 确认频率稳定，建议 `50 Hz` 以上。
+6. 用 `ros2 topic hz` 确认频率稳定，建议接近 PICO 端配置的发送频率。
 
 如果要排除 PICO 端问题，可用本机 sample sender 验证 ROS 端：
 
@@ -427,7 +430,7 @@ payload.quat[3] = rotation.w;
 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` 的方向。
+4. 记录 `/xr_rm/left_rm75/target_pose` 的方向。
 5. 右手重复。
 
 如果两个手柄在 ROS topic 里的某个轴都反了，优先检查 Unity 的坐标转换。如果 ROS topic 正确，但某一只机械臂运动方向不符合现场坐标，只改对应 YAML 的 `xr_to_robot_matrix`。
@@ -439,7 +442,7 @@ payload.quat[3] = rotation.w;
 - 急停可用。
 - 机械臂工作区清空。
 - PICO topic 在 mock 下已经稳定。
-- `grip=false` 时 `/xr_rm/<arm>/cmd_vel` 为 0。
+- `grip=false` 时机械臂慢停，`/xr_rm/<arm>/cmd_vel` 为 0。
 - `move_to_initial_pose_on_connect` 保持 `false`。
 
 单臂真机：