feat: 添加多摄像头测试工具，支持实时预览和快照功能

AGENTS.md

@@ -288,7 +288,7 @@ test: 添加 xxx 测试
 
 ## 项目专属规则
 
-* 本项目面向 Ubuntu 22.04 和 ROS2 Humble；构建、测试和运行命令应在工作空间根目录 `/home/robot/WS_xr` 执行，并先 `source /opt/ros/humble/setup.bash`。
+* 本项目面向 Ubuntu 22.04 和 ROS2 Humble；编译前必须先切换到工作空间根目录 `/home/robot/WS_xr`，并执行 `source /opt/ros/humble/setup.bash`。禁止在 `/home/robot/WS_xr/src` 中运行 `colcon build`，否则会在源码目录生成多余的 `build/`、`install/` 和 `log/`；测试和运行命令也应在工作空间根目录执行。
 * 工作空间包含 `xr_rm_input`、`xr_rm_teleop` 两个 `ament_python` 包，以及 `xr_rm_interfaces`、`xr_rm_bringup` 两个 `ament_cmake` 包；优先使用现有 ROS2 包、节点和消息，不要另建重复入口。
 * 修改 ROS 节点、launch、消息定义或安装配置后，至少运行 `colcon build --symlink-install`；涉及遥操作姿态控制时，再运行 `pytest src/xr_rm_teleop/test/test_orientation_control.py`。
 * 遥操作统一使用 `xr_rm_bringup/launch/arm_debug.launch.py`；调试和验证默认使用 `use_mock:=true`，未经用户明确要求不得连接真机、移动机械臂或操作夹爪。

README.md

@@ -1,44 +1,97 @@
-# XR-RM75 双臂遥操作
+# XR-RM75 双臂遥操作工作空间
 
-基于 **Ubuntu 22.04、ROS2 Humble、PICO 4 Ultra 和睿尔曼 RM75** 的双臂 XR 遥操作
-工作空间，支持单臂/双臂 Mock 与真机控制，以及 MuJoCo 运动学显示。
-
-> [!WARNING]
-> 真机命令会连接并控制机械臂。首次运行必须从 Mock 和单臂低速验证开始，确保急停
-> 可用且工作区无人。当前项目没有双臂碰撞检测或避障。
-
-## 当前能力
-
-- PICO/XR 双手柄 UDP 输入，相对位姿目标与 Placo QP 七关节控制。
-- 单臂/双臂 Mock 与真机、手柄/话题夹爪控制，以及只读 MuJoCo 双臂显示。
-- 工作空间/圆柱限位、速度限制、指令超时和安全慢停。
-- 统一 launch、Tkinter 启动面板、调试话题和 Mock 输入工具。
-
-尚未完成：D405/D435 视频流、数据记录、相机标定、目标检测、双臂碰撞避障、任务级状态机，以及 PICO 与 ROS 的完整时间同步和状态回传。
-
-## 系统架构
+本仓库是面向 **Ubuntu 22.04 + ROS2 Humble + PICO 4 Ultra + 睿尔曼 RM75** 的阶段一 XR 双臂遥操作项目。当前目标是先跑通一条低速、安全、可调试的闭环：
 
 ```text
-PICO / XRoboToolkit
-  -> UDP JSON
+PICO/XR 双手柄 UDP JSON
   -> xr_rm_input/udp_controller_receiver
-  -> /xr/left_controller、/xr/right_controller
+  -> /xr/left_controller 与 /xr/right_controller
   -> xr_rm_teleop/single_arm_velocity_teleop
-  -> 相对 TCP 目标 + Placo QP
-  -> Mock 或 RM75 rm_movej_canfd
-  -> joint_states / 调试话题
-       -> 可选 xr_rm_mujoco/dual_arm_simulator
+  -> Placo QP 单步逆解
+  -> 左右 RM75 七关节角透传控制
+  -> /xr_rm/<arm_name>/joint_states
+  -> 可选 xr_rm_mujoco/dual_arm_simulator
+  -> /xr_rm/<arm_name>/current_pose、raw_target_pose、target_pose、cmd_vel、target_clamped 调试话题
 ```
 
-工作空间包含五个 ROS2 包：`xr_rm_input` 负责手柄输入，`xr_rm_interfaces` 定义消息，
-`xr_rm_teleop` 实现遥操作与真机适配，`xr_rm_bringup` 提供启动和配置，
-`xr_rm_mujoco` 负责只读运动学显示。
+当前控制方式是“手柄相对位姿 + 单步 QP”遥操作：按住 `grip` 时锁定当前手柄位姿和机械臂 TCP 位姿，之后根据手柄相对位移和相对旋转生成目标 TCP。姿态目标使用旋转矩阵和 SO(3) 最短路径完成死区、滤波与限速，不经过 RPY。每个控制周期执行一次 Placo QP，并通过 `rm_movej_canfd` 下发 7 个关节目标。松开 `grip`、UDP 或关节反馈超时、节点退出时会请求机械臂慢停。
 
-`single_arm_velocity_teleop` 每个实例只控制一台机械臂；双臂模式分别启动 `left_arm_teleop` 和 `right_arm_teleop`。
+## 当前范围
 
-## 环境与构建
+已完成：
 
-在工作空间根目录执行：
+- PICO/XR 手柄 UDP 数据接收，并分发到左右手柄 ROS2 话题。
+- 通过统一的 `arm_debug.launch.py` 支持左臂、右臂、双臂的 mock 调试和真机调试。
+- 使用现有双臂 URDF 的 MuJoCo 运动学显示，可由 Mock 或真机反馈同步双臂姿态。
+- RM75 真机连接适配，包含关节反馈缓存、`rm_movej_canfd` 关节透传、安全速度/加速度配置、可选初始化点位移动。
+- Placo 0.9.4 RM75 QP 逆解；收到首帧有效关节反馈后才启用，求解失败时保留上一组有效关节目标。
+- 真机模式下，点击对应手柄 `trigger` 可切换并保持对应夹爪开/关状态。
+- Tkinter 启动面板 `launcher_ui.py`，用于现场快速启动、监控 topic、检查环境和清理进程。
+- XRoboToolkit bridge 读取左右手柄 pose、Grip、Trigger、摇杆和主副按键。
+
+暂未完成：
+
+- D405/D435 视频流、数据记录、相机标定和目标检测链路。
+- 双臂碰撞模型、任务级状态机、自动采摘策略。
+- PICO 端与 ROS 端的完整时间同步和状态回传。
+
+## 项目结构
+
+```text
+src/
+├── README.md                       # 项目主文档
+├── AGENTS.md                       # Codex 项目工作流和安全规则
+├── docs/superpowers/               # Superpowers 设计与实施计划
+├── xr_rm_bringup/
+│   ├── config/
+│   │   ├── dual_arm_mujoco.yaml    # MuJoCo 显示刷新参数
+│   │   ├── dual_arm_rm75.yaml      # 双臂配置：left_arm_teleop 与 right_arm_teleop
+│   │   ├── left_arm_rm75.yaml      # 左臂单独调试配置
+│   │   ├── right_arm_rm75.yaml     # 右臂单独调试配置
+│   │   └── peripherals_rm75.yaml   # 左右臂末端外设配置
+│   ├── launch/
+│   │   └── arm_debug.launch.py     # 统一入口：单臂/双臂、Mock/真机、可选 MuJoCo
+│   └── tools/
+│       ├── launcher_ui.py          # 图形化调试启动面板
+│       └── realman_dual_arm_state_monitor.py
+├── xr_rm_input/
+│   ├── launch/
+│   │   └── udp_receiver.launch.py  # 低层 UDP 接收测试入口
+│   ├── test/
+│   │   └── test_controller_fields.py
+│   └── xr_rm_input/
+│       ├── udp_controller_receiver.py
+│       ├── xrobotoolkit_to_udp_bridge.py
+│       └── sample_udp_sender.py    # 本机扫轴/正弦模拟手柄 UDP 数据
+├── xr_rm_interfaces/
+│   └── msg/
+│       └── XrController.msg        # 手柄状态与位姿
+├── xr_rm_mujoco/
+│   └── xr_rm_mujoco/
+│       └── dual_arm_simulator.py   # 双臂 URDF 运动学映射与 MuJoCo viewer
+└── xr_rm_teleop/
+    ├── models/
+    │   ├── rm75/                   # 旧 RM75 模型资源（launch 不再选用）
+    │   ├── rm75_omnipicker/        # 旧单臂 OmniPicker 模型资源
+    │   └── dual_rm75/              # 当前左右臂统一使用的双 RM75 URDF 与网格
+    └── xr_rm_teleop/
+        ├── placo_ik_solver.py      # Placo 0.9.4 单步 QP 逆解
+        ├── single_arm_velocity_teleop.py
+        ├── realman_adapter.py
+        └── fun_peripheral.py
+```
+
+`single_arm_velocity_teleop` 这个名字保留是有意的：双臂模式不是一个大节点直接控制两台机械臂，而是启动两个相同的单臂控制节点，分别命名为 `left_arm_teleop` 和 `right_arm_teleop`。
+
+## Superpowers Git 约束
+
+使用 Superpowers 执行任务时，只允许按 skill 工作流创建本地 Git 提交。
+禁止执行 `git push`、合并本地分支、合并 PR 或其他远程写操作。skill 如需
+独立 worktree 或配套本地分支，可以创建，但不得将其合并到其他分支。
+
+## 环境准备
+
+在工作空间根目录，也就是包含 `src/` 的目录执行：
 
 ```bash
 cd /home/robot/WS_xr
@@ -49,125 +102,440 @@ colcon build --symlink-install
 source install/setup.bash
 ```
 
-遥操作和 MuJoCo 节点固定使用 `/home/robot/miniconda3/envs/xr/bin/python`，
-其中固定 Placo 0.9.4、Pinocchio 3.7.0、NumPy 2.2.6 和 MuJoCo 3.10.0；不要从
-用户或系统 Python 覆盖这些版本。
+真机模式还需要安装睿尔曼 Python API2。若未安装，mock 模式仍可正常使用；真机启动时会提示缺少 `Robotic_Arm` 包。
 
-真机模式另需睿尔曼 Python API2；Mock 模式不依赖厂商 SDK。
+遥操作和 MuJoCo 节点固定由 `/home/robot/miniconda3/envs/xr/bin/python` 启动，并复用其中的 Python 3.10、Placo 0.9.4、Pinocchio 3.7.0、NumPy 2.2.6 和 MuJoCo 3.10.0。`ros2`、`colcon`、pytest 和 `udp_controller_receiver` 仍使用系统 Python。禁止通过 `pip --user`、`sudo pip` 或系统安装升级 Placo、Pinocchio、EigenPy 和 NumPy。
 
-## 快速开始
-
-以下命令均在 `/home/robot/WS_xr` 执行，并先 source ROS2 与 `install/setup.bash`。
-
-### Mock
+只读检查 Placo 版本：
 
 ```bash
-ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=true
+/home/robot/miniconda3/envs/xr/bin/python -c \
+  "import importlib.metadata; print(importlib.metadata.version('placo'))"
 ```
 
-另开终端发送模拟手柄数据：
+输出必须为 `0.9.4`。
+
+同时检查 MuJoCo：
 
 ```bash
-ros2 run xr_rm_input sample_udp_sender \
-  --hand both --host 127.0.0.1 --port 15000 \
+/home/robot/miniconda3/envs/xr/bin/python -c \
+  "import mujoco; print(mujoco.__version__)"
+```
+
+当前验证版本为 `3.10.0`。系统 pytest 会通过 `xr_rm_mujoco/test/conftest.py` 复用该固定 XR 环境中的 MuJoCo，因此新包可直接按 ROS2 标准方式测试：
+
+```bash
+colcon test --packages-select xr_rm_mujoco --event-handlers console_direct+
+colcon test-result --verbose
+```
+
+如果希望 `launcher_ui.py` 从任意目录找到工作空间，可以设置：
+
+```bash
+export XR_RM_WS=/home/robot/WS_xr
+```
+
+## 使用 launcher_ui.py 调试
+
+推荐现场调试优先使用图形化启动面板。它会自动进入工作空间、source ROS2 与 `install/setup.bash`，并把每个命令放到独立终端中运行。
+
+源码方式启动：
+
+```bash
+cd /home/robot/WS_xr
+python3 src/xr_rm_bringup/tools/launcher_ui.py
+```
+
+构建后也可以通过 ROS2 入口启动：
+
+```bash
+source /opt/ros/humble/setup.bash
+source install/setup.bash
+ros2 run xr_rm_bringup launcher_ui
+```
+
+面板顶部的 `Mode` 分为四类：
+
+- `Simulation`：双臂 mock、XRoboToolkit bridge、双手 sample UDP 和 controller 频率监控。
+- `MuJoCo`：双臂 Mock/真机 MuJoCo launch、XRoboToolkit bridge 和 controller 频率监控；真机命令会连接两台 RM75。
+- `Real Hardware`：左右臂网络 ping、左臂/右臂/双臂真机 launch、XRoboToolkit bridge 和左右夹爪开合。
+- `Diagnostics`：`ros2 doctor --report`、四个核心包的 `ros2 pkg prefix`、controller 位置/频率监控。
+
+常用按钮：
+
+- `Run Selected`：运行当前选中的命令。双击列表项也可以运行。
+- `Check Env`：检查 ROS2 Humble、工作空间 build、终端、四个核心 ROS 包、睿尔曼 API2。
+- `Stop All`：结束由本工作空间启动的 launch、sample sender、topic monitor、MuJoCo viewer、相关 ROS 节点和终端窗口。
+
+`Stop All` 会保留现有 PC Service；点击启动器窗口 `X` 并确认退出时会额外停止 PC Service。两条清理路径都会停止 `dual_arm_simulator`，关闭 MuJoCo viewer。
+
+每个模式都会附带基础监控入口：
+
+- `Open Controller Topic Monitor`：同时查看 `/xr/left_controller` 和 `/xr/right_controller`。
+- `Open ROS Topic/Node List Monitor`：每秒刷新 `ros2 topic list` 和 `ros2 node list`。
+
+`Simulation` 和 `MuJoCo` 模式还提供 `Open Controller Hz Monitor`；`Diagnostics` 同时提供 controller 位置与频率监控。
+
+分屏监控依赖 `x-terminal-emulator` 指向 Terminator。若提示不支持，可安装并切换：
+
+```bash
+sudo apt install terminator wmctrl xdotool
+sudo update-alternatives --config x-terminal-emulator
+```
+
+## 推荐调试顺序
+
+第一步：检查环境。
+
+打开 `launcher_ui.py`，点击 `Check Env`。如果 `install/setup.bash` 缺失，先回工作空间根目录重新执行 `colcon build --symlink-install`。
+
+第二步：分别跑左、右臂 mock 闭环。
+
+分两个终端依次验证左臂：
+
+```bash
+ros2 launch xr_rm_bringup arm_debug.launch.py arm:=left use_mock:=true
+ros2 run xr_rm_input sample_udp_sender --hand left --host 127.0.0.1 --port 15000 \
   --pattern axis_sweep --seconds 30
 ```
 
-单臂调试时将 `arm` 改为 `left` 或 `right`。推荐先分别完成左右单臂
-Mock，再进入双臂或真机验证。
-
-### MuJoCo
+停止左臂进程后，再分别验证右臂：
 
 ```bash
-ros2 launch xr_rm_bringup arm_debug.launch.py \
-  arm:=both use_mock:=true use_mujoco:=true
+ros2 launch xr_rm_bringup arm_debug.launch.py arm:=right use_mock:=true
+ros2 run xr_rm_input sample_udp_sender --hand right --host 127.0.0.1 --port 15000 \
+  --pattern axis_sweep --seconds 30
 ```
 
-MuJoCo 只订阅关节状态，不参与控制。真机显示可将 `use_mock` 改为 `false`，
-但该命令会同时连接两台 RM75。
+`sample_udp_sender` 默认使用 `axis_sweep` 扫轴轨迹，并在终端打印 `XR +X/-X/+Y/-Y/+Z/-Z` 标签。需要检查末端姿态时可增加 `--rotation-pattern rpy_steps --rotation-amplitude-deg 25`。
 
-### PICO 输入
-
-只保留一个 UDP 输入源，然后启动 XRoboToolkit bridge：
+观察：
 
 ```bash
-ros2 run xr_rm_input xrobotoolkit_to_udp_bridge \
-  --host 127.0.0.1 --port 15000 --hz 90
+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
 ```
 
-确认左右 topic 持续接收数据：
+第三步：单臂真机。
 
-```bash
-ros2 topic hz /xr/left_controller
-ros2 topic hz /xr/right_controller
-```
-
-图形启动面板可运行 `python3 src/xr_rm_bringup/tools/launcher_ui.py`，提供
-Simulation、MuJoCo、Real Hardware 和 Diagnostics 模式。
-
-### 真机
-
-确认对应 YAML 中 `move_to_initial_pose_on_connect: false`，再从单臂开始：
+先只上一个臂，确认网络、方向、急停和限幅：
 
 ```bash
 ros2 launch xr_rm_bringup arm_debug.launch.py arm:=left use_mock:=false
 ros2 launch xr_rm_bringup arm_debug.launch.py arm:=right use_mock:=false
 ```
 
-单臂方向、限位、急停、超时停止和夹爪均验证后，才能启动双臂：
+所有 YAML 默认都不会执行 `movej(initial_joint_pose)`。只有确认安全区清空后，才可在当前使用的
+`left_arm_rm75.yaml`、`right_arm_rm75.yaml` 或 `dual_arm_rm75.yaml` 中将
+`move_to_initial_pose_on_connect` 改为 `true`。
+
+第四步：双臂真机。
 
 ```bash
 ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false
 ```
 
-按住 `grip` 控制对应机械臂；松开后停止。点击 `trigger` 切换对应夹爪开/关。
-左手 X、右手 A 会请求对应机械臂回到配置的初始位姿；真机使用前必须清空安全区。
+双臂默认不会自动移动到初始化点；机器人地址、控制参数和初始化移动开关均从
+`dual_arm_rm75.yaml` 读取。
 
-## Launch 参数
+## Launch 入口说明
 
-统一入口为 `xr_rm_bringup/launch/arm_debug.launch.py`：
+`arm_debug.launch.py` 是当前唯一的遥操作 launch 主入口，`launcher_ui.py` 中的 Simulation、MuJoCo 和 Real Hardware launch 命令都调用它。
 
-| 参数 | 默认值 | 说明 |
-| --- | --- | --- |
-| `arm` | `right` | `left`、`right` 或 `both` |
-| `use_mock` | `true` | `false` 会连接真机 |
-| `use_mujoco` | `false` | 仅支持 `arm:=both` |
-| `udp_host` | `0.0.0.0` | UDP 监听地址 |
-| `udp_port` | `15000` | UDP 监听端口 |
-| `udp_timer_hz` | `200.0` | UDP receiver 轮询频率 |
+常用参数：
 
-## 配置
+- `arm`：`left`、`right`、`both`，默认 `right`。
+- `use_mock`：`true` 不连接真机，`false` 连接 RM75。
+- `use_mujoco`：`true` 额外启动双臂 MuJoCo 显示，默认 `false`，仅支持 `arm:=both`。
+- `udp_host`：UDP 监听地址，默认 `0.0.0.0`。
+- `udp_port`：UDP 监听端口，默认 `15000`。
+- `udp_timer_hz`：UDP receiver 轮询频率，默认 `200.0`。
 
-| 文件 | 用途 |
-| --- | --- |
-| `dual_arm_rm75.yaml` | 双臂节点、网络、控制与安全参数 |
-| `left_arm_rm75.yaml` | 左臂单独调试 |
-| `right_arm_rm75.yaml` | 右臂单独调试 |
-| `peripherals_rm75.yaml` | 工具坐标、负载和末端执行器 |
-| `dual_arm_mujoco.yaml` | MuJoCo 刷新频率 |
+机器人 IP/端口、控制频率、CANFD、限速、工具和初始化位姿等行为参数只由对应 YAML
+配置，launch 不再提供同名覆盖项。
 
-修改某一侧控制参数时，同时检查单臂和双臂 YAML 是否需要同步。必须保留工作空间/
-圆柱限位、线速度与角速度限制、关节速度/加速度限制、指令超时和安全停止逻辑。
+## MuJoCo 双臂仿真
 
-`configure_safety_limits` 不得默认关闭；
-`move_to_initial_pose_on_connect` 必须保持默认 `false`。
-
-## 测试
-
-在工作空间根目录执行：
+无真机时，由 Mock 关节状态驱动 MuJoCo：
 
 ```bash
+ros2 launch xr_rm_bringup arm_debug.launch.py \
+  arm:=both use_mock:=true use_mujoco:=true
+```
+
+连接真机时，由两台 RM75 的实际关节反馈同步 MuJoCo。下面命令会连接真机，执行前必须完成真机安全检查：
+
+```bash
+ros2 launch xr_rm_bringup arm_debug.launch.py \
+  arm:=both use_mock:=false use_mujoco:=true
+```
+
+桌面 UI 的 `MuJoCo` 模式分别提供上述 Mock 和真机命令，并明确标记会连接真机的 `Dual Arm MuJoCo Real Hardware Launch`。
+
+MuJoCo 只订阅当前关节状态，不参与控制，也不向真机下发指令：
+
+- `/xr_rm/left_rm75/joint_states`、`/xr_rm/right_rm75/joint_states`：当前适配器反馈；Mock 与真机模式均按控制周期约 `90 Hz` 发布。真机底层原始反馈周期仍为 `5 ms`（约 `200 Hz`），由遥操作节点按 `90 Hz` 采样发布。
+- `/xr_rm/left_rm75/joint_target`、`/xr_rm/right_rm75/joint_target`：经 QP 和现有限制处理后的目标关节角，仅用于调试，MuJoCo 不订阅。
+- MuJoCo viewer 默认按 `dual_arm_mujoco.yaml` 中的 `60 Hz` 刷新。其初始姿态直接来自 `dual_arm_rm75.yaml` 的 `initial_joint_pose`，不会在 MuJoCo YAML 中重复保存。
+
+Mock 模式的遥操作目标仍经过 `dual_arm_rm75.yaml` 中的工作空间、圆柱、线速度、角速度、关节速度/加速度、超时和停止限制。左手 X、右手 A 分别立即复位对应 Mock 机械臂；Grip 保持按下时，下一控制周期会重新锚定并继续遥操作。真机复位完成后仍需松开 Grip 才能恢复遥操作，且 `move_to_initial_pose_on_connect` 保持为 `false`，连接真机不会自动移动。
+
+## 配置文件说明
+
+`xr_rm_bringup/config/dual_arm_rm75.yaml` 是双臂配置主文件，包含两个 ROS 节点命名空间：
+
+- `left_arm_teleop`
+- `right_arm_teleop`
+
+`left_arm_rm75.yaml` 和 `right_arm_rm75.yaml` 用于 `arm_debug.launch.py arm:=left/right` 的单臂调试，因为单臂节点名是 `single_arm_velocity_teleop`。
+
+`dual_arm_mujoco.yaml` 只保存 MuJoCo viewer 刷新频率；双臂初始关节角和遥操作限制继续统一读取 `dual_arm_rm75.yaml`。
+
+`xr_rm_bringup/config/peripherals_rm75.yaml` 保存真实控制器使用的末端工具坐标、负载和左右臂外设选择。左臂 `scissorgripper: 2` 是外设选择值，选择 `minisci`，TCP 的 Z 向偏移为 `0.165 m`；右臂 `scissorgripper: 1`，选择 `omnipic`，TCP 的 Z 向偏移为 `0.14 m`。真机连接阶段仍会初始化外设，关节反馈、关节指令、慢停和开合命令复用该单臂节点的同一个 RealMan 连接。
+
+Placo 使用 `xr_rm_teleop/models/dual_rm75/Dual_arm.urdf`。左右 ROS 节点分别创建独立 solver：左臂从 `scissor_base_link` 到 `scissor_scissor_tcp`，并 mask 右臂；右臂从 `omnipic_base_link` 到 `omnipic_OmniPic_tcp`，并 mask 左臂。节点目标仍在各自局部基坐标系中，现有 PICO 映射不改为公共坐标系。
+
+重点控制参数：
+
+- `controller_topic`：订阅的手柄话题。
+- `scale`：手柄位移到 TCP 位移的比例。
+- `target_filter_alpha` / `target_filter_alpha_fast`：目标 TCP 低通滤波系数，快速移动时自动使用更大的系数。
+- `target_filter_fast_threshold_m`：进入快速滤波区间的目标变化阈值。
+- `max_linear_speed`：目标位姿单帧步长限制对应的最大线速度。
+- `enable_orientation_control`：是否把手柄相对旋转映射到 TCP 姿态。
+- `orientation_filter_alpha` / `orientation_deadband_rad`：按 SO(3) 最短旋转角处理的目标 TCP 姿态滤波和死区。
+- `max_orientation_speed`：目标 TCP 姿态沿 SO(3) 最短路径的最大角速度，当前为 `0.5 rad/s`。
+- `workspace_min` / `workspace_max`：笛卡尔工作空间边界。
+- `cyl_radius_limit`：基座圆柱半径限制。
+- `xr_to_robot_matrix`：`/xr/*_controller` Project 位移到 RM75 base 坐标的映射矩阵。
+- `robot_ip` / `robot_port`：RM75 TCP 控制连接地址。
+- `realtime_push_host_ip`：连接机械臂 Wi-Fi 后本机实际 IPv4；可用
+  `ip -4 route get 192.168.192.19` 查看输出中的 `src`，当前为 `192.168.192.148`。
+- `realtime_push_port`：UDP 主动反馈端口；左臂 `8089`、右臂 `8090`，同机双臂不能重复。
+- `realtime_push_cycle_ms`：UDP 主动反馈周期，当前为厂商支持的 `5 ms`。
+- `follow` / `canfd_trajectory_mode`：`rm_movej_canfd` 的高跟随和轨迹模式参数。
+- 当前三份 YAML 默认均使用 `follow: false` 完成安全基线验证；确认关节加速度与反馈稳定后，再单独测试高跟随。
+- `initial_joint_pose`：mock 的初始关节反馈，以及显式开启初始化移动时的真机初始关节角。
+
+当前 `/xr/*_controller` 的坐标处理：
+
+- XRoboToolkit bridge 原样转发 SDK 的手柄位置和四元数，不额外转换坐标轴。
+- receiver 默认按 `xyzw` 解析四元数，也可通过 `quat_order:=wxyz` 切换。
+- 左臂映射：机器人位移增量 = `[-手柄y, 手柄z, -手柄x]`。
+- 右臂映射：机器人位移增量 = `[手柄y, 手柄z, 手柄x]`。
+- 两侧局部 `-Y` 均指向机器人前方；局部 `+Y` 指向后方，后方工作空间仅保留 `0.10 m`。
+- 左臂局部 `+X/+Y/+Z` 分别指向下/后/左外侧；右臂分别指向上/后/右外侧。
+
+如果某个机械臂方向相反，只改对应臂的 `xr_to_robot_matrix` 符号，不要同时改多个控制参数。
+
+## 末端工具开合
+
+真机 launch 默认会在遥操作节点内启用工具控制。左/右手柄 `trigger` 从低于阈值按到 `>= 0.95` 时，会切换一次对应夹爪开/关状态，并保持到下一次点击。`grip` 仍只控制机械臂运动，不影响夹爪 trigger 切换。
+
+也可以用 Bool 话题手动控制开合，`true` 表示打开，`false` 表示闭合：
+
+```bash
+ros2 topic pub --once /xr_rm/left_rm75/tool_enable std_msgs/msg/Bool "{data: true}"
+ros2 topic pub --once /xr_rm/left_rm75/tool_enable std_msgs/msg/Bool "{data: false}"
+
+ros2 topic pub --once /xr_rm/right_rm75/tool_enable std_msgs/msg/Bool "{data: true}"
+ros2 topic pub --once /xr_rm/right_rm75/tool_enable std_msgs/msg/Bool "{data: false}"
+```
+
+桌面 UI 的 `Real Hardware` 模式提供 `Left/Right Gripper Open/Close` 命令项；运行双臂真机 launch 时也可直接通过左右手柄 `trigger` 分别切换夹爪。
+
+## UDP 数据格式
+
+当前 XRoboToolkit bridge 每个周期发送一个双手柄 JSON 包：
+
+```json
+{
+  "t": 12.345,
+  "source_time": 12.345,
+  "seq": 42,
+  "frame_id": "xr_world",
+  "controllers": {
+    "left": {
+      "hand": "left",
+      "grip": true,
+      "trigger": 0.0,
+      "axis": [0.2, -0.4],
+      "buttons": {
+        "primary": true,
+        "secondary": false
+      },
+      "pos": [-0.12, 1.05, 0.30],
+      "quat": [0.0, 0.0, 0.0, 1.0],
+      "pose_valid": true,
+      "pose_source": "xrobotoolkit"
+    },
+    "right": {
+      "hand": "right",
+      "grip": true,
+      "trigger": 1.0,
+      "axis": [-0.1, 0.3],
+      "buttons": {
+        "primary": false,
+        "secondary": true
+      },
+      "pos": [0.12, 1.05, 0.30],
+      "quat": [0.0, 0.0, 0.0, 1.0],
+      "pose_valid": true,
+      "pose_source": "xrobotoolkit"
+    }
+  }
+}
+```
+
+字段说明：
+
+- `t` / `source_time`：bridge 的 PC 单调时间，用于诊断发送周期。
+- `seq`：bridge 递增的 UDP 包序号，bridge 重启后重新计数。
+- `frame_id`：默认 `xr_world`，会写入 `XrController.header.frame_id`。
+- `grip`：运动使能。`true` 时进入相对位姿控制，`false` 时停止。
+- `trigger`：经过 bridge 滞回处理的 `0.0/1.0` 值；上升沿切换对应夹爪状态。
+- `axis`：摇杆 `[x, y]`，每个分量限制在 `-1.0` 到 `1.0`。
+- `buttons.primary`：左手 X 键或右手 A 键。
+- `buttons.secondary`：左手 Y 键或右手 B 键。
+- `pos`：手柄位置，长度 3。
+- `quat`：手柄姿态四元数，默认按 `xyzw` 解析。
+- `pose_valid`：姿态是否可信；`false` 时接收端强制 `grip=false`。
+- `pose_source`：当前 bridge 使用 `xrobotoolkit`。
+
+`axis`、`buttons.primary` 和 `buttons.secondary` 会进入 `XrController`；旧 UDP
+包缺少这些字段时分别回退为 `[0,0]`、`false` 和 `false`。
+
+接收端发布的消息格式为：
+
+```text
+std_msgs/Header header
+string hand
+
+bool grip
+float32 trigger
+bool primary
+bool secondary
+float32[2] axis
+
+geometry_msgs/Pose pose
+```
+
+`udp_controller_receiver` 仍兼容调试用的单手柄包：可以直接发送带 `hand`、`pos`、
+`quat` 的 JSON object，也可以用 `controllers` list、顶层 `left/right`、
+`pose.position`、`position`、`p`、`q` 等常见字段。
+
+## 官方 XRoboToolkit bridge
+
+如果使用官方 XRoboToolkit APK 和 PC-Service，可以用 `xrobotoolkit_to_udp_bridge` 从本机 ROS Python 环境中的 `xrobotoolkit_sdk` 读取左右手柄数据，再转换成当前 `udp_controller_receiver` 支持的 UDP JSON。
+
+正式运行时不要同时启动官方 `PXREAClientUnity` / `RobotLinuxDemo` 可视化窗口。`/opt/apps/roboticsservice/run3D.sh` 会启动这个可视化 demo，适合单独确认 PICO 与 PC-Service 已连接；bridge 遥操作链路中只需要 PC-Service。
+
+运行前只保留一个 UDP 输入源。先清掉重复 bridge、sample sender 和官方 Unity 可视化 demo，再保留或启动 PC-Service：
+
+```bash
+pkill -f '[x]robotoolkit_to_udp_bridge'
+pkill -f '[s]ample_udp_sender'
+pkill -f '[R]obotLinuxDemo.x86_64'
+pkill -f '[P]XREAClientUnity'
+pgrep -af RoboticsServiceProcess || /opt/apps/roboticsservice/runService.sh
+```
+
+启动 ROS mock 接收链路：
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+source install/setup.bash
+ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=true
+```
+
+另开终端启动 bridge：
+
+```bash
+cd /home/robot/WS_xr
+source ~/.bashrc
+source /opt/ros/humble/setup.bash
+source install/setup.bash
+ros2 run xr_rm_input xrobotoolkit_to_udp_bridge \
+  --host 127.0.0.1 --port 15000 --hz 90
+```
+
+bridge 默认对 grip/trigger 做轻量滞回：`grip` 按下阈值 `0.90`、松开阈值 `0.75`；`trigger` 按下阈值 `0.95`、松开阈值 `0.75`。启动日志会打印 PID、UDP endpoint 和阈值，便于确认当前只运行了一个 bridge。
+
+验证手柄数据是否进入 ROS：
+
+```bash
+ps -ef | grep -E 'xrobotoolkit_to_udp_bridge|sample_udp_sender|RobotLinuxDemo|PXREAClientUnity' | grep -v grep
+ros2 topic hz /xr/left_controller
+ros2 topic hz /xr/right_controller
+ros2 topic echo /xr/left_controller --field pose.position
+ros2 topic echo /xr/right_controller --field pose.position
+ros2 topic echo /xr/left_controller --field grip
+ros2 topic echo /xr/right_controller --field grip
+ros2 topic echo /xr/right_controller --field trigger
+```
+
+`/xr/left_controller` 和 `/xr/right_controller` 持续刷新、位置随手柄移动变化、`grip` 随握持键切换，即表示官方 XRoboToolkit 数据已经进入当前遥操作输入层。
+
+## 真机安全验证
+
+第一次接真机时按这个顺序走：
+
+1. 确认急停、网络、机械臂工作区和人员位置。
+2. `launcher_ui.py` 中先 `Ping Left RM75` 或 `Ping Right RM75`。
+3. 确认对应 YAML 中 `move_to_initial_pose_on_connect: false` 后单臂启动。
+4. 手握急停，按住 `grip` 后只做小幅单轴移动。
+5. 逐个确认上/下、前/后、左/右方向。
+6. 小角度转动手柄，确认 `/xr_rm/<arm>/target_pose` 姿态和 `/xr_rm/<arm>/cmd_vel.twist.angular` 变化符合预期。
+7. 点击对应 `trigger`，确认每次点击都会切换对应夹爪状态，松开 trigger 后状态保持且左右不串臂。
+8. 确认松开 `grip` 后机械臂慢停，`/xr_rm/<arm>/cmd_vel` 回到零；trigger 仍只影响夹爪，不影响机械臂运动门控。
+9. 左右臂都确认后，再运行 `Dual Arm RealMan Launch`。
+
+当前项目没有双臂碰撞检测/避障。双臂首次联调时，请让两个工作区在物理上分开，低速验证，不要让两臂末端互相靠近。
+
+## 后续优化路线
+
+为了达到“稳定可用的双臂 XR 遥操作/采摘平台”，建议按下面顺序推进：
+
+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. 细化末端执行器：增加夹爪状态反馈、力控比例、安全上限和现场可视化提示。
+4. 接入视觉和数据记录：加入 D405/D435 相机 launch、TF、内外参和 rosbag2 实验记录。
+5. 从遥操作走向半自动：先做目标检测和 3D 定位提示，再做单臂辅助，最后做双臂任务分配和任务级状态机。
+
+## 常见问题
+
+`launcher_ui.py` 提示找不到 `install/setup.bash`：
+
+```bash
+cd /home/robot/WS_xr
 source /opt/ros/humble/setup.bash
 colcon build --symlink-install
-colcon test --event-handlers console_direct+
-colcon test-result --verbose
+source install/setup.bash
 ```
 
-涉及遥操作姿态控制时，额外运行：
+真机模式提示缺少 `Robotic_Arm`：
 
-```bash
-pytest src/xr_rm_teleop/test/test_orientation_control.py
+```text
+未安装睿尔曼 Python API2。请安装厂商 SDK，或用 use_mock:=true 先跑模拟模式。
 ```
 
-真机验证不属于自动测试。默认使用 `use_mock:=true`，未经现场安全确认不要连接或
-移动机械臂。
+Controller topic 没有数据：
+
+- 确认 UDP 发送端目标 IP 是运行 ROS2 的主机 IP。
+- 确认端口是 `15000`，或 launch 与发送端端口一致。
+- 用 `sample_udp_sender` 在本机验证接收链路。
+- 确认 `xrobotoolkit_to_udp_bridge` 没有持续打印 SDK read failed；SDK
+  读取失败时 bridge 会发送 `pose_valid=false` 的停止包。
+
+机械臂不动：
+
+- 确认 `grip=true`。
+- 确认 `udp_controller_receiver` 终端没有持续 `pose_valid=false` 日志；该字段不会写入 `XrController` 消息，但会让接收端强制停止。
+- 确认 `/xr_rm/<arm>/raw_target_pose` 与 `/xr_rm/<arm>/target_pose` 是否在变化。
+- 确认 `/xr_rm/<arm>/target_clamped` 是否持续为 `true`，如果是，目标 TCP 可能被工作空间、圆柱半径或单帧步长限制夹住。
+- 确认真机 SDK 连接成功，且 RM75 没有报警或急停。

xr_rm_bringup/test/camera_test_output/.gitignore

@@ -0,0 +1,2 @@
+*
+!.gitignore

xr_rm_bringup/test/test_realsense_multi_camera.py

@@ -0,0 +1,76 @@
+from __future__ import annotations
+
+import importlib.util
+from pathlib import Path
+import sys
+
+import pytest
+
+
+MODULE_PATH = (
+    Path(__file__).resolve().parents[1]
+    / "tools"
+    / "realsense_multi_camera_test.py"
+)
+SPEC = importlib.util.spec_from_file_location("realsense_multi_camera_test", MODULE_PATH)
+assert SPEC is not None and SPEC.loader is not None
+camera_test = importlib.util.module_from_spec(SPEC)
+sys.modules[SPEC.name] = camera_test
+SPEC.loader.exec_module(camera_test)
+
+
+def devices() -> list[camera_test.DeviceInfo]:
+    return [
+        camera_test.DeviceInfo("Intel RealSense D405", "D405", "412622272532", "3.2"),
+        camera_test.DeviceInfo("Intel RealSense D455", "D455", "234222303366", "3.2"),
+        camera_test.DeviceInfo("Intel RealSense D405", "D405", "260322272273", "3.2"),
+    ]
+
+
+def test_assigns_camera_roles_with_and_without_left_serial() -> None:
+    unidentified = camera_test.assign_camera_roles(devices(), None)
+    assert [camera.role for camera in unidentified] == ["GLOBAL", "D405-A", "D405-B"]
+    assert [camera.serial for camera in unidentified[1:]] == ["260322272273", "412622272532"]
+
+    identified = camera_test.assign_camera_roles(devices(), "412622272532")
+    assert {camera.role: camera.serial for camera in identified} == {
+        "GLOBAL": "234222303366",
+        "LEFT": "412622272532",
+        "RIGHT": "260322272273",
+    }
+
+
+def test_rejects_invalid_camera_selection() -> None:
+    with pytest.raises(ValueError, match="左臂序列号"):
+        camera_test.assign_camera_roles(devices(), "missing")
+
+    with pytest.raises(ValueError, match="2 台 D405 和 1 台 D455"):
+        camera_test.assign_camera_roles(devices()[:-1], None)
+
+
+def test_counts_frame_number_gaps() -> None:
+    stats = camera_test.FrameStats(target_fps=30, start_time=0.0)
+    stats.update(10, 0.0)
+    stats.update(11, 1.0 / 30.0)
+    stats.update(14, 2.0 / 30.0)
+
+    assert stats.received == 3
+    assert stats.dropped == 2
+    assert stats.drop_rate == pytest.approx(0.4)
+    assert stats.average_fps(2.0 / 30.0) == pytest.approx(30.0)
+
+
+def test_snapshot_names_follow_camera_roles() -> None:
+    identified = camera_test.assign_camera_roles(devices(), "412622272532")
+    assert camera_test.snapshot_filenames(identified) == {
+        "234222303366": "global.png",
+        "412622272532": "left.png",
+        "260322272273": "right.png",
+    }
+
+    unidentified = camera_test.assign_camera_roles(devices(), None)
+    assert camera_test.snapshot_filenames(unidentified) == {
+        "234222303366": "global.png",
+        "260322272273": "d405_260322272273.png",
+        "412622272532": "d405_412622272532.png",
+    }

xr_rm_bringup/tools/realsense_multi_camera_test.py

@@ -0,0 +1,494 @@
+#!/usr/bin/env python3
+"""同时预览并检查两台 D405 和一台 D455 的彩色画面。"""
+
+from __future__ import annotations
+
+import argparse
+from collections import deque
+from dataclasses import dataclass, field
+from datetime import datetime
+from pathlib import Path
+import threading
+import time
+from typing import Any
+
+
+DEFAULT_WIDTH = 640
+DEFAULT_HEIGHT = 480
+DEFAULT_FPS = 30
+DEFAULT_LEFT_SERIAL = "260322272273"
+FPS_WINDOW_SECONDS = 2.0
+WARMUP_SECONDS = 5.0
+MAX_DROP_RATE = 0.01
+MIN_FPS_RATIO = 0.9
+PREVIEW_TILE_WIDTH = 640
+WINDOW_NAME = "XR RM - Three RealSense Camera Test"
+OUTPUT_DIR = Path(__file__).resolve().parents[1] / "test" / "camera_test_output"
+
+
+@dataclass(frozen=True)
+class DeviceInfo:
+    name: str
+    model: str
+    serial: str
+    usb_type: str
+
+
+@dataclass(frozen=True)
+class CameraAssignment:
+    role: str
+    name: str
+    model: str
+    serial: str
+    usb_type: str
+
+
+def assign_camera_roles(
+    devices: list[DeviceInfo], left_serial: str | None
+) -> list[CameraAssignment]:
+    d405 = sorted(
+        (device for device in devices if device.model == "D405"),
+        key=lambda device: device.serial,
+    )
+    d455 = [device for device in devices if device.model == "D455"]
+    if len(d405) != 2 or len(d455) != 1 or len(devices) != 3:
+        raise ValueError(
+            f"需要连接 2 台 D405 和 1 台 D455，当前识别到 "
+            f"{len(d405)} 台 D405、{len(d455)} 台 D455、共 {len(devices)} 台 RealSense"
+        )
+
+    for device in devices:
+        if not device.usb_type.startswith("3"):
+            raise ValueError(
+                f"{device.model} ({device.serial}) 当前为 USB {device.usb_type}，"
+                "请检查扩展坞和数据线"
+            )
+
+    global_camera = CameraAssignment("GLOBAL", **d455[0].__dict__)
+    if left_serial is None:
+        arms = [
+            CameraAssignment(f"D405-{suffix}", **device.__dict__)
+            for suffix, device in zip(("A", "B"), d405)
+        ]
+    else:
+        matches = [device for device in d405 if device.serial == left_serial]
+        if not matches:
+            raise ValueError(f"左臂序列号 {left_serial} 不属于当前连接的 D405")
+        left = matches[0]
+        right = next(device for device in d405 if device.serial != left_serial)
+        arms = [
+            CameraAssignment("LEFT", **left.__dict__),
+            CameraAssignment("RIGHT", **right.__dict__),
+        ]
+    return [global_camera, *arms]
+
+
+def snapshot_filenames(assignments: list[CameraAssignment]) -> dict[str, str]:
+    role_names = {
+        "GLOBAL": "global.png",
+        "LEFT": "left.png",
+        "RIGHT": "right.png",
+    }
+    return {
+        camera.serial: role_names.get(camera.role, f"d405_{camera.serial}.png")
+        for camera in assignments
+    }
+
+
+@dataclass
+class FrameStats:
+    target_fps: int
+    start_time: float
+    received: int = 0
+    dropped: int = 0
+    last_frame_number: int | None = None
+    first_frame_time: float | None = None
+    recent_times: deque[float] = field(default_factory=deque)
+
+    def update(self, frame_number: int, now: float) -> None:
+        if self.last_frame_number is not None and frame_number > self.last_frame_number:
+            self.dropped += max(0, frame_number - self.last_frame_number - 1)
+        self.last_frame_number = frame_number
+        self.received += 1
+        if self.first_frame_time is None:
+            self.first_frame_time = now
+        self.recent_times.append(now)
+        cutoff = now - FPS_WINDOW_SECONDS
+        while self.recent_times and self.recent_times[0] < cutoff:
+            self.recent_times.popleft()
+
+    @property
+    def drop_rate(self) -> float:
+        expected = self.received + self.dropped
+        return self.dropped / expected if expected else 0.0
+
+    @property
+    def rolling_fps(self) -> float:
+        if len(self.recent_times) < 2:
+            return 0.0
+        elapsed = self.recent_times[-1] - self.recent_times[0]
+        return (len(self.recent_times) - 1) / elapsed if elapsed > 0 else 0.0
+
+    def average_fps(self, now: float) -> float:
+        if self.received < 2 or self.first_frame_time is None:
+            return 0.0
+        last_frame_time = self.recent_times[-1] if self.recent_times else now
+        elapsed = last_frame_time - self.first_frame_time
+        return (self.received - 1) / elapsed if elapsed > 0 else 0.0
+
+
+@dataclass(frozen=True)
+class CameraState:
+    frame: Any | None
+    actual_size: tuple[int, int]
+    received: int
+    dropped: int
+    drop_rate: float
+    rolling_fps: float
+    average_fps: float
+    elapsed: float
+    error: str
+
+
+class CameraWorker:
+    def __init__(
+        self,
+        assignment: CameraAssignment,
+        width: int,
+        height: int,
+        fps: int,
+        rs: Any,
+        np: Any,
+    ) -> None:
+        self.assignment = assignment
+        self.width = width
+        self.height = height
+        self.fps = fps
+        self._rs = rs
+        self._np = np
+        self._pipeline = rs.pipeline()
+        self._stop_event = threading.Event()
+        self._lock = threading.Lock()
+        self._thread: threading.Thread | None = None
+        self._frame: Any | None = None
+        self._actual_size = (width, height)
+        self._stats = FrameStats(fps, time.monotonic())
+        self._error = ""
+
+    def start(self) -> None:
+        config = self._rs.config()
+        config.enable_device(self.assignment.serial)
+        config.enable_stream(
+            self._rs.stream.color,
+            self.width,
+            self.height,
+            self._rs.format.bgr8,
+            self.fps,
+        )
+        profile = self._pipeline.start(config)
+        video_profile = profile.get_stream(self._rs.stream.color).as_video_stream_profile()
+        with self._lock:
+            self._actual_size = (video_profile.width(), video_profile.height())
+            self._stats = FrameStats(self.fps, time.monotonic())
+        self._thread = threading.Thread(
+            target=self._capture_loop,
+            name=f"camera-{self.assignment.serial}",
+            daemon=True,
+        )
+        self._thread.start()
+
+    def _capture_loop(self) -> None:
+        while not self._stop_event.is_set():
+            try:
+                frames = self._pipeline.wait_for_frames(timeout_ms=1000)
+            except RuntimeError as exc:
+                if self._stop_event.is_set():
+                    return
+                with self._lock:
+                    self._error = str(exc)
+                return
+
+            color_frame = frames.get_color_frame()
+            if not color_frame:
+                continue
+            frame = self._np.asanyarray(color_frame.get_data()).copy()
+            now = time.monotonic()
+            with self._lock:
+                self._frame = frame
+                self._stats.update(color_frame.get_frame_number(), now)
+
+    def state(self, now: float) -> CameraState:
+        with self._lock:
+            return CameraState(
+                frame=self._frame,
+                actual_size=self._actual_size,
+                received=self._stats.received,
+                dropped=self._stats.dropped,
+                drop_rate=self._stats.drop_rate,
+                rolling_fps=self._stats.rolling_fps,
+                average_fps=self._stats.average_fps(now),
+                elapsed=now - self._stats.start_time,
+                error=self._error,
+            )
+
+    def stop(self) -> None:
+        self._stop_event.set()
+        if self._thread is not None:
+            self._thread.join(timeout=1.2)
+        try:
+            self._pipeline.stop()
+        except RuntimeError:
+            pass
+        if self._thread is not None and self._thread.is_alive():
+            self._thread.join(timeout=1.0)
+
+
+def load_runtime_dependencies() -> tuple[Any, Any, Any]:
+    try:
+        import cv2
+        import numpy as np
+        import pyrealsense2 as rs
+    except ImportError as exc:
+        raise RuntimeError(
+            "缺少相机测试依赖。请使用 /home/robot/miniconda3/envs/xr/bin/python "
+            "运行，并确认 xr 环境已安装 pyrealsense2、numpy 和 opencv-python。"
+        ) from exc
+    return cv2, np, rs
+
+
+def enumerate_devices(rs: Any) -> list[DeviceInfo]:
+    devices = []
+    for device in rs.context().query_devices():
+        name = device.get_info(rs.camera_info.name)
+        if "D405" in name:
+            model = "D405"
+        elif "D455" in name:
+            model = "D455"
+        else:
+            model = name
+        usb_type = (
+            device.get_info(rs.camera_info.usb_type_descriptor)
+            if device.supports(rs.camera_info.usb_type_descriptor)
+            else "unknown"
+        )
+        devices.append(
+            DeviceInfo(
+                name=name,
+                model=model,
+                serial=device.get_info(rs.camera_info.serial_number),
+                usb_type=usb_type,
+            )
+        )
+    return devices
+
+
+def camera_status(state: CameraState, target_fps: int) -> tuple[str, tuple[int, int, int]]:
+    if state.error:
+        return "ERROR", (0, 0, 255)
+    if state.received == 0:
+        return "WAITING", (0, 215, 255)
+    if state.elapsed < WARMUP_SECONDS:
+        return "WARMUP", (0, 215, 255)
+    if state.rolling_fps >= target_fps * MIN_FPS_RATIO and state.drop_rate <= MAX_DROP_RATE:
+        return "PASS", (0, 200, 0)
+    return "FAIL", (0, 0, 255)
+
+
+def render_tile(
+    worker: CameraWorker,
+    state: CameraState,
+    tile_width: int,
+    tile_height: int,
+    cv2: Any,
+    np: Any,
+) -> Any:
+    if state.frame is None:
+        tile = np.zeros((tile_height, tile_width, 3), dtype=np.uint8)
+    else:
+        tile = cv2.resize(state.frame, (tile_width, tile_height))
+
+    status, color = camera_status(state, worker.fps)
+    cv2.rectangle(tile, (0, 0), (tile_width, 100), (0, 0, 0), -1)
+    width, height = state.actual_size
+    lines = [
+        f"{worker.assignment.role}  {worker.assignment.model}  {worker.assignment.serial}",
+        f"USB {worker.assignment.usb_type}  {width}x{height}@{worker.fps}",
+        f"FPS {state.rolling_fps:.1f}  Frames {state.received}  "
+        f"Dropped {state.dropped} ({state.drop_rate:.2%})",
+        status if not state.error else f"ERROR: {state.error[:70]}",
+    ]
+    for index, line in enumerate(lines):
+        cv2.putText(
+            tile,
+            line,
+            (10, 22 + index * 24),
+            cv2.FONT_HERSHEY_SIMPLEX,
+            0.55,
+            color if index == len(lines) - 1 else (255, 255, 255),
+            1,
+            cv2.LINE_AA,
+        )
+    return tile
+
+
+def compose_preview(
+    workers: list[CameraWorker],
+    states: dict[str, CameraState],
+    capture_width: int,
+    capture_height: int,
+    cv2: Any,
+    np: Any,
+) -> Any:
+    tile_width = min(capture_width, PREVIEW_TILE_WIDTH)
+    tile_height = round(tile_width * capture_height / capture_width)
+    tiles = {
+        worker.assignment.serial: render_tile(
+            worker,
+            states[worker.assignment.serial],
+            tile_width,
+            tile_height,
+            cv2,
+            np,
+        )
+        for worker in workers
+    }
+    global_worker = next(worker for worker in workers if worker.assignment.role == "GLOBAL")
+    arm_workers = [worker for worker in workers if worker.assignment.role != "GLOBAL"]
+
+    top = np.zeros((tile_height, tile_width * 2, 3), dtype=np.uint8)
+    offset = tile_width // 2
+    top[:, offset : offset + tile_width] = tiles[global_worker.assignment.serial]
+    bottom = np.hstack([tiles[worker.assignment.serial] for worker in arm_workers])
+    return np.vstack((top, bottom))
+
+
+def save_snapshots(
+    assignments: list[CameraAssignment],
+    states: dict[str, CameraState],
+    cv2: Any,
+) -> Path:
+    missing = [camera.role for camera in assignments if states[camera.serial].frame is None]
+    if missing:
+        raise RuntimeError(f"以下相机尚无有效画面，不能保存快照: {', '.join(missing)}")
+
+    timestamp = datetime.now().strftime("%Y%m%d_%H%M%S_%f")[:-3]
+    snapshot_dir = OUTPUT_DIR / timestamp
+    snapshot_dir.mkdir(parents=True, exist_ok=False)
+    filenames = snapshot_filenames(assignments)
+    for camera in assignments:
+        path = snapshot_dir / filenames[camera.serial]
+        if not cv2.imwrite(str(path), states[camera.serial].frame):
+            raise RuntimeError(f"保存快照失败: {path}")
+    return snapshot_dir
+
+
+def print_summary(workers: list[CameraWorker]) -> bool:
+    now = time.monotonic()
+    print("\n相机测试汇总:")
+    passed = True
+    for worker in workers:
+        state = worker.state(now)
+        status, _color = camera_status(state, worker.fps)
+        passed = passed and status == "PASS"
+        print(
+            f"  {worker.assignment.role:<7} {worker.assignment.serial}: "
+            f"平均 {state.average_fps:.1f} FPS, 接收 {state.received}, "
+            f"掉帧 {state.dropped} ({state.drop_rate:.2%}), {status}"
+        )
+    print("结论: " + ("三路链路满足当前阈值" if passed else "至少一路未满足当前阈值"))
+    return passed
+
+
+def parse_args() -> argparse.Namespace:
+    parser = argparse.ArgumentParser(description=__doc__)
+    parser.add_argument("--width", type=int, default=DEFAULT_WIDTH, help="采集宽度")
+    parser.add_argument("--height", type=int, default=DEFAULT_HEIGHT, help="采集高度")
+    parser.add_argument("--fps", type=int, default=DEFAULT_FPS, help="目标帧率")
+    parser.add_argument(
+        "--left-serial",
+        default=DEFAULT_LEFT_SERIAL,
+        help=f"左臂 D405 的 RealSense 序列号（默认: {DEFAULT_LEFT_SERIAL}）",
+    )
+    args = parser.parse_args()
+    if args.width <= 0 or args.height <= 0 or args.fps <= 0:
+        parser.error("width、height 和 fps 必须为正数")
+    return args
+
+
+def main() -> int:
+    args = parse_args()
+    try:
+        cv2, np, rs = load_runtime_dependencies()
+        assignments = assign_camera_roles(enumerate_devices(rs), args.left_serial)
+    except RuntimeError as exc:
+        print(f"错误: {exc}")
+        return 2
+    except ValueError as exc:
+        print(f"设备检查失败: {exc}")
+        return 2
+
+    print("相机分配:")
+    for camera in assignments:
+        print(
+            f"  {camera.role:<7} {camera.model}  serial={camera.serial}  "
+            f"USB={camera.usb_type}"
+        )
+
+    workers: list[CameraWorker] = []
+    passed = False
+    try:
+        for assignment in assignments:
+            worker = CameraWorker(
+                assignment,
+                args.width,
+                args.height,
+                args.fps,
+                rs,
+                np,
+            )
+            workers.append(worker)
+            worker.start()
+
+        cv2.namedWindow(WINDOW_NAME, cv2.WINDOW_NORMAL)
+        print("按 S 保存三路快照，按 Q 或 Esc 退出。")
+        while True:
+            now = time.monotonic()
+            states = {worker.assignment.serial: worker.state(now) for worker in workers}
+            preview = compose_preview(
+                workers,
+                states,
+                args.width,
+                args.height,
+                cv2,
+                np,
+            )
+            cv2.imshow(WINDOW_NAME, preview)
+            key = cv2.waitKey(1) & 0xFF
+            if key in (ord("q"), ord("Q"), 27):
+                break
+            if key in (ord("s"), ord("S")):
+                try:
+                    output = save_snapshots(assignments, states, cv2)
+                    print(f"快照已保存: {output}")
+                except RuntimeError as exc:
+                    print(f"快照失败: {exc}")
+            if cv2.getWindowProperty(WINDOW_NAME, cv2.WND_PROP_VISIBLE) < 1:
+                break
+    except KeyboardInterrupt:
+        print("\n收到 Ctrl+C，正在停止相机。")
+    except Exception as exc:
+        print(f"相机启动或显示失败: {exc}")
+    finally:
+        for worker in reversed(workers):
+            worker.stop()
+        try:
+            cv2.destroyAllWindows()
+        except Exception:
+            pass
+        if workers:
+            passed = print_summary(workers)
+    return 0 if passed else 1
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())

xr_rm_teleop/xr_rm_teleop/realman_adapter.py

@@ -237,9 +237,9 @@ class RealManAdapter:
             )
 
     def read_joint_state(self) -> JointStateSnapshot:
-        self._require_arm()
+        arm = self._require_arm()
         started_at = time.monotonic()
-        result = self._arm.rm_get_joint_degree()
+        result = arm.rm_get_joint_degree()
         finished_at = time.monotonic()
         if not isinstance(result, tuple) or len(result) != 2:
             raise RuntimeError(
@@ -256,10 +256,10 @@ class RealManAdapter:
         )
 
     def send_joint_target(self, joints: list[float], follow: bool) -> None:
-        self._require_arm()
+        arm = self._require_arm()
         if len(joints) != 7 or not all(math.isfinite(value) for value in joints):
             raise ValueError("joint target must contain 7 finite values")
-        ret = self._arm.rm_movej_canfd(
+        ret = arm.rm_movej_canfd(
             [math.degrees(value) for value in joints],
             follow,
             0,
@@ -315,9 +315,10 @@ class RealManAdapter:
             self._arm = None
             self._realtime_callback = None
 
-    def _require_arm(self) -> None:
+    def _require_arm(self) -> Any:
         if self._arm is None:
             raise RuntimeError("睿尔曼机械臂尚未连接")
+        return self._arm
 
     def _on_realtime_arm_state(self, data: Any) -> None:
         if not self._accept_realtime_feedback:
@@ -457,11 +458,11 @@ class RealManAdapter:
             self._try_call("rm_set_joint_max_acc", joint_index, self._joint_max_acc)
 
     def move_to_initial_pose(self) -> None:
-        self._require_arm()
+        arm = self._require_arm()
         if self._initial_joint_pose is None:
             raise RuntimeError("启用初始位姿移动时必须配置 initial_joint_pose")
 
-        ret = self._arm.rm_movej(
+        ret = arm.rm_movej(
             self._initial_joint_pose,
             self._init_move_speed,
             0,