Compare commits
7
Commits
e20c5a983e
...
xr_rm_qp
| Author | SHA1 | Date | |
|---|---|---|---|
 YikaiFu-cart
|
05bed64c46 | ||
|
YikaiFu-cart
|
79e7c12989 | ||
|
YikaiFu-cart
|
235ba61454 | ||
|
YikaiFu-cart
|
676b33bfe4 | ||
|
YikaiFu-cart
|
8329c6a44d | ||
|
YikaiFu-cart
|
94e1bf9467 | ||
|
YikaiFu-cart
|
2d89fe1820 |
@@ -44,3 +44,6 @@ AMENT_IGNORE
|
|||||||
*.vsix
|
*.vsix
|
||||||
|
|
||||||
.codex
|
.codex
|
||||||
|
|
||||||
|
# RealSense camera test snapshots
|
||||||
|
/xr_rm_bringup/test/camera_test_output/
|
||||||
|
|||||||
@@ -1,97 +1,47 @@
|
|||||||
# 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 显示和右臂番茄采摘 ACT 数据采集。
|
||||||
|
|
||||||
|
> [!WARNING]
|
||||||
|
> 真机命令会连接并控制机械臂。首次运行必须从 Mock 和单臂低速验证开始，确保急停
|
||||||
|
> 可用且工作区无人。当前项目没有双臂碰撞检测或避障。
|
||||||
|
|
||||||
|
## 当前能力
|
||||||
|
|
||||||
|
- PICO/XR 双手柄 UDP 输入，相对位姿目标与 Placo QP 七关节控制。
|
||||||
|
- 单臂/双臂 Mock 与真机、夹爪开合，以及只读 MuJoCo 双臂显示。
|
||||||
|
- 工作空间/圆柱限位、速度限制、指令超时和安全慢停。
|
||||||
|
- 三路 RealSense 链路测试与右臂 ACT/ALOHA 风格 HDF5 采集。
|
||||||
|
|
||||||
|
尚未完成：左腕 D405 的 ACT 接入、相机 ROS launch/TF/标定、目标检测、双臂碰撞避障、任务级状态机，以及 PICO 与 ROS 的完整时间同步和状态回传。
|
||||||
|
|
||||||
|
## 系统架构
|
||||||
|
|
||||||
```text
|
```text
|
||||||
PICO/XR 双手柄 UDP JSON
|
PICO / XRoboToolkit
|
||||||
|
-> UDP JSON
|
||||||
-> xr_rm_input/udp_controller_receiver
|
-> 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
|
-> xr_rm_teleop/single_arm_velocity_teleop
|
||||||
-> Placo QP 单步逆解
|
-> 相对 TCP 目标 + Placo QP
|
||||||
-> 左右 RM75 七关节角透传控制
|
-> Mock 或 RM75 rm_movej_canfd
|
||||||
-> /xr_rm/<arm_name>/joint_states
|
-> joint_states / 调试话题
|
||||||
-> 可选 xr_rm_mujoco/dual_arm_simulator
|
├── xr_rm_mujoco/dual_arm_simulator
|
||||||
-> /xr_rm/<arm_name>/current_pose、raw_target_pose、target_pose、cmd_vel、target_clamped 调试话题
|
└── ActControlSample + D455/D405
|
||||||
|
-> act_episode_recorder
|
||||||
|
-> episode_<编号>.hdf5
|
||||||
```
|
```
|
||||||
|
|
||||||
当前控制方式是“手柄相对位姿 + 单步 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` 实现控制与 ACT 录制，`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
|
```bash
|
||||||
cd /home/robot/WS_xr
|
cd /home/robot/WS_xr
|
||||||
@@ -102,440 +52,177 @@ colcon build --symlink-install
|
|||||||
source install/setup.bash
|
source install/setup.bash
|
||||||
```
|
```
|
||||||
|
|
||||||
真机模式还需要安装睿尔曼 Python API2。若未安装，mock 模式仍可正常使用；真机启动时会提示缺少 `Robotic_Arm` 包。
|
遥操作、MuJoCo 和 ACT 节点固定使用 `/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。ACT 采集需要 `h5py` 和 `pyrealsense2`，
|
||||||
|
三相机测试还需要 OpenCV。Mock 模式不依赖厂商 SDK。
|
||||||
|
|
||||||
只读检查 Placo 版本：
|
## 快速开始
|
||||||
|
|
||||||
|
以下命令均在 `/home/robot/WS_xr` 执行，并先 source ROS2 与 `install/setup.bash`。
|
||||||
|
|
||||||
|
### Mock
|
||||||
|
|
||||||
```bash
|
```bash
|
||||||
/home/robot/miniconda3/envs/xr/bin/python -c \
|
ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=true
|
||||||
"import importlib.metadata; print(importlib.metadata.version('placo'))"
|
|
||||||
```
|
```
|
||||||
|
|
||||||
输出必须为 `0.9.4`。
|
另开终端发送模拟手柄数据：
|
||||||
|
|
||||||
同时检查 MuJoCo：
|
|
||||||
|
|
||||||
```bash
|
```bash
|
||||||
/home/robot/miniconda3/envs/xr/bin/python -c \
|
ros2 run xr_rm_input sample_udp_sender \
|
||||||
"import mujoco; print(mujoco.__version__)"
|
--hand both --host 127.0.0.1 --port 15000 \
|
||||||
```
|
|
||||||
|
|
||||||
当前验证版本为 `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
|
--pattern axis_sweep --seconds 30
|
||||||
```
|
```
|
||||||
|
|
||||||
停止左臂进程后，再分别验证右臂：
|
单臂调试时将 `arm` 改为 `left` 或 `right`。推荐先分别完成左右单臂
|
||||||
|
Mock，再进入双臂或真机验证。
|
||||||
|
|
||||||
```bash
|
### MuJoCo
|
||||||
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：
|
|
||||||
|
|
||||||
```bash
|
```bash
|
||||||
ros2 launch xr_rm_bringup arm_debug.launch.py \
|
ros2 launch xr_rm_bringup arm_debug.launch.py \
|
||||||
arm:=both use_mock:=true use_mujoco:=true
|
arm:=both use_mock:=true use_mujoco:=true
|
||||||
```
|
```
|
||||||
|
|
||||||
连接真机时，由两台 RM75 的实际关节反馈同步 MuJoCo。下面命令会连接真机，执行前必须完成真机安全检查：
|
MuJoCo 只订阅关节状态，不参与控制。真机显示可将 `use_mock` 改为 `false`，
|
||||||
|
但该命令会同时连接两台 RM75。
|
||||||
|
|
||||||
|
### PICO 输入
|
||||||
|
|
||||||
|
只保留一个 UDP 输入源，然后启动 XRoboToolkit bridge：
|
||||||
|
|
||||||
```bash
|
```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 \
|
ros2 run xr_rm_input xrobotoolkit_to_udp_bridge \
|
||||||
--host 127.0.0.1 --port 15000 --hz 90
|
--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。
|
确认左右 topic 持续接收数据：
|
||||||
|
|
||||||
验证手柄数据是否进入 ROS：
|
|
||||||
|
|
||||||
```bash
|
```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/left_controller
|
||||||
ros2 topic hz /xr/right_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. 确认急停、网络、机械臂工作区和人员位置。
|
```bash
|
||||||
2. `launcher_ui.py` 中先 `Ping Left RM75` 或 `Ping Right RM75`。
|
ros2 launch xr_rm_bringup arm_debug.launch.py arm:=left use_mock:=false
|
||||||
3. 确认对应 YAML 中 `move_to_initial_pose_on_connect: false` 后单臂启动。
|
ros2 launch xr_rm_bringup arm_debug.launch.py arm:=right use_mock:=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:=both use_mock:=false
|
||||||
|
```
|
||||||
|
|
||||||
为了达到“稳定可用的双臂 XR 遥操作/采摘平台”，建议按下面顺序推进：
|
按住 `grip` 控制对应机械臂；松开后停止。点击 `trigger` 切换对应夹爪开/关。
|
||||||
|
|
||||||
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`。
|
## RealSense 与 ACT 采集
|
||||||
2. 提升真机安全性：增加启动前安全检查、软件急停 topic、UI Stop 状态提示、双臂中间区域互斥边界和速度/加速度限幅。
|
|
||||||
3. 细化末端执行器：增加夹爪状态反馈、力控比例、安全上限和现场可视化提示。
|
|
||||||
4. 接入视觉和数据记录：加入 D405/D435 相机 launch、TF、内外参和 rosbag2 实验记录。
|
|
||||||
5. 从遥操作走向半自动：先做目标检测和 3D 定位提示，再做单臂辅助，最后做双臂任务分配和任务级状态机。
|
|
||||||
|
|
||||||
## 常见问题
|
### 三相机测试
|
||||||
|
|
||||||
`launcher_ui.py` 提示找不到 `install/setup.bash`：
|
连接两台 D405 和一台 D455 后执行：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
/home/robot/miniconda3/envs/xr/bin/python \
|
||||||
|
src/xr_rm_bringup/tools/realsense_multi_camera_test.py
|
||||||
|
```
|
||||||
|
|
||||||
|
默认将序列号 `260322272273` 识别为左腕 D405，另一台 D405 为右腕，D455 为
|
||||||
|
全局相机。按 `S` 保存三路快照，按 `Q` 或 `Esc` 退出并打印链路汇总。
|
||||||
|
快照目录为 `src/xr_rm_bringup/test/camera_test_output/`。
|
||||||
|
|
||||||
|
### ACT episode
|
||||||
|
|
||||||
|
ACT 目前仅支持右臂真机。以下命令会连接并控制右侧 RM75：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
ros2 launch xr_rm_bringup arm_debug.launch.py \
|
||||||
|
arm:=right use_mock:=false record_act:=true
|
||||||
|
```
|
||||||
|
|
||||||
|
`record_act:=true` 启动后默认显示全局 D455 和右腕 D405 双路画面，并显示实时
|
||||||
|
FPS、真实丢帧率、帧龄、双相机时间差、录制状态、episode 编号和样本数。按
|
||||||
|
`Q`、`Esc` 或关闭窗口只会停止预览，ACT 相机采集和录制继续运行；没有桌面环境
|
||||||
|
或 OpenCV 显示失败时也不会影响录制。
|
||||||
|
|
||||||
|
相机采集线程观察到的真实掉帧仍会拒绝 episode。独立 `30 Hz` 控制和相机时钟
|
||||||
|
造成的 ACT 样本重复/跨帧只写入 HDF5 质量指标，不再误报为相机丢包。
|
||||||
|
|
||||||
|
默认配置位于 `xr_rm_bringup/config/act_tomato_pick.yaml`：D455 序列号
|
||||||
|
`234222303366`，右腕 D405 序列号 `412622272532`，90 Hz 控制数据下采样为
|
||||||
|
30 Hz。输出位于 `/home/robot/ACT_Data/tomato_pick/`，状态发布到
|
||||||
|
`/act/recording_status`。
|
||||||
|
|
||||||
|
录制流程：
|
||||||
|
|
||||||
|
1. 打开夹爪并松开右手 `grip`。
|
||||||
|
2. 点击右手 B 完成预检并进入 `ARMED`。
|
||||||
|
3. 按住右手 `grip` 开始遥操作和录制。
|
||||||
|
4. 松开 `grip`，点击右手 B 保存。
|
||||||
|
|
||||||
|
`ARMED` 或录制期间长按左手 Y 一秒可丢弃当前 episode。录制期间点击右手 A
|
||||||
|
会触发初始化位姿并使当前数据无效。
|
||||||
|
|
||||||
|
预检和保存会检查控制连续性、反馈有效性、夹爪状态、磁盘空间、相机帧率/掉帧、
|
||||||
|
帧龄和双相机时间差。不合格或中断的数据保存在 `rejected/`。
|
||||||
|
|
||||||
|
## Launch 参数
|
||||||
|
|
||||||
|
统一入口为 `xr_rm_bringup/launch/arm_debug.launch.py`：
|
||||||
|
|
||||||
|
| 参数 | 默认值 | 说明 |
|
||||||
|
| --- | --- | --- |
|
||||||
|
| `arm` | `right` | `left`、`right` 或 `both` |
|
||||||
|
| `use_mock` | `true` | `false` 会连接真机 |
|
||||||
|
| `use_mujoco` | `false` | 仅支持 `arm:=both` |
|
||||||
|
| `record_act` | `false` | 仅支持 `arm:=right use_mock:=false` |
|
||||||
|
| `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 刷新频率 |
|
||||||
|
| `act_tomato_pick.yaml` | ACT 相机、存储与质量阈值 |
|
||||||
|
|
||||||
|
修改某一侧控制参数时，同时检查单臂和双臂 YAML 是否需要同步。必须保留工作空间/
|
||||||
|
圆柱限位、线速度与角速度限制、关节速度/加速度限制、指令超时和安全停止逻辑。
|
||||||
|
|
||||||
|
`configure_safety_limits` 不得默认关闭；
|
||||||
|
`move_to_initial_pose_on_connect` 必须保持默认 `false`。
|
||||||
|
|
||||||
|
## 测试
|
||||||
|
|
||||||
|
在工作空间根目录执行：
|
||||||
|
|
||||||
```bash
|
```bash
|
||||||
cd /home/robot/WS_xr
|
|
||||||
source /opt/ros/humble/setup.bash
|
source /opt/ros/humble/setup.bash
|
||||||
colcon build --symlink-install
|
colcon build --symlink-install
|
||||||
source install/setup.bash
|
colcon test --event-handlers console_direct+
|
||||||
|
colcon test-result --verbose
|
||||||
```
|
```
|
||||||
|
|
||||||
真机模式提示缺少 `Robotic_Arm`：
|
涉及遥操作姿态控制时，额外运行：
|
||||||
|
|
||||||
```text
|
```bash
|
||||||
未安装睿尔曼 Python API2。请安装厂商 SDK，或用 use_mock:=true 先跑模拟模式。
|
pytest src/xr_rm_teleop/test/test_orientation_control.py
|
||||||
```
|
```
|
||||||
|
|
||||||
Controller topic 没有数据：
|
真机验证不属于自动测试。默认使用 `use_mock:=true`，未经现场安全确认不要连接或
|
||||||
|
移动机械臂。
|
||||||
- 确认 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 没有报警或急停。
|
|
||||||
|
|||||||
@@ -0,0 +1,756 @@
|
|||||||
|
# ACT 双相机预览与采样质量判定修正实施计划
|
||||||
|
|
||||||
|
> **供代理执行者使用：** 必须使用 `superpowers:subagent-driven-development`（推荐）或 `superpowers:executing-plans`，逐项执行本计划。所有步骤使用复选框（`- [ ]`）跟踪。
|
||||||
|
|
||||||
|
**目标：** 保持 ACT 当前因果时间对齐，同时把真实相机丢帧与软件采样相位漂移分开判定，并在 ACT 数采启动时默认显示全局 D455 和右腕 D405 实时画面。
|
||||||
|
|
||||||
|
**架构：** 继续由 `act_episode_recorder` 独占两台 RealSense，并按每三个 `90 Hz` 控制周期选择不晚于控制时刻的最新图像。`CameraBuffer` 负责真实采集质量，HDF5 校验只记录 ACT 样本重复/跨帧率；同一节点内新增一个只读 OpenCV 预览线程，复用现有帧缓冲且不进入机器人控制链路。
|
||||||
|
|
||||||
|
**技术栈：** Ubuntu 22.04、ROS2 Humble、Python 3、rclpy、NumPy、h5py、pyrealsense2、OpenCV、pytest、HDF5。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 文件结构
|
||||||
|
|
||||||
|
本次不新建 ROS 包或运行进程，文件职责保持如下：
|
||||||
|
|
||||||
|
- 修改 `xr_rm_teleop/xr_rm_teleop/act_episode_recorder.py`：真实采集统计、ACT 采样相位指标、质量判定、双路预览及关闭顺序；
|
||||||
|
- 修改 `xr_rm_teleop/test/test_act_episode_recorder.py`：指标口径、误拒绝复现、帧号回退和预览隔离测试；
|
||||||
|
- 修改 `README.md`：说明 ACT 数采默认预览、显示内容和关闭行为。
|
||||||
|
|
||||||
|
不修改 `arm_debug.launch.py`、`launcher_ui.py`、ROS 消息、机器人控制节点和相机 YAML。工作区已有的 `xr_rm_teleop/xr_rm_teleop/fun_peripheral.py` 未提交改动属于用户，不加入本任务提交。
|
||||||
|
|
||||||
|
所有构建和测试命令从工作空间根目录执行：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
cd /home/robot/WS_xr
|
||||||
|
source /opt/ros/humble/setup.bash
|
||||||
|
```
|
||||||
|
|
||||||
|
自动化测试不得启动真机 launch、移动机械臂或操作夹爪。
|
||||||
|
|
||||||
|
### 任务 1：区分真实丢帧与 ACT 采样相位漂移
|
||||||
|
|
||||||
|
**文件：**
|
||||||
|
|
||||||
|
- 修改：`xr_rm_teleop/xr_rm_teleop/act_episode_recorder.py:408-579`
|
||||||
|
- 修改：`xr_rm_teleop/xr_rm_teleop/act_episode_recorder.py:595-657`
|
||||||
|
- 修改：`xr_rm_teleop/xr_rm_teleop/act_episode_recorder.py:1472-1511`
|
||||||
|
- 测试：`xr_rm_teleop/test/test_act_episode_recorder.py:273-296`
|
||||||
|
- 测试：`xr_rm_teleop/test/test_act_episode_recorder.py:491-607`
|
||||||
|
|
||||||
|
- [ ] **步骤 1：写异步采样不拒绝和指标测试**
|
||||||
|
|
||||||
|
在测试导入中加入 `sample_frame_metrics`，并增加：
|
||||||
|
|
||||||
|
```python
|
||||||
|
def test_sample_frame_metrics_separate_repeats_skips_and_regressions():
|
||||||
|
repeat, skip, regression = sample_frame_metrics(
|
||||||
|
np.asarray((986, 986, 988), dtype=np.uint64)
|
||||||
|
)
|
||||||
|
|
||||||
|
assert repeat == pytest.approx(0.5)
|
||||||
|
assert skip == pytest.approx(0.5)
|
||||||
|
assert regression == 0
|
||||||
|
|
||||||
|
|
||||||
|
@requires_h5py
|
||||||
|
def test_validate_episode_accepts_async_camera_phase_drift(tmp_path):
|
||||||
|
path = _valid_episode(tmp_path)
|
||||||
|
with h5py.File(path, "r+") as root:
|
||||||
|
root["debug/cameras/cam_high_frame_number"][:] = (986, 986, 988)
|
||||||
|
|
||||||
|
report = validate_episode(path, _quality_limits())
|
||||||
|
|
||||||
|
assert report.accepted
|
||||||
|
assert report.metrics["cam_high_sample_repeat_ratio"] == pytest.approx(0.5)
|
||||||
|
assert report.metrics["cam_high_sample_skip_ratio"] == pytest.approx(0.5)
|
||||||
|
```
|
||||||
|
|
||||||
|
在 `_valid_episode()` 写入新增的真实采集属性：
|
||||||
|
|
||||||
|
```python
|
||||||
|
root.attrs["camera_high_frame_number_regression_count"] = 0
|
||||||
|
root.attrs["camera_right_wrist_frame_number_regression_count"] = 0
|
||||||
|
```
|
||||||
|
|
||||||
|
- [ ] **步骤 2：写真实帧号回退测试**
|
||||||
|
|
||||||
|
扩展现有 `CameraBuffer` 测试：
|
||||||
|
|
||||||
|
```python
|
||||||
|
def test_camera_buffer_counts_frame_number_regressions():
|
||||||
|
buffer = CameraBuffer(maxlen=4)
|
||||||
|
for frame_number in (100, 101, 1, 2):
|
||||||
|
buffer.push(
|
||||||
|
CameraFrame(
|
||||||
|
_image(frame_number),
|
||||||
|
frame_number,
|
||||||
|
float(frame_number),
|
||||||
|
time.monotonic_ns(),
|
||||||
|
)
|
||||||
|
)
|
||||||
|
|
||||||
|
stats = buffer.stats()
|
||||||
|
|
||||||
|
assert stats.frame_number_regression_count == 1
|
||||||
|
assert stats.dropped_frames == 0
|
||||||
|
```
|
||||||
|
|
||||||
|
测试文件顶部增加标准库导入：
|
||||||
|
|
||||||
|
```python
|
||||||
|
import time
|
||||||
|
```
|
||||||
|
|
||||||
|
并在稳定拒绝原因参数中增加真实采集帧号回退：
|
||||||
|
|
||||||
|
```python
|
||||||
|
elif mutation == "camera_frame_regression":
|
||||||
|
root.attrs["camera_high_frame_number_regression_count"] = 1
|
||||||
|
```
|
||||||
|
|
||||||
|
对应期望：
|
||||||
|
|
||||||
|
```python
|
||||||
|
("camera_frame_regression", "camera_frame_number_regression"),
|
||||||
|
```
|
||||||
|
|
||||||
|
- [ ] **步骤 3：运行新增测试并确认失败**
|
||||||
|
|
||||||
|
运行：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
cd /home/robot/WS_xr
|
||||||
|
source /opt/ros/humble/setup.bash
|
||||||
|
pytest src/xr_rm_teleop/test/test_act_episode_recorder.py \
|
||||||
|
-k "sample_frame_metrics or async_camera_phase_drift or frame_number_regressions or camera_frame_regression" -v
|
||||||
|
```
|
||||||
|
|
||||||
|
预期：测试因 `sample_frame_metrics` 尚不存在、`CameraStats` 没有回退计数，或旧校验仍以 `camera_sample_drop_ratio` 拒绝而失败。
|
||||||
|
|
||||||
|
- [ ] **步骤 4：实现最小采样相位指标**
|
||||||
|
|
||||||
|
在质量校验辅助函数附近增加纯函数：
|
||||||
|
|
||||||
|
```python
|
||||||
|
def sample_frame_metrics(
|
||||||
|
frame_numbers: np.ndarray,
|
||||||
|
) -> tuple[float, float, int]:
|
||||||
|
diffs = np.diff(np.asarray(frame_numbers, dtype=np.int64))
|
||||||
|
denominator = max(1, len(diffs))
|
||||||
|
return (
|
||||||
|
float(np.count_nonzero(diffs == 0) / denominator),
|
||||||
|
float(np.count_nonzero(diffs > 1) / denominator),
|
||||||
|
int(np.count_nonzero(diffs < 0)),
|
||||||
|
)
|
||||||
|
```
|
||||||
|
|
||||||
|
把 `validate_episode()` 中原有“所有 `diff != 1` 都拒绝”的循环替换为：
|
||||||
|
|
||||||
|
```python
|
||||||
|
for camera in ("cam_high", "cam_wrist"):
|
||||||
|
frame_numbers = datasets[
|
||||||
|
f"debug/cameras/{camera}_frame_number"
|
||||||
|
][:]
|
||||||
|
repeat_ratio, skip_ratio, regression_count = sample_frame_metrics(
|
||||||
|
frame_numbers
|
||||||
|
)
|
||||||
|
metrics[f"{camera}_sample_repeat_ratio"] = repeat_ratio
|
||||||
|
metrics[f"{camera}_sample_skip_ratio"] = skip_ratio
|
||||||
|
if regression_count:
|
||||||
|
return _quality_failure("camera_frame_number_regression", metrics)
|
||||||
|
```
|
||||||
|
|
||||||
|
这样 `986 → 986 → 988` 只产生统计，不再触发 `camera_sample_drop_ratio`。
|
||||||
|
|
||||||
|
- [ ] **步骤 5：在采集层统计帧号回退**
|
||||||
|
|
||||||
|
扩展 `CameraStats`：
|
||||||
|
|
||||||
|
```python
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class CameraStats:
|
||||||
|
frame_count: int
|
||||||
|
dropped_frames: int
|
||||||
|
frame_number_regression_count: int
|
||||||
|
first_host_monotonic_ns: int | None
|
||||||
|
last_host_monotonic_ns: int | None
|
||||||
|
```
|
||||||
|
|
||||||
|
在 `CameraBuffer.__init__()` 增加：
|
||||||
|
|
||||||
|
```python
|
||||||
|
self._frame_number_regression_count = 0
|
||||||
|
```
|
||||||
|
|
||||||
|
在 `CameraBuffer.push()` 更新帧号前使用互斥分支：
|
||||||
|
|
||||||
|
```python
|
||||||
|
if self._last_frame_number is not None:
|
||||||
|
if frame.frame_number <= self._last_frame_number:
|
||||||
|
self._frame_number_regression_count += 1
|
||||||
|
elif frame.frame_number > self._last_frame_number + 1:
|
||||||
|
self._dropped_frames += (
|
||||||
|
frame.frame_number - self._last_frame_number - 1
|
||||||
|
)
|
||||||
|
```
|
||||||
|
|
||||||
|
在 `stats()` 返回：
|
||||||
|
|
||||||
|
```python
|
||||||
|
frame_number_regression_count=self._frame_number_regression_count,
|
||||||
|
```
|
||||||
|
|
||||||
|
- [ ] **步骤 6：把真实采集回退纳入 episode 属性和拒绝条件**
|
||||||
|
|
||||||
|
把 `_interval_camera_metrics()` 的返回值扩展为 FPS、真实丢帧率和本 episode 新增的回退数：
|
||||||
|
|
||||||
|
```python
|
||||||
|
@staticmethod
|
||||||
|
def _interval_camera_metrics(
|
||||||
|
baseline: CameraStats,
|
||||||
|
current: CameraStats,
|
||||||
|
) -> tuple[float, float, int]:
|
||||||
|
frames = max(0, current.frame_count - baseline.frame_count)
|
||||||
|
dropped = max(0, current.dropped_frames - baseline.dropped_frames)
|
||||||
|
regressions = max(
|
||||||
|
0,
|
||||||
|
current.frame_number_regression_count
|
||||||
|
- baseline.frame_number_regression_count,
|
||||||
|
)
|
||||||
|
if (
|
||||||
|
baseline.last_host_monotonic_ns is None
|
||||||
|
or current.last_host_monotonic_ns is None
|
||||||
|
):
|
||||||
|
return 0.0, 1.0, regressions
|
||||||
|
elapsed_ns = (
|
||||||
|
current.last_host_monotonic_ns
|
||||||
|
- baseline.last_host_monotonic_ns
|
||||||
|
)
|
||||||
|
fps = frames * 1e9 / elapsed_ns if elapsed_ns > 0 else 0.0
|
||||||
|
expected = frames + dropped
|
||||||
|
drop_ratio = dropped / expected if expected else 1.0
|
||||||
|
return fps, drop_ratio, regressions
|
||||||
|
```
|
||||||
|
|
||||||
|
`_write_camera_metrics()` 写入：
|
||||||
|
|
||||||
|
```python
|
||||||
|
"camera_high_frame_number_regression_count": high[2],
|
||||||
|
"camera_right_wrist_frame_number_regression_count": wrist[2],
|
||||||
|
```
|
||||||
|
|
||||||
|
`validate_episode()` 在读取 FPS 和真实丢帧率后要求两个回退属性存在且为零：
|
||||||
|
|
||||||
|
```python
|
||||||
|
for name in (
|
||||||
|
"camera_high_frame_number_regression_count",
|
||||||
|
"camera_right_wrist_frame_number_regression_count",
|
||||||
|
):
|
||||||
|
if name not in root.attrs:
|
||||||
|
return _quality_failure("camera_stats_missing", metrics)
|
||||||
|
value = int(root.attrs[name])
|
||||||
|
metrics[name] = value
|
||||||
|
if value:
|
||||||
|
return _quality_failure("camera_frame_number_regression", metrics)
|
||||||
|
```
|
||||||
|
|
||||||
|
- [ ] **步骤 7：确保采样指标写入保存和拒绝文件**
|
||||||
|
|
||||||
|
增加读取 HDF5 根节点的辅助函数：
|
||||||
|
|
||||||
|
```python
|
||||||
|
def episode_sample_frame_metrics(root: Any) -> dict[str, int | float]:
|
||||||
|
metrics: dict[str, int | float] = {}
|
||||||
|
for camera in ("cam_high", "cam_wrist"):
|
||||||
|
repeat, skip, regression = sample_frame_metrics(
|
||||||
|
root[f"debug/cameras/{camera}_frame_number"][:]
|
||||||
|
)
|
||||||
|
metrics[f"{camera}_sample_repeat_ratio"] = repeat
|
||||||
|
metrics[f"{camera}_sample_skip_ratio"] = skip
|
||||||
|
metrics[f"{camera}_sample_frame_number_regression_count"] = regression
|
||||||
|
return metrics
|
||||||
|
```
|
||||||
|
|
||||||
|
`validate_episode()` 复用该函数更新 `report.metrics`。在 `_reject_closed_partial()` 已打开 HDF5 后也执行：
|
||||||
|
|
||||||
|
```python
|
||||||
|
for name, value in episode_sample_frame_metrics(root).items():
|
||||||
|
root.attrs[name] = value
|
||||||
|
```
|
||||||
|
|
||||||
|
保存路径继续由 `_complete_save()` 把 `report.metrics` 写入属性。保存日志追加紧凑摘要：
|
||||||
|
|
||||||
|
```python
|
||||||
|
self.get_logger().info(
|
||||||
|
"ACT相机采样相位："
|
||||||
|
f"high重复={report.metrics['cam_high_sample_repeat_ratio']:.2%}, "
|
||||||
|
f"high跨帧={report.metrics['cam_high_sample_skip_ratio']:.2%}, "
|
||||||
|
f"wrist重复={report.metrics['cam_wrist_sample_repeat_ratio']:.2%}, "
|
||||||
|
f"wrist跨帧={report.metrics['cam_wrist_sample_skip_ratio']:.2%}"
|
||||||
|
)
|
||||||
|
```
|
||||||
|
|
||||||
|
- [ ] **步骤 8：运行相关测试并确认通过**
|
||||||
|
|
||||||
|
运行：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
cd /home/robot/WS_xr
|
||||||
|
source /opt/ros/humble/setup.bash
|
||||||
|
pytest src/xr_rm_teleop/test/test_act_episode_recorder.py \
|
||||||
|
-k "camera or validate_episode" -v
|
||||||
|
```
|
||||||
|
|
||||||
|
预期：所有选中测试通过；真实丢帧率仍使用 `camera_drop_ratio` 拒绝，异步重复/跨帧不拒绝。
|
||||||
|
|
||||||
|
- [ ] **步骤 9：提交任务 1**
|
||||||
|
|
||||||
|
```bash
|
||||||
|
git add src/xr_rm_teleop/xr_rm_teleop/act_episode_recorder.py \
|
||||||
|
src/xr_rm_teleop/test/test_act_episode_recorder.py
|
||||||
|
git commit -m "fix: 修正ACT相机采样质量判定"
|
||||||
|
```
|
||||||
|
|
||||||
|
### 任务 2：在录制器中增加非阻塞双路预览
|
||||||
|
|
||||||
|
**文件：**
|
||||||
|
|
||||||
|
- 修改：`xr_rm_teleop/xr_rm_teleop/act_episode_recorder.py:595-657`
|
||||||
|
- 修改：`xr_rm_teleop/xr_rm_teleop/act_episode_recorder.py:1006-1152`
|
||||||
|
- 修改：`xr_rm_teleop/xr_rm_teleop/act_episode_recorder.py:1668-1671`
|
||||||
|
- 测试：`xr_rm_teleop/test/test_act_episode_recorder.py`
|
||||||
|
|
||||||
|
- [ ] **步骤 1：写两秒滚动 FPS 测试**
|
||||||
|
|
||||||
|
扩展 `CameraBuffer` 测试，使用明确的单调时间：
|
||||||
|
|
||||||
|
```python
|
||||||
|
def test_camera_buffer_reports_two_second_rolling_fps():
|
||||||
|
buffer = CameraBuffer(maxlen=4)
|
||||||
|
start_ns = 10_000_000_000
|
||||||
|
for index in range(61):
|
||||||
|
buffer.push(
|
||||||
|
CameraFrame(
|
||||||
|
_image(index),
|
||||||
|
index,
|
||||||
|
float(index),
|
||||||
|
start_ns + index * 33_333_333,
|
||||||
|
)
|
||||||
|
)
|
||||||
|
|
||||||
|
assert buffer.stats().rolling_fps == pytest.approx(30.0, rel=0.02)
|
||||||
|
```
|
||||||
|
|
||||||
|
- [ ] **步骤 2：写预览显示异常隔离测试**
|
||||||
|
|
||||||
|
构造不经过 ROS 初始化的录制器和抛错的 `cv2` 替身：
|
||||||
|
|
||||||
|
```python
|
||||||
|
def test_preview_failure_does_not_change_recording_state():
|
||||||
|
recorder = object.__new__(ActEpisodeRecorder)
|
||||||
|
recorder._session = _recording_session()
|
||||||
|
recorder._preview_stop = threading.Event()
|
||||||
|
recorder._preview_thread = None
|
||||||
|
recorder.get_logger = lambda: _Logger()
|
||||||
|
|
||||||
|
class FailingCv2:
|
||||||
|
WINDOW_NORMAL = 0
|
||||||
|
|
||||||
|
@staticmethod
|
||||||
|
def namedWindow(*_args):
|
||||||
|
raise RuntimeError("no display")
|
||||||
|
|
||||||
|
recorder._preview_loop(FailingCv2())
|
||||||
|
|
||||||
|
assert recorder.state is RecordingState.RECORDING
|
||||||
|
```
|
||||||
|
|
||||||
|
`_Logger` 增加 `warns` 收集和 `warn()`：
|
||||||
|
|
||||||
|
```python
|
||||||
|
self.warns = []
|
||||||
|
|
||||||
|
def warn(self, message):
|
||||||
|
self.warns.append(message)
|
||||||
|
```
|
||||||
|
|
||||||
|
- [ ] **步骤 3：写预览关闭顺序测试**
|
||||||
|
|
||||||
|
验证关闭节点时先停预览，再停相机：
|
||||||
|
|
||||||
|
```python
|
||||||
|
def test_close_stops_preview_before_cameras_and_releases_lock():
|
||||||
|
events = []
|
||||||
|
recorder = object.__new__(ActEpisodeRecorder)
|
||||||
|
recorder._stop_preview = lambda: events.append("preview")
|
||||||
|
recorder._high_camera = SimpleNamespace(
|
||||||
|
stop=lambda: events.append("high")
|
||||||
|
)
|
||||||
|
recorder._wrist_camera = SimpleNamespace(
|
||||||
|
stop=lambda: events.append("wrist")
|
||||||
|
)
|
||||||
|
recorder._directory_lock = SimpleNamespace(
|
||||||
|
release=lambda: events.append("lock")
|
||||||
|
)
|
||||||
|
|
||||||
|
recorder.close()
|
||||||
|
|
||||||
|
assert events == ["preview", "high", "wrist", "lock"]
|
||||||
|
```
|
||||||
|
|
||||||
|
- [ ] **步骤 4：运行新增预览测试并确认失败**
|
||||||
|
|
||||||
|
运行：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
cd /home/robot/WS_xr
|
||||||
|
source /opt/ros/humble/setup.bash
|
||||||
|
pytest src/xr_rm_teleop/test/test_act_episode_recorder.py \
|
||||||
|
-k "rolling_fps or preview or close_stops_preview" -v
|
||||||
|
```
|
||||||
|
|
||||||
|
预期：测试因 `rolling_fps`、`_preview_loop()` 和 `_stop_preview()` 尚不存在而失败。
|
||||||
|
|
||||||
|
- [ ] **步骤 5：实现两秒滚动 FPS**
|
||||||
|
|
||||||
|
在 `CameraBuffer.__init__()` 增加：
|
||||||
|
|
||||||
|
```python
|
||||||
|
self._recent_host_monotonic_ns: deque[int] = deque()
|
||||||
|
```
|
||||||
|
|
||||||
|
每次 `push()` 时裁剪两秒窗口：
|
||||||
|
|
||||||
|
```python
|
||||||
|
self._recent_host_monotonic_ns.append(frame.host_monotonic_ns)
|
||||||
|
cutoff_ns = frame.host_monotonic_ns - 2_000_000_000
|
||||||
|
while (
|
||||||
|
self._recent_host_monotonic_ns
|
||||||
|
and self._recent_host_monotonic_ns[0] < cutoff_ns
|
||||||
|
):
|
||||||
|
self._recent_host_monotonic_ns.popleft()
|
||||||
|
```
|
||||||
|
|
||||||
|
`CameraStats` 增加字段和属性：
|
||||||
|
|
||||||
|
```python
|
||||||
|
recent_host_monotonic_ns: tuple[int, ...]
|
||||||
|
|
||||||
|
@property
|
||||||
|
def rolling_fps(self) -> float:
|
||||||
|
if len(self.recent_host_monotonic_ns) < 2:
|
||||||
|
return 0.0
|
||||||
|
elapsed_ns = (
|
||||||
|
self.recent_host_monotonic_ns[-1]
|
||||||
|
- self.recent_host_monotonic_ns[0]
|
||||||
|
)
|
||||||
|
return (
|
||||||
|
(len(self.recent_host_monotonic_ns) - 1) * 1e9 / elapsed_ns
|
||||||
|
if elapsed_ns > 0
|
||||||
|
else 0.0
|
||||||
|
)
|
||||||
|
```
|
||||||
|
|
||||||
|
`stats()` 使用：
|
||||||
|
|
||||||
|
```python
|
||||||
|
recent_host_monotonic_ns=tuple(self._recent_host_monotonic_ns),
|
||||||
|
```
|
||||||
|
|
||||||
|
- [ ] **步骤 6：实现最小预览渲染方法**
|
||||||
|
|
||||||
|
在 `ActEpisodeRecorder` 增加固定窗口名：
|
||||||
|
|
||||||
|
```python
|
||||||
|
PREVIEW_WINDOW = "ACT - D455 Global / D405 Right Wrist"
|
||||||
|
```
|
||||||
|
|
||||||
|
增加生成单个画面块的方法。相机帧是 RGB，因此显示前转换成 BGR：
|
||||||
|
|
||||||
|
```python
|
||||||
|
@staticmethod
|
||||||
|
def _preview_tile(role: str, frame, stats, cv2):
|
||||||
|
if frame is None:
|
||||||
|
tile = np.zeros((480, 640, 3), dtype=np.uint8)
|
||||||
|
frame_number = "-"
|
||||||
|
else:
|
||||||
|
tile = cv2.cvtColor(frame.image, cv2.COLOR_RGB2BGR)
|
||||||
|
frame_number = str(frame.frame_number)
|
||||||
|
tile = tile.copy()
|
||||||
|
cv2.rectangle(tile, (0, 0), (640, 74), (0, 0, 0), -1)
|
||||||
|
lines = (
|
||||||
|
f"{role} FPS {stats.rolling_fps:.1f} Frame {frame_number}",
|
||||||
|
f"Received {stats.frame_count} Dropped "
|
||||||
|
f"{stats.dropped_frames} ({stats.drop_ratio:.2%})",
|
||||||
|
)
|
||||||
|
for index, line in enumerate(lines):
|
||||||
|
cv2.putText(
|
||||||
|
tile,
|
||||||
|
line,
|
||||||
|
(10, 28 + index * 30),
|
||||||
|
cv2.FONT_HERSHEY_SIMPLEX,
|
||||||
|
0.65,
|
||||||
|
(255, 255, 255),
|
||||||
|
1,
|
||||||
|
cv2.LINE_AA,
|
||||||
|
)
|
||||||
|
return tile
|
||||||
|
```
|
||||||
|
|
||||||
|
增加 `_compose_preview()`：读取两个缓冲最新帧，水平拼接，并在底部显示状态：
|
||||||
|
|
||||||
|
```python
|
||||||
|
def _compose_preview(self, cv2):
|
||||||
|
high_frames = self._high_camera.buffer.snapshot()
|
||||||
|
wrist_frames = self._wrist_camera.buffer.snapshot()
|
||||||
|
high = high_frames[-1] if high_frames else None
|
||||||
|
wrist = wrist_frames[-1] if wrist_frames else None
|
||||||
|
high_stats = self._high_camera.buffer.stats()
|
||||||
|
wrist_stats = self._wrist_camera.buffer.stats()
|
||||||
|
image = np.hstack(
|
||||||
|
(
|
||||||
|
self._preview_tile("GLOBAL D455", high, high_stats, cv2),
|
||||||
|
self._preview_tile("RIGHT WRIST D405", wrist, wrist_stats, cv2),
|
||||||
|
)
|
||||||
|
)
|
||||||
|
|
||||||
|
now_ns = self._now_ns()
|
||||||
|
high_age = (
|
||||||
|
f"{(now_ns - high.host_monotonic_ns) * 1e-6:.1f} ms"
|
||||||
|
if high is not None
|
||||||
|
else "-"
|
||||||
|
)
|
||||||
|
wrist_age = (
|
||||||
|
f"{(now_ns - wrist.host_monotonic_ns) * 1e-6:.1f} ms"
|
||||||
|
if wrist is not None
|
||||||
|
else "-"
|
||||||
|
)
|
||||||
|
skew = (
|
||||||
|
f"{abs(high.host_monotonic_ns - wrist.host_monotonic_ns) * 1e-6:.1f} ms"
|
||||||
|
if high is not None and wrist is not None
|
||||||
|
else "-"
|
||||||
|
)
|
||||||
|
episode = (
|
||||||
|
f"episode_{self._episode_index}"
|
||||||
|
if self._episode_index is not None
|
||||||
|
else "-"
|
||||||
|
)
|
||||||
|
samples = self._store.count if self._store is not None else 0
|
||||||
|
footer = np.zeros((80, image.shape[1], 3), dtype=np.uint8)
|
||||||
|
lines = (
|
||||||
|
f"Age high={high_age} wrist={wrist_age} Camera skew={skew}",
|
||||||
|
f"ACT {self.state.value} {episode} Samples {samples}",
|
||||||
|
)
|
||||||
|
for index, line in enumerate(lines):
|
||||||
|
cv2.putText(
|
||||||
|
footer,
|
||||||
|
line,
|
||||||
|
(10, 30 + index * 32),
|
||||||
|
cv2.FONT_HERSHEY_SIMPLEX,
|
||||||
|
0.7,
|
||||||
|
(255, 255, 255),
|
||||||
|
1,
|
||||||
|
cv2.LINE_AA,
|
||||||
|
)
|
||||||
|
return np.vstack((image, footer))
|
||||||
|
```
|
||||||
|
|
||||||
|
若尚无帧，对应数值显示 `-`；该方法不修改任何录制器状态。
|
||||||
|
|
||||||
|
- [ ] **步骤 7：实现预览线程生命周期和故障隔离**
|
||||||
|
|
||||||
|
在相机成员创建前初始化：
|
||||||
|
|
||||||
|
```python
|
||||||
|
self._preview_stop = threading.Event()
|
||||||
|
self._preview_thread: threading.Thread | None = None
|
||||||
|
```
|
||||||
|
|
||||||
|
两台相机成功启动后调用 `_start_preview()`：
|
||||||
|
|
||||||
|
```python
|
||||||
|
if self._camera_start_error is None:
|
||||||
|
self._start_preview()
|
||||||
|
```
|
||||||
|
|
||||||
|
实现：
|
||||||
|
|
||||||
|
```python
|
||||||
|
def _start_preview(self) -> None:
|
||||||
|
if not (os.environ.get("DISPLAY") or os.environ.get("WAYLAND_DISPLAY")):
|
||||||
|
self.get_logger().warn("未检测到桌面显示环境，ACT双相机预览已停用。")
|
||||||
|
return
|
||||||
|
try:
|
||||||
|
import cv2
|
||||||
|
except ImportError as exc:
|
||||||
|
self.get_logger().warn(f"OpenCV不可用，ACT双相机预览已停用：{exc}")
|
||||||
|
return
|
||||||
|
self._preview_stop.clear()
|
||||||
|
self._preview_thread = threading.Thread(
|
||||||
|
target=self._preview_loop,
|
||||||
|
args=(cv2,),
|
||||||
|
name="act_camera_preview",
|
||||||
|
daemon=True,
|
||||||
|
)
|
||||||
|
self._preview_thread.start()
|
||||||
|
|
||||||
|
def _preview_loop(self, cv2) -> None:
|
||||||
|
try:
|
||||||
|
cv2.namedWindow(self.PREVIEW_WINDOW, cv2.WINDOW_NORMAL)
|
||||||
|
while not self._preview_stop.is_set():
|
||||||
|
cv2.imshow(self.PREVIEW_WINDOW, self._compose_preview(cv2))
|
||||||
|
key = cv2.waitKey(1) & 0xFF
|
||||||
|
if key in (ord("q"), ord("Q"), 27):
|
||||||
|
break
|
||||||
|
if cv2.getWindowProperty(
|
||||||
|
self.PREVIEW_WINDOW,
|
||||||
|
cv2.WND_PROP_VISIBLE,
|
||||||
|
) < 1:
|
||||||
|
break
|
||||||
|
self._preview_stop.wait(0.1)
|
||||||
|
except Exception as exc:
|
||||||
|
self.get_logger().warn(f"ACT双相机预览已停用：{exc}")
|
||||||
|
finally:
|
||||||
|
try:
|
||||||
|
cv2.destroyWindow(self.PREVIEW_WINDOW)
|
||||||
|
except Exception:
|
||||||
|
pass
|
||||||
|
|
||||||
|
def _stop_preview(self) -> None:
|
||||||
|
self._preview_stop.set()
|
||||||
|
if self._preview_thread is not None:
|
||||||
|
self._preview_thread.join(timeout=2.0)
|
||||||
|
self._preview_thread = None
|
||||||
|
```
|
||||||
|
|
||||||
|
`close()` 的第一步调用 `self._stop_preview()`，然后保持现有相机停止和目录锁释放顺序。
|
||||||
|
|
||||||
|
- [ ] **步骤 8：运行预览相关测试并确认通过**
|
||||||
|
|
||||||
|
运行：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
cd /home/robot/WS_xr
|
||||||
|
source /opt/ros/humble/setup.bash
|
||||||
|
pytest src/xr_rm_teleop/test/test_act_episode_recorder.py \
|
||||||
|
-k "rolling_fps or preview or close_stops_preview" -v
|
||||||
|
```
|
||||||
|
|
||||||
|
预期：所有选中测试通过，测试过程不打开真实窗口或相机。
|
||||||
|
|
||||||
|
- [ ] **步骤 9：运行 ACT 录制器完整单元测试**
|
||||||
|
|
||||||
|
运行：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
cd /home/robot/WS_xr
|
||||||
|
source /opt/ros/humble/setup.bash
|
||||||
|
pytest src/xr_rm_teleop/test/test_act_episode_recorder.py -v
|
||||||
|
```
|
||||||
|
|
||||||
|
预期：全部通过。
|
||||||
|
|
||||||
|
- [ ] **步骤 10：提交任务 2**
|
||||||
|
|
||||||
|
```bash
|
||||||
|
git add src/xr_rm_teleop/xr_rm_teleop/act_episode_recorder.py \
|
||||||
|
src/xr_rm_teleop/test/test_act_episode_recorder.py
|
||||||
|
git commit -m "feat: 添加ACT双相机实时预览"
|
||||||
|
```
|
||||||
|
|
||||||
|
### 任务 3：更新使用说明并完成工作空间验证
|
||||||
|
|
||||||
|
**文件：**
|
||||||
|
|
||||||
|
- 修改：`README.md:144-169`
|
||||||
|
|
||||||
|
- [ ] **步骤 1：更新 ACT 数采说明**
|
||||||
|
|
||||||
|
在 ACT episode 启动命令后补充：
|
||||||
|
|
||||||
|
```markdown
|
||||||
|
`record_act:=true` 启动后默认显示全局 D455 和右腕 D405 双路画面，并显示实时
|
||||||
|
FPS、真实丢帧率、帧龄、双相机时间差、录制状态、episode 编号和样本数。按
|
||||||
|
`Q`、`Esc` 或关闭窗口只会停止预览，ACT 相机采集和录制继续运行；没有桌面环境
|
||||||
|
或 OpenCV 显示失败时也不会影响录制。
|
||||||
|
|
||||||
|
相机采集线程观察到的真实掉帧仍会拒绝 episode。独立 `30 Hz` 控制和相机时钟
|
||||||
|
造成的 ACT 样本重复/跨帧只写入 HDF5 质量指标，不再误报为相机丢包。
|
||||||
|
```
|
||||||
|
|
||||||
|
- [ ] **步骤 2：运行文档和 Python 静态检查**
|
||||||
|
|
||||||
|
运行：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
cd /home/robot/WS_xr
|
||||||
|
source /opt/ros/humble/setup.bash
|
||||||
|
python -m py_compile \
|
||||||
|
src/xr_rm_teleop/xr_rm_teleop/act_episode_recorder.py \
|
||||||
|
src/xr_rm_teleop/test/test_act_episode_recorder.py
|
||||||
|
git diff --check
|
||||||
|
```
|
||||||
|
|
||||||
|
预期：命令返回码为 `0`，没有语法或空白错误。
|
||||||
|
|
||||||
|
- [ ] **步骤 3：运行项目要求的工作空间构建**
|
||||||
|
|
||||||
|
运行：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
cd /home/robot/WS_xr
|
||||||
|
source /opt/ros/humble/setup.bash
|
||||||
|
colcon build --symlink-install
|
||||||
|
```
|
||||||
|
|
||||||
|
预期：所有包构建成功。不得在 `/home/robot/WS_xr/src` 中运行该命令。
|
||||||
|
|
||||||
|
- [ ] **步骤 4：构建后再次运行 ACT 录制器测试**
|
||||||
|
|
||||||
|
运行：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
cd /home/robot/WS_xr
|
||||||
|
source /opt/ros/humble/setup.bash
|
||||||
|
source install/setup.bash
|
||||||
|
pytest src/xr_rm_teleop/test/test_act_episode_recorder.py -v
|
||||||
|
```
|
||||||
|
|
||||||
|
预期：全部通过。
|
||||||
|
|
||||||
|
- [ ] **步骤 5：确认提交范围并提交任务 3**
|
||||||
|
|
||||||
|
运行：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
git status --short
|
||||||
|
git diff -- README.md \
|
||||||
|
src/xr_rm_teleop/xr_rm_teleop/act_episode_recorder.py \
|
||||||
|
src/xr_rm_teleop/test/test_act_episode_recorder.py
|
||||||
|
git add README.md
|
||||||
|
git commit -m "docs: 更新ACT相机预览说明"
|
||||||
|
```
|
||||||
|
|
||||||
|
不得暂存或提交 `xr_rm_teleop/xr_rm_teleop/fun_peripheral.py`。
|
||||||
|
|
||||||
|
## 现场验证
|
||||||
|
|
||||||
|
自动化实施结束后，由用户在确认现场安全条件后运行现有 ACT 数采入口：
|
||||||
|
|
||||||
|
```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:=right use_mock:=false record_act:=true
|
||||||
|
```
|
||||||
|
|
||||||
|
该命令会连接右臂真机，代理不得自行执行。现场确认：
|
||||||
|
|
||||||
|
1. 双路画面默认出现且角色正确；
|
||||||
|
2. FPS、真实丢帧率、帧龄、双相机时间差和 ACT 状态持续更新；
|
||||||
|
3. 关闭窗口后录制状态和 HDF5 写入继续；
|
||||||
|
4. 正常异步重复/跨帧的 episode 能保存；
|
||||||
|
5. HDF5 属性包含真实采集质量与两路采样重复/跨帧指标。
|
||||||
@@ -0,0 +1,218 @@
|
|||||||
|
# ACT 双相机预览与采样质量判定修正设计
|
||||||
|
|
||||||
|
## 背景
|
||||||
|
|
||||||
|
右臂番茄采摘 ACT 采集当前以 `90 Hz` 接收原子控制消息，每三个控制周期生成一个
|
||||||
|
`30 Hz` 样本，并为该样本选择不晚于控制时刻的最新全局 D455 和右腕 D405 RGB
|
||||||
|
帧。现阶段测试暴露出两个相机相关问题：
|
||||||
|
|
||||||
|
1. ACT 采样帧号偶尔出现 `986 → 986 → 988` 一类重复和跨帧，episode 因
|
||||||
|
`camera_sample_drop_ratio` 被拒绝；
|
||||||
|
2. ACT 数采启动后没有现场预览，操作者不能直观看到两路画面、相机质量和录制
|
||||||
|
状态。
|
||||||
|
|
||||||
|
已保存 HDF5 的调查结果表明，被 `camera_sample_drop_ratio` 拒绝的 episode 中，
|
||||||
|
相机采集线程统计的 `camera_high_drop_ratio` 和
|
||||||
|
`camera_right_wrist_drop_ratio` 均为 `0.0`。异常主要出现在独立运行的两个
|
||||||
|
`30 Hz` 时钟之间：控制采样早于下一张相机帧时会再次选择上一帧，下一个采样点
|
||||||
|
则可能选择更新两号的帧。相机采集线程实际收到过中间帧，因此这不是 RealSense
|
||||||
|
传输丢帧。
|
||||||
|
|
||||||
|
## 目标
|
||||||
|
|
||||||
|
- 保持现有因果时间对齐，不选择控制时刻之后的图像；
|
||||||
|
- 只用相机采集线程观察到的真实帧号缺失率判定相机丢帧；
|
||||||
|
- 将 ACT 样本中的重复帧和跨帧改为可观测指标，不再据此拒绝 episode；
|
||||||
|
- `record_act:=true` 启动录制器时默认显示全局 D455 和右腕 D405 实时画面；
|
||||||
|
- 在预览中显示相机质量和 ACT 录制状态；
|
||||||
|
- 预览关闭或显示故障不得影响相机采集、HDF5 写入和机器人控制。
|
||||||
|
|
||||||
|
## 非目标
|
||||||
|
|
||||||
|
本次不实现：
|
||||||
|
|
||||||
|
- 原始视频流保存和离线重采样；
|
||||||
|
- D455 与 D405 硬件同步；
|
||||||
|
- ROS 图像话题、Web 界面或新的相机进程；
|
||||||
|
- 深度图、点云、图像压缩或 HDF5 核心训练字段变更；
|
||||||
|
- 关节曲线、机器人控制按钮或预览截图；
|
||||||
|
- 夹爪实际开度反馈或 episode 终点裁剪逻辑修改；
|
||||||
|
- 修改 `record_act` 的全局默认值；
|
||||||
|
- 修改工作空间/圆柱限位、速度限制、指令超时、安全停止或真机连接行为。
|
||||||
|
|
||||||
|
夹爪录制继续使用现有操作顺序：保持 Grip，按 Trigger 打开并等待打开命令完成，
|
||||||
|
然后松开 Grip，最后按 B 保存。
|
||||||
|
|
||||||
|
## 方案选择
|
||||||
|
|
||||||
|
### 采用：保持当前因果对齐
|
||||||
|
|
||||||
|
数据流保持为：
|
||||||
|
|
||||||
|
```text
|
||||||
|
90 Hz ActControlSample
|
||||||
|
↓ 每三个连续控制周期选择一次
|
||||||
|
30 Hz ACT 目标时刻
|
||||||
|
↓ 分别选择 host_monotonic_ns 不晚于目标时刻的最新帧
|
||||||
|
D455 图像 + D405 图像 + qpos + action
|
||||||
|
↓
|
||||||
|
现有 HDF5
|
||||||
|
```
|
||||||
|
|
||||||
|
该方案维持现有训练数据语义，图像不会包含控制时刻之后的未来信息。少量重复帧和
|
||||||
|
跨帧作为异步时钟相位漂移保留在数据中，并用明确指标量化。
|
||||||
|
|
||||||
|
### 未采用：以 D455 为软件主时钟
|
||||||
|
|
||||||
|
该方案可以避免 D455 重复帧，但会使控制序号间隔不再固定，并需要重新定义状态、
|
||||||
|
动作和 D405 图像的对齐语义，当前收益不足以覆盖兼容性成本。
|
||||||
|
|
||||||
|
### 未采用：保存原始流并离线重采样
|
||||||
|
|
||||||
|
该方案最灵活，但需要新的原始存储结构和转换工具，无压缩双路 RGB 也会显著增加
|
||||||
|
存储开销。只有后续训练表明快速接触或释放动作受到当前单帧级时间抖动影响时，才
|
||||||
|
考虑升级。
|
||||||
|
|
||||||
|
## 相机质量指标
|
||||||
|
|
||||||
|
### 真实采集质量
|
||||||
|
|
||||||
|
`CameraBuffer` 在每次收到 RealSense 帧时比较相邻原始帧号。原始帧号向前跳过的
|
||||||
|
数量计入真实丢帧数；帧号不递增单独计为回退/重启异常。episode 期间的统计写入
|
||||||
|
现有或新增 HDF5 属性：
|
||||||
|
|
||||||
|
- `camera_high_fps`；
|
||||||
|
- `camera_right_wrist_fps`；
|
||||||
|
- `camera_high_drop_ratio`；
|
||||||
|
- `camera_right_wrist_drop_ratio`；
|
||||||
|
- `camera_high_frame_number_regression_count`；
|
||||||
|
- `camera_right_wrist_frame_number_regression_count`。
|
||||||
|
|
||||||
|
以下条件继续拒绝 episode：
|
||||||
|
|
||||||
|
- 任一路实际采集 FPS 小于配置的 `min_camera_fps`，当前为 `27 Hz`；
|
||||||
|
- 任一路真实丢帧率大于配置的 `max_drop_ratio`，当前为 `1%`；
|
||||||
|
- 任一路图像帧龄超过 `max_camera_age_ms`，当前为 `50 ms`；
|
||||||
|
- 两路所选图像的主机单调时间差超过 `max_camera_skew_ms`，当前为 `50 ms`；
|
||||||
|
- 任一路原始帧号发生回退或重启；
|
||||||
|
- 相机启动、取帧或图像格式发生错误。
|
||||||
|
|
||||||
|
### ACT 采样相位指标
|
||||||
|
|
||||||
|
对每路写入 HDF5 的 ACT 样本帧号计算相邻差值：
|
||||||
|
|
||||||
|
```text
|
||||||
|
diff == 0：重复使用同一相机帧
|
||||||
|
diff == 1：理想连续取样
|
||||||
|
diff > 1：相邻 ACT 样本跨过相机帧
|
||||||
|
diff < 0：帧号回退，仍按异常拒绝
|
||||||
|
```
|
||||||
|
|
||||||
|
对 `N` 个 ACT 样本，分母为 `max(1, N - 1)`：
|
||||||
|
|
||||||
|
```text
|
||||||
|
sample_repeat_ratio = count(diff == 0) / max(1, N - 1)
|
||||||
|
sample_skip_ratio = count(diff > 1) / max(1, N - 1)
|
||||||
|
```
|
||||||
|
|
||||||
|
分别写入：
|
||||||
|
|
||||||
|
- `cam_high_sample_repeat_ratio`；
|
||||||
|
- `cam_high_sample_skip_ratio`；
|
||||||
|
- `cam_wrist_sample_repeat_ratio`；
|
||||||
|
- `cam_wrist_sample_skip_ratio`。
|
||||||
|
|
||||||
|
这些指标写入保存/拒绝文件属性，并在保存日志中摘要输出，但不参与 episode 接受
|
||||||
|
判定。原有 `camera_sample_drop_ratio` 拒绝路径移除，避免把软件采样相位漂移误报
|
||||||
|
为相机传输丢帧。
|
||||||
|
|
||||||
|
## 双路实时预览
|
||||||
|
|
||||||
|
### 生命周期
|
||||||
|
|
||||||
|
`ActEpisodeRecorder` 成功启动两台相机后，默认启动一个独立的 OpenCV 预览线程。
|
||||||
|
该线程只读取两个现有 `CameraBuffer` 的最新帧和统计，不打开新的 RealSense
|
||||||
|
pipeline，也不发布 ROS 图像话题。
|
||||||
|
|
||||||
|
预览约以 `10 Hz` 刷新，降低显示开销；相机采集和 HDF5 录制仍保持 `30 Hz`。
|
||||||
|
节点退出时先通知并回收预览线程，再停止两台相机。关闭预览不会重新启动。
|
||||||
|
|
||||||
|
`arm_debug.launch.py` 继续保持 `record_act:=false` 的安全默认值。tools 中现有 ACT
|
||||||
|
数采入口已经显式传入 `arm:=right use_mock:=false record_act:=true`，因此无需修改
|
||||||
|
launch 参数或增加新的预览开关；只要录制器节点启动，预览就默认启动。
|
||||||
|
|
||||||
|
### 布局与信息
|
||||||
|
|
||||||
|
窗口使用左右并排的两块画面：
|
||||||
|
|
||||||
|
```text
|
||||||
|
┌──────────────────────┬──────────────────────┐
|
||||||
|
│ 全局 D455 │ 右腕 D405 │
|
||||||
|
│ 实时画面 │ 实时画面 │
|
||||||
|
│ FPS / 当前帧号 │ FPS / 当前帧号 │
|
||||||
|
│ 接收数 / 真实丢帧率 │ 接收数 / 真实丢帧率 │
|
||||||
|
├──────────────────────┴──────────────────────┤
|
||||||
|
│ 两路帧龄 / 两相机时间差 │
|
||||||
|
│ ACT 状态 / episode 编号 / 已写入样本数 │
|
||||||
|
└─────────────────────────────────────────────┘
|
||||||
|
```
|
||||||
|
|
||||||
|
实时 FPS 使用相机采集线程最近约两秒的到帧时间计算，而不是预览刷新率。未开始
|
||||||
|
episode 时编号和样本数显示为空或 `-`；录制过程中读取当前录制器状态。
|
||||||
|
|
||||||
|
### 关闭和异常处理
|
||||||
|
|
||||||
|
- 按 `Q`、`Esc` 或点击窗口关闭按钮，只停止预览线程；
|
||||||
|
- 没有 `DISPLAY` 和 `WAYLAND_DISPLAY` 时不创建窗口，只记录一次警告；
|
||||||
|
- `cv2` 导入失败、窗口创建失败或显示过程中抛出异常时，记录一次警告并停止
|
||||||
|
预览；
|
||||||
|
- 预览异常不改变 ACT 状态，不关闭相机，不丢弃或拒绝 episode；
|
||||||
|
- RealSense 相机本身启动或采集失败仍沿用现有预检/拒绝行为；
|
||||||
|
- 预览线程不得调用机器人适配器、发布夹爪命令或阻塞 ROS 控制样本回调。
|
||||||
|
|
||||||
|
本次复用 `xr` 运行环境中现有的 OpenCV，不新增 Python 或 ROS 依赖。
|
||||||
|
|
||||||
|
## 代码范围
|
||||||
|
|
||||||
|
预计只修改：
|
||||||
|
|
||||||
|
- `xr_rm_teleop/xr_rm_teleop/act_episode_recorder.py`：真实丢帧与采样相位指标、
|
||||||
|
双路预览及生命周期；
|
||||||
|
- `xr_rm_teleop/test/test_act_episode_recorder.py`：质量判定和预览失败隔离测试；
|
||||||
|
- `README.md`：补充 ACT 默认双路预览及关闭方式。
|
||||||
|
|
||||||
|
无需修改 `arm_debug.launch.py`、`launcher_ui.py`、ROS 消息、相机配置或机器人控制
|
||||||
|
节点。实现继续保留在现有录制器文件中，只提取必要的纯计算/渲染辅助函数，不创建
|
||||||
|
通用相机框架。
|
||||||
|
|
||||||
|
## 测试与验证
|
||||||
|
|
||||||
|
自动化测试不连接 RealSense、RM75 或真实夹爪：
|
||||||
|
|
||||||
|
1. 构造 `986 → 986 → 988`，验证重复率和跨帧率均被记录，episode 不再因
|
||||||
|
`camera_sample_drop_ratio` 被拒绝；
|
||||||
|
2. 在 `CameraBuffer` 原始输入中跳过帧号，验证真实丢帧数和丢帧率仍触发拒绝；
|
||||||
|
3. 构造原始帧号回退，验证 episode 被拒绝；
|
||||||
|
4. 验证两路采样指标分别计算，且分母在单样本时安全；
|
||||||
|
5. 使用替代显示函数验证按键关闭、窗口关闭、无显示环境和显示异常只停用预览，
|
||||||
|
不改变录制状态；
|
||||||
|
6. 运行 `xr_rm_teleop/test/test_act_episode_recorder.py`；
|
||||||
|
7. 从 `/home/robot/WS_xr` 执行：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
source /opt/ros/humble/setup.bash
|
||||||
|
colcon build --symlink-install
|
||||||
|
```
|
||||||
|
|
||||||
|
现场再通过现有 ACT 数采入口验证窗口布局、画面刷新和关闭行为。该验证会连接右臂
|
||||||
|
真机，必须由用户明确执行；自动化过程不得启动真机 launch 或移动机器人。
|
||||||
|
|
||||||
|
## 完成标准
|
||||||
|
|
||||||
|
- 真实相机丢帧、帧龄超限、双相机偏差和相机错误仍能拒绝不合格 episode；
|
||||||
|
- 正常异步相位漂移造成的样本重复/跨帧不再拒绝 episode；
|
||||||
|
- HDF5 和日志能够区分真实丢帧与 ACT 采样相位指标；
|
||||||
|
- ACT 数采启动时默认出现全局 D455 与右腕 D405 双路预览；
|
||||||
|
- 关闭或损坏预览不影响 ACT 采集；
|
||||||
|
- 现有 HDF5 核心结构、机器人控制和安全行为保持不变；
|
||||||
|
- 相关测试与工作空间构建通过。
|
||||||
@@ -45,7 +45,7 @@ left_arm_teleop:
|
|||||||
realtime_push_host_ip: 192.168.192.148
|
realtime_push_host_ip: 192.168.192.148
|
||||||
realtime_push_port: 8089
|
realtime_push_port: 8089
|
||||||
realtime_push_cycle_ms: 5
|
realtime_push_cycle_ms: 5
|
||||||
avoid_singularity: 1
|
avoid_singularity: 0
|
||||||
follow: false
|
follow: false
|
||||||
canfd_trajectory_mode: 2
|
canfd_trajectory_mode: 2
|
||||||
canfd_radio: 0
|
canfd_radio: 0
|
||||||
@@ -102,7 +102,7 @@ right_arm_teleop:
|
|||||||
realtime_push_host_ip: 192.168.192.148
|
realtime_push_host_ip: 192.168.192.148
|
||||||
realtime_push_port: 8090
|
realtime_push_port: 8090
|
||||||
realtime_push_cycle_ms: 5
|
realtime_push_cycle_ms: 5
|
||||||
avoid_singularity: 1
|
avoid_singularity: 0
|
||||||
follow: false
|
follow: false
|
||||||
canfd_trajectory_mode: 2
|
canfd_trajectory_mode: 2
|
||||||
canfd_radio: 0
|
canfd_radio: 0
|
||||||
|
|||||||
@@ -38,7 +38,7 @@ single_arm_velocity_teleop:
|
|||||||
realtime_push_host_ip: 192.168.192.148
|
realtime_push_host_ip: 192.168.192.148
|
||||||
realtime_push_port: 8089
|
realtime_push_port: 8089
|
||||||
realtime_push_cycle_ms: 5
|
realtime_push_cycle_ms: 5
|
||||||
avoid_singularity: 1
|
avoid_singularity: 0
|
||||||
follow: false
|
follow: false
|
||||||
canfd_trajectory_mode: 2
|
canfd_trajectory_mode: 2
|
||||||
canfd_radio: 0
|
canfd_radio: 0
|
||||||
|
|||||||
@@ -37,7 +37,7 @@ single_arm_velocity_teleop:
|
|||||||
realtime_push_host_ip: 192.168.192.148
|
realtime_push_host_ip: 192.168.192.148
|
||||||
realtime_push_port: 8090
|
realtime_push_port: 8090
|
||||||
realtime_push_cycle_ms: 5
|
realtime_push_cycle_ms: 5
|
||||||
avoid_singularity: 1
|
avoid_singularity: 0
|
||||||
# 厂商 MovejCANFD 示例默认低跟随；高跟随仅在验证规划轨迹后单独开启。
|
# 厂商 MovejCANFD 示例默认低跟随；高跟随仅在验证规划轨迹后单独开启。
|
||||||
follow: false
|
follow: false
|
||||||
canfd_trajectory_mode: 2
|
canfd_trajectory_mode: 2
|
||||||
|
|||||||
@@ -1,2 +0,0 @@
|
|||||||
*
|
|
||||||
!.gitignore
|
|
||||||
@@ -46,6 +46,14 @@ class LauncherCommandsTest(unittest.TestCase):
|
|||||||
"Open ROS Topic/Node List Monitor",
|
"Open ROS Topic/Node List Monitor",
|
||||||
"Open Controller Topic Monitor",
|
"Open Controller Topic Monitor",
|
||||||
],
|
],
|
||||||
|
"ACT Data Collection": [
|
||||||
|
"Right Arm ACT Data Collection Launch",
|
||||||
|
"XRobotoolkit UDP Bridge (90 Hz)",
|
||||||
|
"Open ACT Recording Status",
|
||||||
|
"Open Right Arm ACT Control Sample Hz",
|
||||||
|
"Open ROS Topic/Node List Monitor",
|
||||||
|
"Open Controller Topic Monitor",
|
||||||
|
],
|
||||||
"Diagnostics": [
|
"Diagnostics": [
|
||||||
"ROS Doctor Report",
|
"ROS Doctor Report",
|
||||||
"XR-RM Bringup Prefix",
|
"XR-RM Bringup Prefix",
|
||||||
@@ -79,6 +87,22 @@ class LauncherCommandsTest(unittest.TestCase):
|
|||||||
commands["2. Dual Arm MuJoCo Real Hardware Launch"],
|
commands["2. Dual Arm MuJoCo Real Hardware Launch"],
|
||||||
)
|
)
|
||||||
|
|
||||||
|
def test_act_mode_uses_confirmed_right_hardware_topics(self) -> None:
|
||||||
|
commands = dict(launcher_ui.build_commands_by_mode("ACT Data Collection"))
|
||||||
|
|
||||||
|
self.assertIn(
|
||||||
|
"arm:=right use_mock:=false record_act:=true",
|
||||||
|
commands["1. Right Arm ACT Data Collection Launch"],
|
||||||
|
)
|
||||||
|
self.assertEqual(
|
||||||
|
commands["3. Open ACT Recording Status"],
|
||||||
|
"ros2 topic echo /act/recording_status",
|
||||||
|
)
|
||||||
|
self.assertEqual(
|
||||||
|
commands["4. Open Right Arm ACT Control Sample Hz"],
|
||||||
|
"ros2 topic hz /xr_rm/right_rm75/act_control_sample",
|
||||||
|
)
|
||||||
|
|
||||||
def test_cmd_vel_monitor_is_completely_removed(self) -> None:
|
def test_cmd_vel_monitor_is_completely_removed(self) -> None:
|
||||||
self.assertFalse(hasattr(launcher_ui, "CMD_VEL_MONITOR_ACTION"))
|
self.assertFalse(hasattr(launcher_ui, "CMD_VEL_MONITOR_ACTION"))
|
||||||
for mode in launcher_ui.MODES:
|
for mode in launcher_ui.MODES:
|
||||||
|
|||||||
@@ -1,7 +1,7 @@
|
|||||||
#!/usr/bin/env python3
|
#!/usr/bin/env python3
|
||||||
"""XR-RM 桌面调试启动器。
|
"""XR-RM 桌面调试启动器。
|
||||||
|
|
||||||
提供 Tkinter 图形界面，按“仿真/MuJoCo/真机/诊断”组织常用 ROS2 launch、
|
提供 Tkinter 图形界面，按“仿真/MuJoCo/真机/ACT采集/诊断”组织常用 ROS2 launch、
|
||||||
sample_udp_sender、topic 监控和环境检查命令，降低现场调试时的命令输入成本。
|
sample_udp_sender、topic 监控和环境检查命令，降低现场调试时的命令输入成本。
|
||||||
"""
|
"""
|
||||||
|
|
||||||
@@ -76,6 +76,7 @@ MODES = [
|
|||||||
"Simulation",
|
"Simulation",
|
||||||
"MuJoCo",
|
"MuJoCo",
|
||||||
"Real Hardware",
|
"Real Hardware",
|
||||||
|
"ACT Data Collection",
|
||||||
"Diagnostics",
|
"Diagnostics",
|
||||||
]
|
]
|
||||||
|
|
||||||
@@ -275,6 +276,23 @@ def build_commands_by_mode(mode: str) -> list[tuple[str, str]]:
|
|||||||
("Right Gripper Open", _tool_command("right", True)),
|
("Right Gripper Open", _tool_command("right", True)),
|
||||||
("Right Gripper Close", _tool_command("right", False)),
|
("Right Gripper Close", _tool_command("right", False)),
|
||||||
]
|
]
|
||||||
|
elif mode == "ACT Data Collection":
|
||||||
|
items = [
|
||||||
|
(
|
||||||
|
"Right Arm ACT Data Collection Launch",
|
||||||
|
"ros2 launch xr_rm_bringup arm_debug.launch.py "
|
||||||
|
"arm:=right use_mock:=false record_act:=true",
|
||||||
|
),
|
||||||
|
("XRobotoolkit UDP Bridge (90 Hz)", _xrobotoolkit_bridge_command()),
|
||||||
|
(
|
||||||
|
"Open ACT Recording Status",
|
||||||
|
"ros2 topic echo /act/recording_status",
|
||||||
|
),
|
||||||
|
(
|
||||||
|
"Open Right Arm ACT Control Sample Hz",
|
||||||
|
"ros2 topic hz /xr_rm/right_rm75/act_control_sample",
|
||||||
|
),
|
||||||
|
]
|
||||||
else:
|
else:
|
||||||
items = [
|
items = [
|
||||||
("ROS Doctor Report", "ros2 doctor --report"),
|
("ROS Doctor Report", "ros2 doctor --report"),
|
||||||
@@ -329,7 +347,7 @@ class LauncherApp:
|
|||||||
mode_frame,
|
mode_frame,
|
||||||
textvariable=self.mode_var,
|
textvariable=self.mode_var,
|
||||||
values=MODES,
|
values=MODES,
|
||||||
width=16,
|
width=20,
|
||||||
state="readonly",
|
state="readonly",
|
||||||
)
|
)
|
||||||
self.mode_combo.grid(row=0, column=1, sticky="ew")
|
self.mode_combo.grid(row=0, column=1, sticky="ew")
|
||||||
|
|||||||
@@ -1,5 +1,6 @@
|
|||||||
import queue
|
import queue
|
||||||
import threading
|
import threading
|
||||||
|
import time
|
||||||
from types import SimpleNamespace
|
from types import SimpleNamespace
|
||||||
|
|
||||||
import numpy as np
|
import numpy as np
|
||||||
@@ -29,6 +30,7 @@ from xr_rm_teleop.act_episode_recorder import (
|
|||||||
next_episode_index,
|
next_episode_index,
|
||||||
publish_without_overwrite,
|
publish_without_overwrite,
|
||||||
recover_partial_files,
|
recover_partial_files,
|
||||||
|
sample_frame_metrics,
|
||||||
select_camera_pair,
|
select_camera_pair,
|
||||||
select_frame,
|
select_frame,
|
||||||
validate_episode,
|
validate_episode,
|
||||||
@@ -296,6 +298,50 @@ def test_camera_buffer_is_bounded_and_counts_dropped_frames():
|
|||||||
assert stats.fps == pytest.approx(800.0)
|
assert stats.fps == pytest.approx(800.0)
|
||||||
|
|
||||||
|
|
||||||
|
def test_camera_buffer_counts_frame_number_regressions():
|
||||||
|
buffer = CameraBuffer(maxlen=4)
|
||||||
|
for frame_number in (100, 101, 1, 2):
|
||||||
|
buffer.push(
|
||||||
|
CameraFrame(
|
||||||
|
_image(frame_number),
|
||||||
|
frame_number,
|
||||||
|
float(frame_number),
|
||||||
|
time.monotonic_ns(),
|
||||||
|
)
|
||||||
|
)
|
||||||
|
|
||||||
|
stats = buffer.stats()
|
||||||
|
|
||||||
|
assert stats.frame_number_regression_count == 1
|
||||||
|
assert stats.dropped_frames == 0
|
||||||
|
|
||||||
|
|
||||||
|
def test_sample_frame_metrics_separate_repeats_skips_and_regressions():
|
||||||
|
repeat, skip, regression = sample_frame_metrics(
|
||||||
|
np.asarray((986, 986, 988), dtype=np.uint64)
|
||||||
|
)
|
||||||
|
|
||||||
|
assert repeat == pytest.approx(0.5)
|
||||||
|
assert skip == pytest.approx(0.5)
|
||||||
|
assert regression == 0
|
||||||
|
|
||||||
|
|
||||||
|
def test_camera_buffer_reports_two_second_rolling_fps():
|
||||||
|
buffer = CameraBuffer(maxlen=4)
|
||||||
|
start_ns = 10_000_000_000
|
||||||
|
for index in range(61):
|
||||||
|
buffer.push(
|
||||||
|
CameraFrame(
|
||||||
|
_image(index),
|
||||||
|
index,
|
||||||
|
float(index),
|
||||||
|
start_ns + index * 33_333_333,
|
||||||
|
)
|
||||||
|
)
|
||||||
|
|
||||||
|
assert buffer.stats().rolling_fps == pytest.approx(30.0, rel=0.02)
|
||||||
|
|
||||||
|
|
||||||
requires_h5py = pytest.mark.skipif(
|
requires_h5py = pytest.mark.skipif(
|
||||||
h5py is None,
|
h5py is None,
|
||||||
reason="h5py is not installed",
|
reason="h5py is not installed",
|
||||||
@@ -506,6 +552,8 @@ def _valid_episode(tmp_path):
|
|||||||
root.attrs["camera_right_wrist_fps"] = 30.0
|
root.attrs["camera_right_wrist_fps"] = 30.0
|
||||||
root.attrs["camera_high_drop_ratio"] = 0.0
|
root.attrs["camera_high_drop_ratio"] = 0.0
|
||||||
root.attrs["camera_right_wrist_drop_ratio"] = 0.0
|
root.attrs["camera_right_wrist_drop_ratio"] = 0.0
|
||||||
|
root.attrs["camera_high_frame_number_regression_count"] = 0
|
||||||
|
root.attrs["camera_right_wrist_frame_number_regression_count"] = 0
|
||||||
return path
|
return path
|
||||||
|
|
||||||
|
|
||||||
@@ -518,6 +566,19 @@ def test_validate_episode_accepts_valid_file(tmp_path):
|
|||||||
assert report.metrics["control_hz"] == pytest.approx(30.0, rel=1e-5)
|
assert report.metrics["control_hz"] == pytest.approx(30.0, rel=1e-5)
|
||||||
|
|
||||||
|
|
||||||
|
@requires_h5py
|
||||||
|
def test_validate_episode_accepts_async_camera_phase_drift(tmp_path):
|
||||||
|
path = _valid_episode(tmp_path)
|
||||||
|
with h5py.File(path, "r+") as root:
|
||||||
|
root["debug/cameras/cam_high_frame_number"][:] = (986, 986, 988)
|
||||||
|
|
||||||
|
report = validate_episode(path, _quality_limits())
|
||||||
|
|
||||||
|
assert report.accepted
|
||||||
|
assert report.metrics["cam_high_sample_repeat_ratio"] == pytest.approx(0.5)
|
||||||
|
assert report.metrics["cam_high_sample_skip_ratio"] == pytest.approx(0.5)
|
||||||
|
|
||||||
|
|
||||||
def _mutate_episode(path, mutation):
|
def _mutate_episode(path, mutation):
|
||||||
with h5py.File(path, "r+") as root:
|
with h5py.File(path, "r+") as root:
|
||||||
if mutation == "short_episode":
|
if mutation == "short_episode":
|
||||||
@@ -547,6 +608,8 @@ def _mutate_episode(path, mutation):
|
|||||||
root.attrs["camera_high_fps"] = 20.0
|
root.attrs["camera_high_fps"] = 20.0
|
||||||
elif mutation == "camera_drop":
|
elif mutation == "camera_drop":
|
||||||
root.attrs["camera_right_wrist_drop_ratio"] = 0.02
|
root.attrs["camera_right_wrist_drop_ratio"] = 0.02
|
||||||
|
elif mutation == "camera_frame_regression":
|
||||||
|
root.attrs["camera_high_frame_number_regression_count"] = 1
|
||||||
elif mutation == "camera_age":
|
elif mutation == "camera_age":
|
||||||
root["debug/timestamps/cam_high_age_ms"][1] = 60.0
|
root["debug/timestamps/cam_high_age_ms"][1] = 60.0
|
||||||
elif mutation == "camera_skew":
|
elif mutation == "camera_skew":
|
||||||
@@ -571,6 +634,7 @@ def _mutate_episode(path, mutation):
|
|||||||
("control_fault", "control_fault"),
|
("control_fault", "control_fault"),
|
||||||
("camera_fps", "camera_fps"),
|
("camera_fps", "camera_fps"),
|
||||||
("camera_drop", "camera_drop_ratio"),
|
("camera_drop", "camera_drop_ratio"),
|
||||||
|
("camera_frame_regression", "camera_frame_number_regression"),
|
||||||
("camera_age", "camera_frame_too_old"),
|
("camera_age", "camera_frame_too_old"),
|
||||||
("camera_skew", "camera_skew"),
|
("camera_skew", "camera_skew"),
|
||||||
("final_gripper_closed", "final_gripper_not_open"),
|
("final_gripper_closed", "final_gripper_not_open"),
|
||||||
@@ -616,14 +680,19 @@ class _StatusPublisher:
|
|||||||
|
|
||||||
|
|
||||||
class _Logger:
|
class _Logger:
|
||||||
def info(self, *_args, **_kwargs):
|
def __init__(self):
|
||||||
pass
|
self.infos = []
|
||||||
|
self.warnings = []
|
||||||
|
self.errors = []
|
||||||
|
|
||||||
def warn(self, *_args, **_kwargs):
|
def info(self, message, *_args, **_kwargs):
|
||||||
pass
|
self.infos.append(message)
|
||||||
|
|
||||||
def error(self, *_args, **_kwargs):
|
def warn(self, message, *_args, **_kwargs):
|
||||||
pass
|
self.warnings.append(message)
|
||||||
|
|
||||||
|
def error(self, message, *_args, **_kwargs):
|
||||||
|
self.errors.append(message)
|
||||||
|
|
||||||
|
|
||||||
def _control_message(seq, control_ns, *, grip=True):
|
def _control_message(seq, control_ns, *, grip=True):
|
||||||
@@ -705,10 +774,53 @@ def _recorder_for_test(tmp_path):
|
|||||||
recorder._status_pub = _StatusPublisher()
|
recorder._status_pub = _StatusPublisher()
|
||||||
recorder._now_ns = lambda: now_ns
|
recorder._now_ns = lambda: now_ns
|
||||||
recorder._disk_usage = lambda _path: SimpleNamespace(free=5 * 1024**3)
|
recorder._disk_usage = lambda _path: SimpleNamespace(free=5 * 1024**3)
|
||||||
recorder.get_logger = lambda: _Logger()
|
recorder._logger = _Logger()
|
||||||
|
recorder.get_logger = lambda: recorder._logger
|
||||||
return recorder
|
return recorder
|
||||||
|
|
||||||
|
|
||||||
|
def test_preview_failure_does_not_change_recording_state():
|
||||||
|
recorder = object.__new__(ActEpisodeRecorder)
|
||||||
|
recorder._session = _recording_session()
|
||||||
|
recorder._preview_stop = threading.Event()
|
||||||
|
recorder._preview_thread = None
|
||||||
|
recorder._logger = _Logger()
|
||||||
|
recorder.get_logger = lambda: recorder._logger
|
||||||
|
|
||||||
|
class FailingCv2:
|
||||||
|
WINDOW_NORMAL = 0
|
||||||
|
|
||||||
|
@staticmethod
|
||||||
|
def namedWindow(*_args):
|
||||||
|
raise RuntimeError("no display")
|
||||||
|
|
||||||
|
recorder._preview_loop(FailingCv2())
|
||||||
|
|
||||||
|
assert recorder.state is RecordingState.RECORDING
|
||||||
|
assert recorder._logger.warnings == [
|
||||||
|
"ACT双相机预览已停用：no display"
|
||||||
|
]
|
||||||
|
|
||||||
|
|
||||||
|
def test_close_stops_preview_before_cameras_and_releases_lock():
|
||||||
|
events = []
|
||||||
|
recorder = object.__new__(ActEpisodeRecorder)
|
||||||
|
recorder._stop_preview = lambda: events.append("preview")
|
||||||
|
recorder._high_camera = SimpleNamespace(
|
||||||
|
stop=lambda: events.append("high")
|
||||||
|
)
|
||||||
|
recorder._wrist_camera = SimpleNamespace(
|
||||||
|
stop=lambda: events.append("wrist")
|
||||||
|
)
|
||||||
|
recorder._directory_lock = SimpleNamespace(
|
||||||
|
release=lambda: events.append("lock")
|
||||||
|
)
|
||||||
|
|
||||||
|
recorder.close()
|
||||||
|
|
||||||
|
assert events == ["preview", "high", "wrist", "lock"]
|
||||||
|
|
||||||
|
|
||||||
@requires_h5py
|
@requires_h5py
|
||||||
def test_preflight_requires_open_gripper_fresh_inputs_and_disk_space(tmp_path):
|
def test_preflight_requires_open_gripper_fresh_inputs_and_disk_space(tmp_path):
|
||||||
recorder = _recorder_for_test(tmp_path)
|
recorder = _recorder_for_test(tmp_path)
|
||||||
@@ -737,8 +849,8 @@ def _push_recording_frames(recorder, control_ns, frame_number):
|
|||||||
recorder._wrist_camera.buffer.push(
|
recorder._wrist_camera.buffer.push(
|
||||||
CameraFrame(
|
CameraFrame(
|
||||||
_image(3),
|
_image(3),
|
||||||
frame_number,
|
frame_number + 1000,
|
||||||
float(frame_number),
|
float(frame_number + 1000),
|
||||||
control_ns - 3_000_000,
|
control_ns - 3_000_000,
|
||||||
)
|
)
|
||||||
)
|
)
|
||||||
@@ -776,6 +888,14 @@ def test_end_to_end_fake_episode_saves_and_returns_idle(tmp_path):
|
|||||||
assert recorder.state is RecordingState.IDLE
|
assert recorder.state is RecordingState.IDLE
|
||||||
assert "SAVING" in recorder._status_pub.messages
|
assert "SAVING" in recorder._status_pub.messages
|
||||||
assert recorder._status_pub.messages[-2:] == ["SAVED", "IDLE"]
|
assert recorder._status_pub.messages[-2:] == ["SAVED", "IDLE"]
|
||||||
|
assert any(
|
||||||
|
"ACT录制状态：RECORDING，episode_0" in message
|
||||||
|
for message in recorder._logger.infos
|
||||||
|
)
|
||||||
|
assert any(
|
||||||
|
"episode_0.hdf5（3 samples）" in message
|
||||||
|
for message in recorder._logger.infos
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
@requires_h5py
|
@requires_h5py
|
||||||
@@ -791,6 +911,11 @@ def test_final_publish_rejects_allocated_episode_number_conflict(tmp_path):
|
|||||||
assert not (recorder._task_dir / "episode_1.hdf5").exists()
|
assert not (recorder._task_dir / "episode_1.hdf5").exists()
|
||||||
rejected = list((recorder._task_dir / "rejected").glob("*.hdf5"))
|
rejected = list((recorder._task_dir / "rejected").glob("*.hdf5"))
|
||||||
assert len(rejected) == 1
|
assert len(rejected) == 1
|
||||||
|
assert any(
|
||||||
|
str(rejected[0]) in message
|
||||||
|
and "原因：episode_number_conflict" in message
|
||||||
|
for message in recorder._logger.warnings
|
||||||
|
)
|
||||||
with h5py.File(rejected[0], "r") as root:
|
with h5py.File(rejected[0], "r") as root:
|
||||||
assert root.attrs["reject_reason"] == "episode_number_conflict"
|
assert root.attrs["reject_reason"] == "episode_number_conflict"
|
||||||
|
|
||||||
|
|||||||
@@ -405,6 +405,32 @@ def _quality_failure(
|
|||||||
return QualityReport(False, reason, metrics)
|
return QualityReport(False, reason, metrics)
|
||||||
|
|
||||||
|
|
||||||
|
def sample_frame_metrics(
|
||||||
|
frame_numbers: np.ndarray,
|
||||||
|
) -> tuple[float, float, int]:
|
||||||
|
diffs = np.diff(np.asarray(frame_numbers, dtype=np.int64))
|
||||||
|
denominator = max(1, len(diffs))
|
||||||
|
return (
|
||||||
|
float(np.count_nonzero(diffs == 0) / denominator),
|
||||||
|
float(np.count_nonzero(diffs > 1) / denominator),
|
||||||
|
int(np.count_nonzero(diffs < 0)),
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def episode_sample_frame_metrics(root: Any) -> dict[str, int | float]:
|
||||||
|
metrics: dict[str, int | float] = {}
|
||||||
|
for camera in ("cam_high", "cam_wrist"):
|
||||||
|
repeat, skip, regression = sample_frame_metrics(
|
||||||
|
root[f"debug/cameras/{camera}_frame_number"][:]
|
||||||
|
)
|
||||||
|
metrics[f"{camera}_sample_repeat_ratio"] = repeat
|
||||||
|
metrics[f"{camera}_sample_skip_ratio"] = skip
|
||||||
|
metrics[f"{camera}_sample_frame_number_regression_count"] = (
|
||||||
|
regression
|
||||||
|
)
|
||||||
|
return metrics
|
||||||
|
|
||||||
|
|
||||||
def validate_episode(path: Path, limits: QualityLimits) -> QualityReport:
|
def validate_episode(path: Path, limits: QualityLimits) -> QualityReport:
|
||||||
metrics: dict[str, int | float] = {}
|
metrics: dict[str, int | float] = {}
|
||||||
try:
|
try:
|
||||||
@@ -541,6 +567,20 @@ def validate_episode(path: Path, limits: QualityLimits) -> QualityReport:
|
|||||||
if reason == "camera_drop_ratio" and value > threshold:
|
if reason == "camera_drop_ratio" and value > threshold:
|
||||||
return _quality_failure(reason, metrics)
|
return _quality_failure(reason, metrics)
|
||||||
|
|
||||||
|
for name in (
|
||||||
|
"camera_high_frame_number_regression_count",
|
||||||
|
"camera_right_wrist_frame_number_regression_count",
|
||||||
|
):
|
||||||
|
if name not in root.attrs:
|
||||||
|
return _quality_failure("camera_stats_missing", metrics)
|
||||||
|
value = int(root.attrs[name])
|
||||||
|
metrics[name] = value
|
||||||
|
if value:
|
||||||
|
return _quality_failure(
|
||||||
|
"camera_frame_number_regression",
|
||||||
|
metrics,
|
||||||
|
)
|
||||||
|
|
||||||
high_age_ms = datasets[
|
high_age_ms = datasets[
|
||||||
"debug/timestamps/cam_high_age_ms"
|
"debug/timestamps/cam_high_age_ms"
|
||||||
][:]
|
][:]
|
||||||
@@ -564,15 +604,15 @@ def validate_episode(path: Path, limits: QualityLimits) -> QualityReport:
|
|||||||
):
|
):
|
||||||
return _quality_failure("camera_skew", metrics)
|
return _quality_failure("camera_skew", metrics)
|
||||||
|
|
||||||
for camera in ("cam_high", "cam_wrist"):
|
sample_metrics = episode_sample_frame_metrics(root)
|
||||||
frame_numbers = datasets[
|
metrics.update(sample_metrics)
|
||||||
f"debug/cameras/{camera}_frame_number"
|
if any(
|
||||||
][:].astype(np.int64)
|
sample_metrics[
|
||||||
discontinuities = int((np.diff(frame_numbers) != 1).sum())
|
f"{camera}_sample_frame_number_regression_count"
|
||||||
ratio = discontinuities / max(1, sample_count - 1)
|
]
|
||||||
metrics[f"{camera}_sample_discontinuity_ratio"] = float(ratio)
|
for camera in ("cam_high", "cam_wrist")
|
||||||
if ratio > limits.max_drop_ratio:
|
):
|
||||||
return _quality_failure("camera_sample_drop_ratio", metrics)
|
return _quality_failure("camera_frame_number_regression", metrics)
|
||||||
|
|
||||||
if qpos[-1, 7] != 1.0:
|
if qpos[-1, 7] != 1.0:
|
||||||
return _quality_failure("final_gripper_not_open", metrics)
|
return _quality_failure("final_gripper_not_open", metrics)
|
||||||
@@ -595,8 +635,10 @@ class CameraFrame:
|
|||||||
class CameraStats:
|
class CameraStats:
|
||||||
frame_count: int
|
frame_count: int
|
||||||
dropped_frames: int
|
dropped_frames: int
|
||||||
|
frame_number_regression_count: int
|
||||||
first_host_monotonic_ns: int | None
|
first_host_monotonic_ns: int | None
|
||||||
last_host_monotonic_ns: int | None
|
last_host_monotonic_ns: int | None
|
||||||
|
recent_host_monotonic_ns: tuple[int, ...]
|
||||||
|
|
||||||
@property
|
@property
|
||||||
def drop_ratio(self) -> float:
|
def drop_ratio(self) -> float:
|
||||||
@@ -614,6 +656,20 @@ class CameraStats:
|
|||||||
return 0.0
|
return 0.0
|
||||||
return (self.frame_count - 1) * 1e9 / elapsed_ns
|
return (self.frame_count - 1) * 1e9 / elapsed_ns
|
||||||
|
|
||||||
|
@property
|
||||||
|
def rolling_fps(self) -> float:
|
||||||
|
if len(self.recent_host_monotonic_ns) < 2:
|
||||||
|
return 0.0
|
||||||
|
elapsed_ns = (
|
||||||
|
self.recent_host_monotonic_ns[-1]
|
||||||
|
- self.recent_host_monotonic_ns[0]
|
||||||
|
)
|
||||||
|
if elapsed_ns <= 0:
|
||||||
|
return 0.0
|
||||||
|
return (
|
||||||
|
(len(self.recent_host_monotonic_ns) - 1) * 1e9 / elapsed_ns
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
class CameraBuffer:
|
class CameraBuffer:
|
||||||
def __init__(self, *, maxlen: int = 4) -> None:
|
def __init__(self, *, maxlen: int = 4) -> None:
|
||||||
@@ -623,24 +679,33 @@ class CameraBuffer:
|
|||||||
self._lock = threading.Lock()
|
self._lock = threading.Lock()
|
||||||
self._frame_count = 0
|
self._frame_count = 0
|
||||||
self._dropped_frames = 0
|
self._dropped_frames = 0
|
||||||
|
self._frame_number_regression_count = 0
|
||||||
self._first_host_monotonic_ns: int | None = None
|
self._first_host_monotonic_ns: int | None = None
|
||||||
self._last_host_monotonic_ns: int | None = None
|
self._last_host_monotonic_ns: int | None = None
|
||||||
self._last_frame_number: int | None = None
|
self._last_frame_number: int | None = None
|
||||||
|
self._recent_host_monotonic_ns: deque[int] = deque()
|
||||||
|
|
||||||
def push(self, frame: CameraFrame) -> None:
|
def push(self, frame: CameraFrame) -> None:
|
||||||
with self._lock:
|
with self._lock:
|
||||||
if (
|
if self._last_frame_number is not None:
|
||||||
self._last_frame_number is not None
|
if frame.frame_number <= self._last_frame_number:
|
||||||
and frame.frame_number > self._last_frame_number + 1
|
self._frame_number_regression_count += 1
|
||||||
):
|
elif frame.frame_number > self._last_frame_number + 1:
|
||||||
self._dropped_frames += (
|
self._dropped_frames += (
|
||||||
frame.frame_number - self._last_frame_number - 1
|
frame.frame_number - self._last_frame_number - 1
|
||||||
)
|
)
|
||||||
self._last_frame_number = frame.frame_number
|
self._last_frame_number = frame.frame_number
|
||||||
self._frame_count += 1
|
self._frame_count += 1
|
||||||
if self._first_host_monotonic_ns is None:
|
if self._first_host_monotonic_ns is None:
|
||||||
self._first_host_monotonic_ns = frame.host_monotonic_ns
|
self._first_host_monotonic_ns = frame.host_monotonic_ns
|
||||||
self._last_host_monotonic_ns = frame.host_monotonic_ns
|
self._last_host_monotonic_ns = frame.host_monotonic_ns
|
||||||
|
self._recent_host_monotonic_ns.append(frame.host_monotonic_ns)
|
||||||
|
cutoff_ns = frame.host_monotonic_ns - 2_000_000_000
|
||||||
|
while (
|
||||||
|
self._recent_host_monotonic_ns
|
||||||
|
and self._recent_host_monotonic_ns[0] < cutoff_ns
|
||||||
|
):
|
||||||
|
self._recent_host_monotonic_ns.popleft()
|
||||||
self._frames.append(frame)
|
self._frames.append(frame)
|
||||||
|
|
||||||
def snapshot(self) -> tuple[CameraFrame, ...]:
|
def snapshot(self) -> tuple[CameraFrame, ...]:
|
||||||
@@ -652,8 +717,14 @@ class CameraBuffer:
|
|||||||
return CameraStats(
|
return CameraStats(
|
||||||
frame_count=self._frame_count,
|
frame_count=self._frame_count,
|
||||||
dropped_frames=self._dropped_frames,
|
dropped_frames=self._dropped_frames,
|
||||||
|
frame_number_regression_count=(
|
||||||
|
self._frame_number_regression_count
|
||||||
|
),
|
||||||
first_host_monotonic_ns=self._first_host_monotonic_ns,
|
first_host_monotonic_ns=self._first_host_monotonic_ns,
|
||||||
last_host_monotonic_ns=self._last_host_monotonic_ns,
|
last_host_monotonic_ns=self._last_host_monotonic_ns,
|
||||||
|
recent_host_monotonic_ns=tuple(
|
||||||
|
self._recent_host_monotonic_ns
|
||||||
|
),
|
||||||
)
|
)
|
||||||
|
|
||||||
|
|
||||||
@@ -1004,6 +1075,8 @@ def _twist_values(twist: Any) -> np.ndarray:
|
|||||||
|
|
||||||
|
|
||||||
class ActEpisodeRecorder(Node):
|
class ActEpisodeRecorder(Node):
|
||||||
|
PREVIEW_WINDOW = "ACT - D455 Global / D405 Right Wrist"
|
||||||
|
|
||||||
def __init__(self) -> None:
|
def __init__(self) -> None:
|
||||||
super().__init__("act_episode_recorder")
|
super().__init__("act_episode_recorder")
|
||||||
defaults = {
|
defaults = {
|
||||||
@@ -1102,6 +1175,8 @@ class ActEpisodeRecorder(Node):
|
|||||||
self._episode_index: int | None = None
|
self._episode_index: int | None = None
|
||||||
self._camera_baselines: tuple[CameraStats, CameraStats] | None = None
|
self._camera_baselines: tuple[CameraStats, CameraStats] | None = None
|
||||||
self._saving_deadline_ns: int | None = None
|
self._saving_deadline_ns: int | None = None
|
||||||
|
self._preview_stop = threading.Event()
|
||||||
|
self._preview_thread: threading.Thread | None = None
|
||||||
|
|
||||||
self._high_camera = RealSenseCamera(
|
self._high_camera = RealSenseCamera(
|
||||||
str(parameters["cam_high_serial"]),
|
str(parameters["cam_high_serial"]),
|
||||||
@@ -1118,6 +1193,8 @@ class ActEpisodeRecorder(Node):
|
|||||||
except Exception as exc:
|
except Exception as exc:
|
||||||
self._camera_start_error = str(exc)
|
self._camera_start_error = str(exc)
|
||||||
self.get_logger().error(f"ACT相机启动失败：{exc}")
|
self.get_logger().error(f"ACT相机启动失败：{exc}")
|
||||||
|
if self._camera_start_error is None:
|
||||||
|
self._start_preview()
|
||||||
|
|
||||||
self._status_pub = self.create_publisher(
|
self._status_pub = self.create_publisher(
|
||||||
String,
|
String,
|
||||||
@@ -1142,6 +1219,13 @@ class ActEpisodeRecorder(Node):
|
|||||||
self._on_left_controller,
|
self._on_left_controller,
|
||||||
10,
|
10,
|
||||||
)
|
)
|
||||||
|
self.get_logger().info(
|
||||||
|
f"ACT录制器已启动，输出目录：{self._task_dir}"
|
||||||
|
)
|
||||||
|
self.get_logger().info(
|
||||||
|
"操作提示：右手B准备/结束录制；准备后握住右手Grip开始采样；"
|
||||||
|
"左手Y长按1秒丢弃；录制中不要按右手A"
|
||||||
|
)
|
||||||
self._publish_state(RecordingState.IDLE)
|
self._publish_state(RecordingState.IDLE)
|
||||||
|
|
||||||
@staticmethod
|
@staticmethod
|
||||||
@@ -1174,6 +1258,158 @@ class ActEpisodeRecorder(Node):
|
|||||||
def state(self) -> RecordingState:
|
def state(self) -> RecordingState:
|
||||||
return self._session.state
|
return self._session.state
|
||||||
|
|
||||||
|
@staticmethod
|
||||||
|
def _preview_tile(
|
||||||
|
role: str,
|
||||||
|
frame: CameraFrame | None,
|
||||||
|
stats: CameraStats,
|
||||||
|
cv2: Any,
|
||||||
|
) -> np.ndarray:
|
||||||
|
if frame is None:
|
||||||
|
tile = np.zeros((480, 640, 3), dtype=np.uint8)
|
||||||
|
frame_number = "-"
|
||||||
|
else:
|
||||||
|
tile = cv2.cvtColor(frame.image, cv2.COLOR_RGB2BGR)
|
||||||
|
frame_number = str(frame.frame_number)
|
||||||
|
tile = tile.copy()
|
||||||
|
cv2.rectangle(tile, (0, 0), (640, 74), (0, 0, 0), -1)
|
||||||
|
lines = (
|
||||||
|
f"{role} FPS {stats.rolling_fps:.1f} Frame {frame_number}",
|
||||||
|
f"Received {stats.frame_count} Dropped "
|
||||||
|
f"{stats.dropped_frames} ({stats.drop_ratio:.2%})",
|
||||||
|
)
|
||||||
|
for index, line in enumerate(lines):
|
||||||
|
cv2.putText(
|
||||||
|
tile,
|
||||||
|
line,
|
||||||
|
(10, 28 + index * 30),
|
||||||
|
cv2.FONT_HERSHEY_SIMPLEX,
|
||||||
|
0.65,
|
||||||
|
(255, 255, 255),
|
||||||
|
1,
|
||||||
|
cv2.LINE_AA,
|
||||||
|
)
|
||||||
|
return tile
|
||||||
|
|
||||||
|
def _compose_preview(self, cv2: Any) -> np.ndarray:
|
||||||
|
high_frames = self._high_camera.buffer.snapshot()
|
||||||
|
wrist_frames = self._wrist_camera.buffer.snapshot()
|
||||||
|
high = high_frames[-1] if high_frames else None
|
||||||
|
wrist = wrist_frames[-1] if wrist_frames else None
|
||||||
|
image = np.hstack(
|
||||||
|
(
|
||||||
|
self._preview_tile(
|
||||||
|
"GLOBAL D455",
|
||||||
|
high,
|
||||||
|
self._high_camera.buffer.stats(),
|
||||||
|
cv2,
|
||||||
|
),
|
||||||
|
self._preview_tile(
|
||||||
|
"RIGHT WRIST D405",
|
||||||
|
wrist,
|
||||||
|
self._wrist_camera.buffer.stats(),
|
||||||
|
cv2,
|
||||||
|
),
|
||||||
|
)
|
||||||
|
)
|
||||||
|
|
||||||
|
now_ns = self._now_ns()
|
||||||
|
high_age = (
|
||||||
|
f"{(now_ns - high.host_monotonic_ns) * 1e-6:.1f} ms"
|
||||||
|
if high is not None
|
||||||
|
else "-"
|
||||||
|
)
|
||||||
|
wrist_age = (
|
||||||
|
f"{(now_ns - wrist.host_monotonic_ns) * 1e-6:.1f} ms"
|
||||||
|
if wrist is not None
|
||||||
|
else "-"
|
||||||
|
)
|
||||||
|
skew = (
|
||||||
|
f"{abs(high.host_monotonic_ns - wrist.host_monotonic_ns) * 1e-6:.1f} ms"
|
||||||
|
if high is not None and wrist is not None
|
||||||
|
else "-"
|
||||||
|
)
|
||||||
|
episode_index = self._episode_index
|
||||||
|
episode = (
|
||||||
|
f"episode_{episode_index}"
|
||||||
|
if episode_index is not None
|
||||||
|
else "-"
|
||||||
|
)
|
||||||
|
store = self._store
|
||||||
|
samples = store.count if store is not None else 0
|
||||||
|
footer = np.zeros((80, image.shape[1], 3), dtype=np.uint8)
|
||||||
|
lines = (
|
||||||
|
f"Age high={high_age} wrist={wrist_age} Camera skew={skew}",
|
||||||
|
f"ACT {self.state.value} {episode} Samples {samples}",
|
||||||
|
)
|
||||||
|
for index, line in enumerate(lines):
|
||||||
|
cv2.putText(
|
||||||
|
footer,
|
||||||
|
line,
|
||||||
|
(10, 30 + index * 32),
|
||||||
|
cv2.FONT_HERSHEY_SIMPLEX,
|
||||||
|
0.7,
|
||||||
|
(255, 255, 255),
|
||||||
|
1,
|
||||||
|
cv2.LINE_AA,
|
||||||
|
)
|
||||||
|
return np.vstack((image, footer))
|
||||||
|
|
||||||
|
def _start_preview(self) -> None:
|
||||||
|
if not (
|
||||||
|
os.environ.get("DISPLAY") or os.environ.get("WAYLAND_DISPLAY")
|
||||||
|
):
|
||||||
|
self.get_logger().warn(
|
||||||
|
"未检测到桌面显示环境，ACT双相机预览已停用。"
|
||||||
|
)
|
||||||
|
return
|
||||||
|
try:
|
||||||
|
import cv2
|
||||||
|
except ImportError as exc:
|
||||||
|
self.get_logger().warn(
|
||||||
|
f"OpenCV不可用，ACT双相机预览已停用：{exc}"
|
||||||
|
)
|
||||||
|
return
|
||||||
|
self._preview_stop.clear()
|
||||||
|
self._preview_thread = threading.Thread(
|
||||||
|
target=self._preview_loop,
|
||||||
|
args=(cv2,),
|
||||||
|
name="act_camera_preview",
|
||||||
|
daemon=True,
|
||||||
|
)
|
||||||
|
self._preview_thread.start()
|
||||||
|
|
||||||
|
def _preview_loop(self, cv2: Any) -> None:
|
||||||
|
try:
|
||||||
|
cv2.namedWindow(self.PREVIEW_WINDOW, cv2.WINDOW_NORMAL)
|
||||||
|
while not self._preview_stop.is_set():
|
||||||
|
cv2.imshow(
|
||||||
|
self.PREVIEW_WINDOW,
|
||||||
|
self._compose_preview(cv2),
|
||||||
|
)
|
||||||
|
key = cv2.waitKey(1) & 0xFF
|
||||||
|
if key in (ord("q"), ord("Q"), 27):
|
||||||
|
break
|
||||||
|
if cv2.getWindowProperty(
|
||||||
|
self.PREVIEW_WINDOW,
|
||||||
|
cv2.WND_PROP_VISIBLE,
|
||||||
|
) < 1:
|
||||||
|
break
|
||||||
|
self._preview_stop.wait(0.1)
|
||||||
|
except Exception as exc:
|
||||||
|
self.get_logger().warn(f"ACT双相机预览已停用：{exc}")
|
||||||
|
finally:
|
||||||
|
try:
|
||||||
|
cv2.destroyWindow(self.PREVIEW_WINDOW)
|
||||||
|
except Exception:
|
||||||
|
pass
|
||||||
|
|
||||||
|
def _stop_preview(self) -> None:
|
||||||
|
self._preview_stop.set()
|
||||||
|
if self._preview_thread is not None:
|
||||||
|
self._preview_thread.join(timeout=2.0)
|
||||||
|
self._preview_thread = None
|
||||||
|
|
||||||
def _publish_state(
|
def _publish_state(
|
||||||
self,
|
self,
|
||||||
state: RecordingState,
|
state: RecordingState,
|
||||||
@@ -1182,6 +1418,12 @@ class ActEpisodeRecorder(Node):
|
|||||||
message = String()
|
message = String()
|
||||||
message.data = state.value if not reason else f"{state.value}:{reason}"
|
message.data = state.value if not reason else f"{state.value}:{reason}"
|
||||||
self._status_pub.publish(message)
|
self._status_pub.publish(message)
|
||||||
|
episode = (
|
||||||
|
f"，episode_{self._episode_index}"
|
||||||
|
if self._episode_index is not None
|
||||||
|
else ""
|
||||||
|
)
|
||||||
|
self.get_logger().info(f"ACT录制状态：{message.data}{episode}")
|
||||||
|
|
||||||
def _run_preflight(self) -> str | None:
|
def _run_preflight(self) -> str | None:
|
||||||
now_ns = self._now_ns()
|
now_ns = self._now_ns()
|
||||||
@@ -1460,14 +1702,19 @@ class ActEpisodeRecorder(Node):
|
|||||||
def _interval_camera_metrics(
|
def _interval_camera_metrics(
|
||||||
baseline: CameraStats,
|
baseline: CameraStats,
|
||||||
current: CameraStats,
|
current: CameraStats,
|
||||||
) -> tuple[float, float]:
|
) -> tuple[float, float, int]:
|
||||||
frames = max(0, current.frame_count - baseline.frame_count)
|
frames = max(0, current.frame_count - baseline.frame_count)
|
||||||
dropped = max(0, current.dropped_frames - baseline.dropped_frames)
|
dropped = max(0, current.dropped_frames - baseline.dropped_frames)
|
||||||
|
regressions = max(
|
||||||
|
0,
|
||||||
|
current.frame_number_regression_count
|
||||||
|
- baseline.frame_number_regression_count,
|
||||||
|
)
|
||||||
if (
|
if (
|
||||||
baseline.last_host_monotonic_ns is None
|
baseline.last_host_monotonic_ns is None
|
||||||
or current.last_host_monotonic_ns is None
|
or current.last_host_monotonic_ns is None
|
||||||
):
|
):
|
||||||
return 0.0, 1.0
|
return 0.0, 1.0, regressions
|
||||||
elapsed_ns = (
|
elapsed_ns = (
|
||||||
current.last_host_monotonic_ns
|
current.last_host_monotonic_ns
|
||||||
- baseline.last_host_monotonic_ns
|
- baseline.last_host_monotonic_ns
|
||||||
@@ -1475,7 +1722,7 @@ class ActEpisodeRecorder(Node):
|
|||||||
fps = frames * 1e9 / elapsed_ns if elapsed_ns > 0 else 0.0
|
fps = frames * 1e9 / elapsed_ns if elapsed_ns > 0 else 0.0
|
||||||
expected = frames + dropped
|
expected = frames + dropped
|
||||||
drop_ratio = dropped / expected if expected else 1.0
|
drop_ratio = dropped / expected if expected else 1.0
|
||||||
return fps, drop_ratio
|
return fps, drop_ratio, regressions
|
||||||
|
|
||||||
def _write_camera_metrics(self) -> None:
|
def _write_camera_metrics(self) -> None:
|
||||||
assert self._store is not None
|
assert self._store is not None
|
||||||
@@ -1492,8 +1739,12 @@ class ActEpisodeRecorder(Node):
|
|||||||
{
|
{
|
||||||
"camera_high_fps": high[0],
|
"camera_high_fps": high[0],
|
||||||
"camera_high_drop_ratio": high[1],
|
"camera_high_drop_ratio": high[1],
|
||||||
|
"camera_high_frame_number_regression_count": high[2],
|
||||||
"camera_right_wrist_fps": wrist[0],
|
"camera_right_wrist_fps": wrist[0],
|
||||||
"camera_right_wrist_drop_ratio": wrist[1],
|
"camera_right_wrist_drop_ratio": wrist[1],
|
||||||
|
"camera_right_wrist_frame_number_regression_count": (
|
||||||
|
wrist[2]
|
||||||
|
),
|
||||||
}
|
}
|
||||||
)
|
)
|
||||||
|
|
||||||
@@ -1557,6 +1808,17 @@ class ActEpisodeRecorder(Node):
|
|||||||
except FileExistsError:
|
except FileExistsError:
|
||||||
self._reject_closed_partial("episode_number_conflict")
|
self._reject_closed_partial("episode_number_conflict")
|
||||||
return
|
return
|
||||||
|
self.get_logger().info(
|
||||||
|
f"ACT数据已保存：{destination}"
|
||||||
|
f"（{report.metrics['sample_count']} samples）"
|
||||||
|
)
|
||||||
|
self.get_logger().info(
|
||||||
|
"ACT相机采样相位："
|
||||||
|
f"high重复={report.metrics['cam_high_sample_repeat_ratio']:.2%}, "
|
||||||
|
f"high跨帧={report.metrics['cam_high_sample_skip_ratio']:.2%}, "
|
||||||
|
f"wrist重复={report.metrics['cam_wrist_sample_repeat_ratio']:.2%}, "
|
||||||
|
f"wrist跨帧={report.metrics['cam_wrist_sample_skip_ratio']:.2%}"
|
||||||
|
)
|
||||||
self._finish_result(RecordingState.SAVED)
|
self._finish_result(RecordingState.SAVED)
|
||||||
|
|
||||||
def _reject_closed_partial(
|
def _reject_closed_partial(
|
||||||
@@ -1570,6 +1832,9 @@ class ActEpisodeRecorder(Node):
|
|||||||
root.attrs["episode_status"] = "rejected"
|
root.attrs["episode_status"] = "rejected"
|
||||||
root.attrs["reject_reason"] = reason
|
root.attrs["reject_reason"] = reason
|
||||||
root.attrs["interrupted"] = np.bool_(interrupted)
|
root.attrs["interrupted"] = np.bool_(interrupted)
|
||||||
|
for name, value in episode_sample_frame_metrics(root).items():
|
||||||
|
root.attrs[name] = value
|
||||||
|
sample_count = int(root["action"].shape[0])
|
||||||
rejected = self._task_dir / "rejected"
|
rejected = self._task_dir / "rejected"
|
||||||
rejected.mkdir(exist_ok=True)
|
rejected.mkdir(exist_ok=True)
|
||||||
safe_reason = re.sub(r"[^a-zA-Z0-9_-]", "_", reason)
|
safe_reason = re.sub(r"[^a-zA-Z0-9_-]", "_", reason)
|
||||||
@@ -1581,6 +1846,10 @@ class ActEpisodeRecorder(Node):
|
|||||||
/ f"episode_{episode_index}_{safe_reason}_{timestamp}.hdf5"
|
/ f"episode_{episode_index}_{safe_reason}_{timestamp}.hdf5"
|
||||||
)
|
)
|
||||||
publish_without_overwrite(self._partial_path, destination)
|
publish_without_overwrite(self._partial_path, destination)
|
||||||
|
self.get_logger().warn(
|
||||||
|
f"ACT数据已拒绝：{destination}（{sample_count} samples，"
|
||||||
|
f"原因：{reason}）"
|
||||||
|
)
|
||||||
self._finish_result(RecordingState.REJECTED, reason)
|
self._finish_result(RecordingState.REJECTED, reason)
|
||||||
|
|
||||||
def _reject_current(
|
def _reject_current(
|
||||||
@@ -1609,11 +1878,16 @@ class ActEpisodeRecorder(Node):
|
|||||||
def _discard_current(self) -> None:
|
def _discard_current(self) -> None:
|
||||||
if self._partial_path is None:
|
if self._partial_path is None:
|
||||||
return
|
return
|
||||||
|
partial = self._partial_path
|
||||||
if self._writer is not None:
|
if self._writer is not None:
|
||||||
self._writer.finish()
|
self._writer.finish()
|
||||||
|
sample_count = self._store.count if self._store is not None else 0
|
||||||
if self._store is not None:
|
if self._store is not None:
|
||||||
self._store.close()
|
self._store.close()
|
||||||
discard_partial(self._partial_path)
|
discard_partial(partial)
|
||||||
|
self.get_logger().info(
|
||||||
|
f"ACT数据已丢弃：{partial}（{sample_count} samples）"
|
||||||
|
)
|
||||||
self._finish_result(RecordingState.DISCARDED)
|
self._finish_result(RecordingState.DISCARDED)
|
||||||
|
|
||||||
def _finish_result(
|
def _finish_result(
|
||||||
@@ -1639,6 +1913,7 @@ class ActEpisodeRecorder(Node):
|
|||||||
self._reject_current(reason, interrupted=True)
|
self._reject_current(reason, interrupted=True)
|
||||||
|
|
||||||
def close(self) -> None:
|
def close(self) -> None:
|
||||||
|
self._stop_preview()
|
||||||
self._high_camera.stop()
|
self._high_camera.stop()
|
||||||
self._wrist_camera.stop()
|
self._wrist_camera.stop()
|
||||||
self._directory_lock.release()
|
self._directory_lock.release()
|
||||||
|
|||||||
@@ -218,7 +218,7 @@ def peripheral_cfg(
|
|||||||
|
|
||||||
addr = 1
|
addr = 1
|
||||||
# 依次设置目标速度、目标力矩、目标加速度和目标减速度。
|
# 依次设置目标速度、目标力矩、目标加速度和目标减速度。
|
||||||
reg_value = [255, 60, 255, 255]
|
reg_value = [255, 150, 255, 255]
|
||||||
for i, reg_addr in enumerate([11, 12, 13, 14]):
|
for i, reg_addr in enumerate([11, 12, 13, 14]):
|
||||||
write_params = rm_peripheral_read_write_params_t(1, reg_addr, addr, 1)
|
write_params = rm_peripheral_read_write_params_t(1, reg_addr, addr, 1)
|
||||||
robot.rm_write_single_register(write_params, reg_value[i])
|
robot.rm_write_single_register(write_params, reg_value[i])
|
||||||
|
|||||||
Reference in New Issue
Block a user