更新README.md

AGENTS.md

@@ -288,7 +288,7 @@ test: 添加 xxx 测试
 
 ## 项目专属规则
 
-* 本项目面向 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/`；测试和运行命令也应在工作空间根目录执行。
+* 本项目面向 Ubuntu 22.04 和 ROS2 Humble；构建、测试和运行命令应在工作空间根目录 `/home/robot/WS_xr` 执行，并先 `source /opt/ros/humble/setup.bash`。
 * 工作空间包含 `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,97 +1,44 @@
-# XR-RM75 双臂遥操作工作空间
+# XR-RM75 双臂遥操作
 
-本仓库是面向 **Ubuntu 22.04 + ROS2 Humble + PICO 4 Ultra + 睿尔曼 RM75** 的阶段一 XR 双臂遥操作项目。当前目标是先跑通一条低速、安全、可调试的闭环：
+基于 **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 的完整时间同步和状态回传。
+
+## 系统架构
 
 ```text
-PICO/XR 双手柄 UDP JSON
+PICO / XRoboToolkit
+  -> 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
-  -> 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 调试话题
+  -> 相对 TCP 目标 + Placo QP
+  -> Mock 或 RM75 rm_movej_canfd
+  -> joint_states / 调试话题
+       -> 可选 xr_rm_mujoco/dual_arm_simulator
 ```
 
-当前控制方式是“手柄相对位姿 + 单步 QP”遥操作：按住 `grip` 时锁定当前手柄位姿和机械臂 TCP 位姿，之后根据手柄相对位移和相对旋转生成目标 TCP。姿态目标使用旋转矩阵和 SO(3) 最短路径完成死区、滤波与限速，不经过 RPY。每个控制周期执行一次 Placo QP，并通过 `rm_movej_canfd` 下发 7 个关节目标。松开 `grip`、UDP 或关节反馈超时、节点退出时会请求机械臂慢停。
+工作空间包含五个 ROS2 包：`xr_rm_input` 负责手柄输入，`xr_rm_interfaces` 定义消息，
+`xr_rm_teleop` 实现遥操作与真机适配，`xr_rm_bringup` 提供启动和配置，
+`xr_rm_mujoco` 负责只读运动学显示。
 
-## 当前范围
+`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
@@ -102,440 +49,125 @@ colcon build --symlink-install
 source install/setup.bash
 ```
 
-真机模式还需要安装睿尔曼 Python API2。若未安装，mock 模式仍可正常使用；真机启动时会提示缺少 `Robotic_Arm` 包。
+遥操作和 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 覆盖这些版本。
 
-遥操作和 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。
+真机模式另需睿尔曼 Python API2；Mock 模式不依赖厂商 SDK。
 
-只读检查 Placo 版本：
+## 快速开始
+
+以下命令均在 `/home/robot/WS_xr` 执行，并先 source ROS2 与 `install/setup.bash`。
+
+### Mock
 
 ```bash
-/home/robot/miniconda3/envs/xr/bin/python -c \
-  "import importlib.metadata; print(importlib.metadata.version('placo'))"
+ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=true
 ```
 
-输出必须为 `0.9.4`。
-
-同时检查 MuJoCo：
+另开终端发送模拟手柄数据：
 
 ```bash
-/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 \
+ros2 run xr_rm_input sample_udp_sender \
+  --hand both --host 127.0.0.1 --port 15000 \
   --pattern axis_sweep --seconds 30
 ```
 
-停止左臂进程后，再分别验证右臂：
+单臂调试时将 `arm` 改为 `left` 或 `right`。推荐先分别完成左右单臂
+Mock，再进入双臂或真机验证。
 
-```bash
-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
-```
-
-`sample_udp_sender` 默认使用 `axis_sweep` 扫轴轨迹，并在终端打印 `XR +X/-X/+Y/-Y/+Z/-Z` 标签。需要检查末端姿态时可增加 `--rotation-pattern rpy_steps --rotation-amplitude-deg 25`。
-
-观察：
-
-```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
-```
-
-第三步：单臂真机。
-
-先只上一个臂，确认网络、方向、急停和限幅：
-
-```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
-```
-
-双臂默认不会自动移动到初始化点；机器人地址、控制参数和初始化移动开关均从
-`dual_arm_rm75.yaml` 读取。
-
-## Launch 入口说明
-
-`arm_debug.launch.py` 是当前唯一的遥操作 launch 主入口，`launcher_ui.py` 中的 Simulation、MuJoCo 和 Real Hardware launch 命令都调用它。
-
-常用参数：
-
-- `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`。
-
-机器人 IP/端口、控制频率、CANFD、限速、工具和初始化位姿等行为参数只由对应 YAML
-配置，launch 不再提供同名覆盖项。
-
-## MuJoCo 双臂仿真
-
-无真机时，由 Mock 关节状态驱动 MuJoCo：
+### MuJoCo
 
 ```bash
 ros2 launch xr_rm_bringup arm_debug.launch.py \
   arm:=both use_mock:=true use_mujoco:=true
 ```
 
-连接真机时，由两台 RM75 的实际关节反馈同步 MuJoCo。下面命令会连接真机，执行前必须完成真机安全检查：
+MuJoCo 只订阅关节状态，不参与控制。真机显示可将 `use_mock` 改为 `false`，
+但该命令会同时连接两台 RM75。
+
+### PICO 输入
+
+只保留一个 UDP 输入源，然后启动 XRoboToolkit bridge：
 
 ```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：
+确认左右 topic 持续接收数据：
 
 ```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 数据已经进入当前遥操作输入层。
+图形启动面板可运行 `python3 src/xr_rm_bringup/tools/launcher_ui.py`，提供
+Simulation、MuJoCo、Real Hardware 和 Diagnostics 模式。
 
-## 真机安全验证
+### 真机
 
-第一次接真机时按这个顺序走：
+确认对应 YAML 中 `move_to_initial_pose_on_connect: false`，再从单臂开始：
 
-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`。
+```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
+```
 
-当前项目没有双臂碰撞检测/避障。双臂首次联调时，请让两个工作区在物理上分开，低速验证，不要让两臂末端互相靠近。
+单臂方向、限位、急停、超时停止和夹爪均验证后，才能启动双臂：
 
-## 后续优化路线
+```bash
+ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false
+```
 
-为了达到“稳定可用的双臂 XR 遥操作/采摘平台”，建议按下面顺序推进：
+按住 `grip` 控制对应机械臂；松开后停止。点击 `trigger` 切换对应夹爪开/关。
+左手 X、右手 A 会请求对应机械臂回到配置的初始位姿；真机使用前必须清空安全区。
 
-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 定位提示，再做单臂辅助，最后做双臂任务分配和任务级状态机。
+## Launch 参数
 
-## 常见问题
+统一入口为 `xr_rm_bringup/launch/arm_debug.launch.py`：
 
-`launcher_ui.py` 提示找不到 `install/setup.bash`：
+| 参数 | 默认值 | 说明 |
+| --- | --- | --- |
+| `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 轮询频率 |
+
+## 配置
+
+| 文件 | 用途 |
+| --- | --- |
+| `dual_arm_rm75.yaml` | 双臂节点、网络、控制与安全参数 |
+| `left_arm_rm75.yaml` | 左臂单独调试 |
+| `right_arm_rm75.yaml` | 右臂单独调试 |
+| `peripherals_rm75.yaml` | 工具坐标、负载和末端执行器 |
+| `dual_arm_mujoco.yaml` | MuJoCo 刷新频率 |
+
+修改某一侧控制参数时，同时检查单臂和双臂 YAML 是否需要同步。必须保留工作空间/
+圆柱限位、线速度与角速度限制、关节速度/加速度限制、指令超时和安全停止逻辑。
+
+`configure_safety_limits` 不得默认关闭；
+`move_to_initial_pose_on_connect` 必须保持默认 `false`。
+
+## 测试
+
+在工作空间根目录执行：
 
 ```bash
-cd /home/robot/WS_xr
 source /opt/ros/humble/setup.bash
 colcon build --symlink-install
-source install/setup.bash
+colcon test --event-handlers console_direct+
+colcon test-result --verbose
 ```
 
-真机模式提示缺少 `Robotic_Arm`：
+涉及遥操作姿态控制时，额外运行：
 
-```text
-未安装睿尔曼 Python API2。请安装厂商 SDK，或用 use_mock:=true 先跑模拟模式。
+```bash
+pytest src/xr_rm_teleop/test/test_orientation_control.py
 ```
 
-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 没有报警或急停。
+真机验证不属于自动测试。默认使用 `use_mock:=true`，未经现场安全确认不要连接或
+移动机械臂。

xr_rm_bringup/test/camera_test_output/.gitignore

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

xr_rm_bringup/test/test_realsense_multi_camera.py

@@ -1,76 +0,0 @@
-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

@@ -1,494 +0,0 @@
-#!/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:
-        arm = self._require_arm()
+        self._require_arm()
         started_at = time.monotonic()
-        result = arm.rm_get_joint_degree()
+        result = self._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:
-        arm = self._require_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 = arm.rm_movej_canfd(
+        ret = self._arm.rm_movej_canfd(
             [math.degrees(value) for value in joints],
             follow,
             0,
@@ -315,10 +315,9 @@ class RealManAdapter:
             self._arm = None
             self._realtime_callback = None
 
-    def _require_arm(self) -> Any:
+    def _require_arm(self) -> None:
         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:
@@ -458,11 +457,11 @@ class RealManAdapter:
             self._try_call("rm_set_joint_max_acc", joint_index, self._joint_max_acc)
 
     def move_to_initial_pose(self) -> None:
-        arm = self._require_arm()
+        self._require_arm()
         if self._initial_joint_pose is None:
             raise RuntimeError("启用初始位姿移动时必须配置 initial_joint_pose")
 
-        ret = arm.rm_movej(
+        ret = self._arm.rm_movej(
             self._initial_joint_pose,
             self._init_move_speed,
             0,