# 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 核心结构、机器人控制和安全行为保持不变； - 相关测试与工作空间构建通过。