diff --git a/docs/superpowers/plans/2026-08-10-act-tomato-pick-data-collection.md b/docs/superpowers/plans/2026-08-10-act-tomato-pick-data-collection.md new file mode 100644 index 0000000..6a7402f --- /dev/null +++ b/docs/superpowers/plans/2026-08-10-act-tomato-pick-data-collection.md @@ -0,0 +1,1573 @@ +# 右臂番茄采摘 ACT 数据采集实施计划 + +> **供代理执行者使用：** 必须使用 `superpowers:subagent-driven-development`（推荐）或 `superpowers:executing-plans`，逐项执行本计划。所有步骤使用复选框（`- [ ]`）跟踪。 + +**目标：** 在现有 ROS2 + PICO + RM75 右臂遥操链路上增加 30 Hz、双 RGB 相机、8 维状态/动作的 ALOHA/ACT 风格 HDF5 episode 采集能力，同时保持控制和安全链路不变。 + +**架构：** `single_arm_velocity_teleop` 在每个 90 Hz 控制周期结束时发布一条小型原子控制消息；独立 `act_episode_recorder` 节点每 3 个周期取样一次，并匹配两台 RealSense 不晚于该控制周期的最新帧。采集节点通过有界队列流式写临时 HDF5，结束后裁剪、校验并以不覆盖已有文件的方式发布到正式目录或拒绝目录。 + +**技术栈：** Ubuntu 22.04、ROS2 Humble、Python 3、rclpy、rosidl、NumPy、pyrealsense2、h5py、pytest、HDF5。 + +--- + +## 文件结构 + +本次不新建 ROS 包。文件职责锁定如下： + +- 新建 `xr_rm_interfaces/msg/ActControlSample.msg`：定义单个控制周期的原子数据契约； +- 修改 `xr_rm_interfaces/CMakeLists.txt`：生成新消息； +- 修改 `xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py`：只读导出 7 关节位置限制； +- 修改 `xr_rm_teleop/xr_rm_teleop/fun_peripheral.py`：初始化工具时只发送一次完全打开； +- 修改 `xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py`：夹爪请求/完成状态与 90 Hz 原子消息； +- 新建 `xr_rm_teleop/xr_rm_teleop/act_episode_recorder.py`：状态机、相机缓冲、对齐、流式 HDF5、质量检查和恢复； +- 修改 `xr_rm_teleop/setup.py`：安装采集节点入口； +- 新建 `xr_rm_bringup/config/act_tomato_pick.yaml`：硬件序列号和采集质量参数； +- 修改 `xr_rm_bringup/config/peripherals_rm75.yaml`：仅为右臂启用初始化打开； +- 修改 `xr_rm_bringup/launch/arm_debug.launch.py`：增加默认关闭的 `record_act`； +- 修改 `xr_rm_teleop/test/test_initial_joint_pose.py`：夹爪初始化与关节限制测试； +- 新建 `xr_rm_teleop/test/test_act_control_sample.py`：原子消息构造测试； +- 新建 `xr_rm_teleop/test/test_act_episode_recorder.py`：状态机、对齐、HDF5、质量和恢复测试； +- 修改 `xr_rm_bringup/test/test_arm_debug_launch.py`：启动参数和非法组合测试。 + +`act_episode_recorder.py` 保持单文件，因为这些逻辑只服务一个节点；只提取可独立测试的小型数据类和纯函数，不创建通用采集框架、工厂或插件层。 + +## 统一执行约定 + +所有构建和测试命令必须从工作空间根目录执行： + +```bash +cd /home/robot/WS_xr +source /opt/ros/humble/setup.bash +``` + +不得在 `/home/robot/WS_xr/src` 运行 `colcon build`。自动化测试不得连接真机、移动 +RM75 或操作真实夹爪。 + +### 任务 1：新增原子控制消息和关节限制只读接口 + +**文件：** + +- 新建：`xr_rm_interfaces/msg/ActControlSample.msg` +- 修改：`xr_rm_interfaces/CMakeLists.txt` +- 修改：`xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py` +- 修改：`xr_rm_teleop/test/test_placo_transforms.py` + +- [ ] **步骤 1：先写关节限制只读副本测试** + +在 `test_placo_transforms.py` 使用该文件现有的求解器构造辅助方式，增加： + +```python +def test_joint_position_limits_returns_a_copy(solver) -> None: + first = solver.joint_position_limits + second = solver.joint_position_limits + + assert first.shape == (7, 2) + assert np.isfinite(first).all() + assert np.all(first[:, 0] < first[:, 1]) + first[0, 0] = 999.0 + assert second[0, 0] != 999.0 +``` + +- [ ] **步骤 2：运行测试并确认失败** + +运行： + +```bash +cd /home/robot/WS_xr +source /opt/ros/humble/setup.bash +pytest src/xr_rm_teleop/test/test_placo_transforms.py -k joint_position_limits -v +``` + +预期：失败，原因是 `PlacoIkSolver` 尚无 `joint_position_limits` 属性。 + +- [ ] **步骤 3：实现只读关节限制属性** + +在 `PlacoIkSolver` 的现有属性旁增加： + +```python +@property +def joint_position_limits(self) -> np.ndarray: + return self._joint_limits.copy() +``` + +- [ ] **步骤 4：定义原子消息** + +创建 `ActControlSample.msg`，内容固定为： + +```text +std_msgs/Header header +uint64 control_seq +int64 control_monotonic_ns +int64 feedback_monotonic_ns +int64 action_monotonic_ns + +float32 feedback_age_ms +float32 qp_duration_ms + +float64[7] q_actual +float64[7] q_qp_raw +float64[7] q_target +float64[7] joint_lower_limits +float64[7] joint_upper_limits + +geometry_msgs/Pose tcp_current +geometry_msgs/Pose tcp_raw_target +geometry_msgs/Pose tcp_target +geometry_msgs/Twist tcp_command_velocity +geometry_msgs/Pose pico_pose + +bool pico_grip +float32 pico_trigger +bool pico_primary +bool pico_secondary +float32[2] pico_axis + +bool gripper_target_open +bool gripper_state_open +bool gripper_state_known +bool gripper_command_pending +bool gripper_command_failed + +bool teleop_active +bool feedback_valid +bool action_valid +bool command_sent +bool qp_attempted +bool qp_success +bool target_clamped +bool control_fault +``` + +在 `rosidl_generate_interfaces` 中把新文件加入现有列表；`geometry_msgs` 和 +`std_msgs` 已经是声明过的依赖，不增加新接口依赖。 + +- [ ] **步骤 5：构建接口并检查生成结果** + +运行： + +```bash +cd /home/robot/WS_xr +source /opt/ros/humble/setup.bash +colcon build --symlink-install --packages-select xr_rm_interfaces +source install/setup.bash +ros2 interface show xr_rm_interfaces/msg/ActControlSample +pytest src/xr_rm_teleop/test/test_placo_transforms.py -k joint_position_limits -v +``` + +预期：接口构建成功，`ros2 interface show` 展示上述全部字段，新增测试通过。 + +- [ ] **步骤 6：提交任务 1** + +```bash +git add src/xr_rm_interfaces/msg/ActControlSample.msg \ + src/xr_rm_interfaces/CMakeLists.txt \ + src/xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py \ + src/xr_rm_teleop/test/test_placo_transforms.py +git commit -m "feat: 添加ACT原子控制消息" +``` + +### 任务 2：把右臂夹爪初始化和逻辑状态改成可观测结果 + +**文件：** + +- 修改：`xr_rm_teleop/xr_rm_teleop/fun_peripheral.py` +- 修改：`xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py` +- 修改：`xr_rm_bringup/config/peripherals_rm75.yaml` +- 修改：`xr_rm_teleop/test/test_initial_joint_pose.py` +- 修改：`xr_rm_teleop/test/test_joint_control.py` + +- [ ] **步骤 1：写右臂配置和完全打开测试** + +在 `test_initial_joint_pose.py` 增加对部署配置的断言： + +```python +def test_right_tool_initializes_open_only_for_right_arm() -> None: + config_file = CONFIG_DIR / "peripherals_rm75.yaml" + left = load_peripheral_config(str(config_file), "left") + right = load_peripheral_config(str(config_file), "right") + + assert not left.set_initial_tool_state + assert right.set_initial_tool_state +``` + +再为 `peripheral_cfg` 增加一个使用假 SDK 类型和 `monkeypatch` 的测试，截获 +`set_tool_position` 调用，核心断言为： + +```python +assert calls == [(1.0, 1, 1)] +``` + +其中三项依次表示 `percent`、`device`、`scissorgripper`，不能出现 `0.75` 或 +`0.15`。 + +- [ ] **步骤 2：运行测试并确认失败** + +```bash +cd /home/robot/WS_xr +source /opt/ros/humble/setup.bash +pytest src/xr_rm_teleop/test/test_initial_joint_pose.py \ + -k "right_tool_initializes_open or omnipic_initial_state" -v +``` + +预期：部署配置仍是 `false`，且旧代码发出 `0.75`、`0.15`，测试失败。 + +- [ ] **步骤 3：最小修改初始化行为** + +把 OmniPicker 的初始化分支替换成一次调用： + +```python +if set_initial_tool_state: + set_tool_position( + robot, + percent=1.0, + device=1, + scissorgripper=scissorgripper, + ) +``` + +在 `peripherals_rm75.yaml` 保持全局默认关闭，并只在右臂覆盖： + +```yaml +set_initial_tool_state: false + +arms: + left: + scissorgripper: 0 + right: + scissorgripper: 1 + set_initial_tool_state: true +``` + +保留仓库当前实际使用的左臂 `scissorgripper` 值，不借本任务修正无关配置。 + +- [ ] **步骤 4：写夹爪请求与成功状态测试** + +在 `test_joint_control.py` 构造无 ROS 初始化的遥操对象和假适配器，验证： + +```python +def test_tool_state_changes_only_after_command_succeeds() -> None: + teleop, worker_gate = _tool_state_teleop() + + teleop._enqueue_tool_command(False, "test") + assert teleop._tool_target_open is False + assert teleop._tool_state_open is True + assert teleop._tool_command_pending + + worker_gate.complete_successfully() + assert teleop._tool_state_open is False + assert not teleop._tool_command_pending + assert not teleop._tool_command_failed +``` + +另加失败测试：失败后 `_tool_state_open` 保持原值、`_tool_command_failed=true`， +下一次成功命令清除失败并更新状态。 + +- [ ] **步骤 5：运行夹爪状态测试并确认失败** + +```bash +cd /home/robot/WS_xr +source /opt/ros/humble/setup.bash +pytest src/xr_rm_teleop/test/test_joint_control.py -k tool_state -v +``` + +预期：失败，原因是当前遥操节点只有 `_trigger_tool_open`，没有请求、成功、处理中和 +失败状态。 + +- [ ] **步骤 6：在线程锁内记录夹爪状态** + +在遥操节点初始化时增加： + +```python +self._tool_state_lock = threading.Lock() +self._tool_target_open = True +self._tool_state_open: bool | None = None +self._tool_command_pending = False +self._tool_command_failed = False +``` + +`configure_peripheral` 成功且右臂配置要求初始化后，将请求和已确认状态都设为 +`True`。每次入队先设置目标与 pending；工作线程只有在 +`self._adapter.set_tool_enabled(open_tool)` 正常返回后才更新 +`_tool_state_open`。异常时保持状态不变并设置失败。所有跨线程读写都在 +`_tool_state_lock` 中完成。 + +提供一个只读快照方法，后续原子消息复用： + +```python +def _tool_state_snapshot(self) -> tuple[bool, bool | None, bool, bool]: + with self._tool_state_lock: + return ( + self._tool_target_open, + self._tool_state_open, + self._tool_command_pending, + self._tool_command_failed, + ) +``` + +- [ ] **步骤 7：运行相关测试** + +```bash +cd /home/robot/WS_xr +source /opt/ros/humble/setup.bash +pytest src/xr_rm_teleop/test/test_initial_joint_pose.py -v +pytest src/xr_rm_teleop/test/test_joint_control.py -k "tool or trigger" -v +``` + +预期：新增测试和现有工具/Trigger 测试全部通过。 + +- [ ] **步骤 8：提交任务 2** + +```bash +git add src/xr_rm_teleop/xr_rm_teleop/fun_peripheral.py \ + src/xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py \ + src/xr_rm_bringup/config/peripherals_rm75.yaml \ + src/xr_rm_teleop/test/test_initial_joint_pose.py \ + src/xr_rm_teleop/test/test_joint_control.py +git commit -m "feat: 记录夹爪逻辑执行状态" +``` + +### 任务 3：在 90 Hz 控制周期发布原子样本 + +**文件：** + +- 修改：`xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py` +- 新建：`xr_rm_teleop/test/test_act_control_sample.py` +- 修改：`xr_rm_teleop/test/test_joint_control.py` +- 修改：`xr_rm_teleop/test/test_orientation_control.py` + +- [ ] **步骤 1：写原子样本构造测试** + +新测试文件使用 `object.__new__(SingleArmVelocityTeleop)`、`FakePublisher` 和假 +`ActCycleContext`，覆盖三个关键行为： + +```python +def test_act_sample_uses_feedback_and_limited_target_from_one_cycle() -> None: + teleop = _act_sample_teleop() + cycle = _cycle( + seq=100, + q_actual=[0.1] * 7, + q_qp_raw=[0.3] * 7, + q_target=[0.2] * 7, + command_sent=True, + qp_attempted=True, + qp_success=True, + ) + + message = teleop._build_act_control_sample(cycle) + + assert message.control_seq == 100 + assert message.q_actual == pytest.approx([0.1] * 7) + assert message.q_qp_raw == pytest.approx([0.3] * 7) + assert message.q_target == pytest.approx([0.2] * 7) + assert message.command_sent + assert message.action_valid + + +def test_act_sample_marks_qp_fallback_as_valid_held_action() -> None: + teleop = _act_sample_teleop(last_successful_target=[0.2] * 7) + cycle = _cycle(qp_attempted=True, qp_success=False, command_sent=True) + + message = teleop._build_act_control_sample(cycle) + + assert message.q_target == pytest.approx([0.2] * 7) + assert message.action_valid + assert not message.qp_success + + +def test_act_sample_holds_last_action_while_grip_is_released() -> None: + teleop = _act_sample_teleop(last_successful_target=[0.4] * 7) + cycle = _cycle(teleop_active=False, command_sent=False) + + message = teleop._build_act_control_sample(cycle) + + assert message.q_target == pytest.approx([0.4] * 7) + assert message.action_valid + assert not message.command_sent +``` + +另测发送失败时 `action_valid=false`、未知夹爪状态时 +`gripper_state_known=false`，以及上下关节限制来自求解器副本。 + +- [ ] **步骤 2：运行测试并确认失败** + +```bash +cd /home/robot/WS_xr +source /opt/ros/humble/setup.bash +source install/setup.bash +pytest src/xr_rm_teleop/test/test_act_control_sample.py -v +``` + +预期：失败，原因是原子周期上下文和构造方法尚不存在。 + +- [ ] **步骤 3：增加仅供单周期使用的上下文数据类** + +在遥操模块内增加私有数据类，默认值确保任何提前返回路径也能发布完整消息： + +```python +@dataclass +class _ActCycleContext: + control_seq: int + control_monotonic_ns: int + feedback_monotonic_ns: int = -1 + action_monotonic_ns: int = -1 + feedback_age_ms: float = math.inf + q_actual: list[float] | None = None + q_qp_raw: list[float] | None = None + q_target: list[float] | None = None + current_pose: np.ndarray | None = None + raw_target_pose: np.ndarray | None = None + target_pose: np.ndarray | None = None + command_velocity: list[float] | None = None + feedback_valid: bool = False + command_sent: bool = False + send_failed: bool = False + qp_attempted: bool = False + qp_success: bool = False + target_clamped: bool = False + control_fault: bool = False +``` + +- [ ] **步骤 4：用包装方法保证所有返回路径发布一次** + +保持现有控制主体顺序不变，把当前 `_control_tick` 主体移入 +`_control_tick_impl(cycle)`，包装器只负责周期号和最终发布： + +```python +def _control_tick(self) -> None: + cycle = _ActCycleContext( + control_seq=self._act_control_seq, + control_monotonic_ns=time.monotonic_ns(), + ) + self._act_control_seq += 1 + try: + self._control_tick_impl(cycle) + finally: + self._publish_act_control_sample(cycle) +``` + +在原有主体已经取得信息的位置只赋值给 `cycle`：反馈同步后写 +`q_actual/current_pose`；QP 前后写目标、尝试、成功和耗时；最终限速并成功发送后写 +`q_target/action_monotonic_ns/command_sent`。不改变这些步骤的先后顺序和异常处理。 + +QP 方法改为返回目标和成功标志： + +```python +def _solve_joint_target( + self, + target_pose: np.ndarray, +) -> tuple[list[float], bool]: + try: + result = self._ik_solver.solve(target_pose) + except Exception as exc: + self.get_logger().warn( + f"{self._arm_name} QP 求解失败，保持上一组关节目标：{exc}", + throttle_duration_sec=1.0, + ) + return list(self._last_valid_joint_target), False + self._last_valid_joint_target = list(result) + return list(result), True +``` + +调用点同步解包；`test_joint_control.py` 中直接调用该方法的测试同步断言返回的 +`qp_success`。`test_joint_control.py` 和 `test_orientation_control.py` 中直接构造遥操 +对象并调用 `_control_tick()` 的辅助对象补齐 `_act_control_seq` 和假 publisher。 +现有 QP 失败保持行为不变。 + +- [ ] **步骤 5：构造并发布消息** + +创建 best-effort、keep-last 深度 10 的 publisher。构造方法必须： + +```python +q_actual = cycle.q_actual or [0.0] * 7 +held_target = ( + cycle.q_target + or self._last_successful_action_target + or q_actual +) +action_valid = ( + self._last_successful_action_target is not None + and cycle.feedback_valid + and not cycle.send_failed + and not cycle.control_fault +) +``` + +成功发送后先更新 `_last_successful_action_target`，该字段不能被 Grip 松开时的 +`_safe_stop(reset_active=True)` 清除。首次 Grip 建基准但尚未发送时保持 +`action_valid=false`。位姿缺失时使用当前 TCP 的有限值回退并保持相应有效标志， +不能把缺失样本写入正式 recording。 + +发布方法必须隔离采集诊断故障，不能让消息构造或 DDS 发布异常中断机器人控制： + +```python +def _publish_act_control_sample(self, cycle: _ActCycleContext) -> None: + try: + self._act_sample_pub.publish( + self._build_act_control_sample(cycle) + ) + except Exception as exc: + self.get_logger().warn( + f"{self._arm_name} ACT原子样本发布失败：{exc}", + throttle_duration_sec=1.0, + ) +``` + +增加一个假 publisher 抛异常的测试，断言 `_publish_act_control_sample` 不向控制 +回调传播异常。 + +- [ ] **步骤 6：运行原子样本和受影响控制测试** + +```bash +cd /home/robot/WS_xr +source /opt/ros/humble/setup.bash +source install/setup.bash +pytest src/xr_rm_teleop/test/test_act_control_sample.py -v +pytest src/xr_rm_teleop/test/test_joint_control.py -v +pytest src/xr_rm_teleop/test/test_orientation_control.py -v +``` + +预期：全部通过；现有安全停止、限速和姿态控制断言不变。 + +- [ ] **步骤 7：提交任务 3** + +```bash +git add src/xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py \ + src/xr_rm_teleop/test/test_act_control_sample.py \ + src/xr_rm_teleop/test/test_joint_control.py \ + src/xr_rm_teleop/test/test_orientation_control.py +git commit -m "feat: 发布同周期ACT控制样本" +``` + +### 任务 4：实现录制状态机、按键判定和 90→30 Hz 选择 + +**文件：** + +- 新建：`xr_rm_teleop/xr_rm_teleop/act_episode_recorder.py` +- 新建：`xr_rm_teleop/test/test_act_episode_recorder.py` + +- [ ] **步骤 1：写状态机测试** + +用纯 Python 对象覆盖：B 预检后进入 ARMED、第一次有效 Grip 进入 RECORDING、 +每 3 周期采样、暂停/恢复、结束请求、Y 长按、A 拒绝和 60 秒上限。核心测试： + +```python +def test_recording_starts_on_first_valid_grip_and_samples_every_third_cycle(): + session = RecordingSession(max_samples=1800) + session.arm() + + assert session.on_control( + 100, grip=False, action_valid=True, command_sent=False + ) == NO_ACTION + assert session.on_control( + 101, grip=True, action_valid=True, command_sent=False + ) == NO_ACTION + first = session.on_control( + 102, grip=True, action_valid=True, command_sent=True + ) + second = session.on_control( + 103, grip=True, action_valid=True, command_sent=True + ) + third = session.on_control( + 105, grip=True, action_valid=True, command_sent=True + ) + + assert first.record_sample + assert not second.record_sample + assert third.record_sample + assert session.sample_origin_seq == 102 + + +def test_final_grip_release_marks_crop_point_but_mid_pause_is_kept(): + session = _recording_session(origin_seq=10) + session.on_control(13, grip=False, action_valid=True, command_sent=False) + first_crop = session.candidate_end_count + session.on_control(14, grip=True, action_valid=True, command_sent=False) + assert session.candidate_end_count is None + session.on_control(16, grip=False, action_valid=True, command_sent=False) + assert session.candidate_end_count > first_crop + + +def test_missing_control_sequence_rejects_recording(): + session = _recording_session(origin_seq=10) + session.on_control(11, grip=True, action_valid=True, command_sent=True) + + decision = session.on_control( + 13, grip=True, action_valid=True, command_sent=True + ) + + assert decision.reject_reason == "control_sequence_gap" +``` + +按键追踪器测试必须证明：右 B 在 Grip 按下时忽略；左 Y 只有在 ARMED/RECORDING +期间发生新的按下并连续 1 秒才触发；IDLE 中按住 Y 后进入 ARMED 不会误丢弃； +RECORDING 中 A 返回 `initial_pose_command_during_episode`。 + +- [ ] **步骤 2：运行测试并确认失败** + +```bash +cd /home/robot/WS_xr +source /opt/ros/humble/setup.bash +pytest src/xr_rm_teleop/test/test_act_episode_recorder.py -k "session or button" -v +``` + +预期：导入失败，因为采集模块尚不存在。 + +- [ ] **步骤 3：实现最小状态类型和控制决策** + +在新模块中定义： + +```python +class RecordingState(str, Enum): + IDLE = "IDLE" + ARMED = "ARMED" + RECORDING = "RECORDING" + SAVING = "SAVING" + SAVED = "SAVED" + DISCARDED = "DISCARDED" + REJECTED = "REJECTED" + + +@dataclass(frozen=True) +class ControlDecision: + record_sample: bool = False + finish: bool = False + reject_reason: str | None = None + + +NO_ACTION = ControlDecision() +``` + +`RecordingSession` 只保存状态、起始序号、上一个 90 Hz 序号、已选择样本数、最终 +Grip 候选裁剪点和结束请求。ARMED 只有在 Grip 按下且 +`action_valid、command_sent` 同时为真时进入 RECORDING，跳过只建立相对位姿基准但 +尚未下发 CANFD 目标的首个 Grip 周期。`on_control` 先检查连续序号，再处理 Grip +边沿，最后用： + +```python +record_sample = ( + self.state is RecordingState.RECORDING + and (control_seq - self.sample_origin_seq) % 3 == 0 +) +``` + +首次有效 Grip 样本必须计入。B 结束只设置 `finish_requested=true`；等待下一条原子 +消息确认 Grip 已松开后才返回 `finish=true`，避免 PICO 回调与控制消息的到达顺序 +造成错误裁剪。 + +- [ ] **步骤 4：实现按键边沿和 Y 长按追踪** + +使用单调纳秒而不是 ROS wall clock： + +```python +class ButtonTracker: + def __init__(self, hold_ns: int) -> None: + self.hold_ns = hold_ns + self.right_b = False + self.right_a = False + self.left_y = False + self.left_y_started_ns: int | None = None + self.left_y_eligible = False + self.left_y_fired = False +``` + +只在按钮上升沿产生 B/A 事件。Y 上升沿时根据当前状态锁定 +`left_y_eligible`；持续按住达到 `1_000_000_000 ns` 只触发一次，松开后完全复位。 + +- [ ] **步骤 5：运行状态机测试** + +```bash +cd /home/robot/WS_xr +source /opt/ros/humble/setup.bash +pytest src/xr_rm_teleop/test/test_act_episode_recorder.py -k "session or button" -v +``` + +预期：所有状态与按键测试通过。 + +- [ ] **步骤 6：提交任务 4** + +```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录制状态机" +``` + +### 任务 5：实现 RealSense 帧缓冲和非未来帧对齐 + +**文件：** + +- 修改：`xr_rm_teleop/xr_rm_teleop/act_episode_recorder.py` +- 修改：`xr_rm_teleop/test/test_act_episode_recorder.py` + +- [ ] **步骤 1：写帧选择和相机质量测试** + +增加纯数据测试： + +```python +def test_select_frame_returns_latest_frame_not_after_control_time(): + frames = [ + CameraFrame(_image(1), 10, 100.0, 900_000_000), + CameraFrame(_image(2), 11, 133.3, 933_000_000), + CameraFrame(_image(3), 12, 166.6, 1_010_000_000), + ] + + selected, age_ms = select_frame(frames, 1_000_000_000, 100.0) + + assert selected.frame_number == 11 + assert age_ms == pytest.approx(67.0, abs=0.1) + + +def test_select_frame_rejects_frame_older_than_limit(): + frames = [CameraFrame(_image(1), 10, 100.0, 900_000_000)] + + with pytest.raises(QualityError, match="camera_frame_too_old"): + select_frame(frames, 1_000_000_000, 50.0) +``` + +第一项使用 `max_age_ms=100.0` 通过；随后显式用 50 ms 验证拒绝。另测未来帧 +不会被选、两相机 skew 超过 50 ms 拒绝、形状或 dtype 错误拒绝、帧号统计能计算 +采集 FPS 和丢帧率。 + +- [ ] **步骤 2：运行测试并确认失败** + +```bash +cd /home/robot/WS_xr +source /opt/ros/humble/setup.bash +pytest src/xr_rm_teleop/test/test_act_episode_recorder.py -k "frame or camera" -v +``` + +预期：失败，因为相机帧类型和选择函数尚不存在。 + +- [ ] **步骤 3：实现线程安全的小缓冲和选择函数** + +```python +@dataclass(frozen=True) +class CameraFrame: + image: np.ndarray + frame_number: int + hardware_timestamp_ms: float + host_monotonic_ns: int + + +def select_frame( + frames: tuple[CameraFrame, ...], + control_monotonic_ns: int, + max_age_ms: float, +) -> tuple[CameraFrame, float]: + eligible = [ + frame + for frame in frames + if frame.host_monotonic_ns <= control_monotonic_ns + ] + if not eligible: + raise QualityError("camera_frame_missing") + frame = max(eligible, key=lambda item: item.host_monotonic_ns) + age_ms = (control_monotonic_ns - frame.host_monotonic_ns) * 1e-6 + if age_ms > max_age_ms: + raise QualityError("camera_frame_too_old") + if frame.image.shape != (480, 640, 3) or frame.image.dtype != np.uint8: + raise QualityError("camera_frame_format") + return frame, age_ms +``` + +`CameraBuffer` 内部只使用 `deque(maxlen=4)` 和 `threading.Lock`，`snapshot()` 返回 +不可变 tuple，避免采样线程持锁写 HDF5。 + +- [ ] **步骤 4：实现可停止的 RealSense 采集器** + +`RealSenseCamera.start()` 内部才导入 `pyrealsense2`。启动时按序列号查找设备并 +校验期望型号，配置唯一 RGB 流： + +```python +config.enable_device(self.serial) +config.enable_stream( + rs.stream.color, + 640, + 480, + rs.format.rgb8, + 30, +) +``` + +线程循环使用 `wait_for_frames(timeout_ms=1000)`，取得 color frame 后立即记录 +`time.monotonic_ns()`，再把 `np.asanyarray(frame.get_data()).copy()`、帧号和硬件 +时间戳推入缓冲。停止时设置 Event、join 线程并调用 `pipeline.stop()`。超时或 SDK +异常保存到 `last_error`，由节点预检/录制逻辑拒绝，不调用任何机器人接口。 + +- [ ] **步骤 5：运行相机纯逻辑测试** + +```bash +cd /home/robot/WS_xr +source /opt/ros/humble/setup.bash +pytest src/xr_rm_teleop/test/test_act_episode_recorder.py -k "frame or camera" -v +``` + +预期：测试仅使用合成 NumPy 图像，不访问 USB 相机，并全部通过。 + +- [ ] **步骤 6：提交任务 5** + +```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双相机帧" +``` + +### 任务 6：实现流式 HDF5、编号防覆盖和崩溃恢复 + +**文件：** + +- 修改：`xr_rm_teleop/xr_rm_teleop/act_episode_recorder.py` +- 修改：`xr_rm_teleop/test/test_act_episode_recorder.py` + +- [ ] **步骤 1：只在现有 XR Conda 环境安装并验证 h5py** + +```bash +/home/robot/miniconda3/envs/xr/bin/python -m pip install h5py +/home/robot/miniconda3/envs/xr/bin/python -c \ + "import h5py; print(h5py.__version__)" +``` + +预期：输出一个 h5py 版本号。不得使用 `sudo`、不得修改系统 Python、不得创建新 +Conda 环境。 + +- [ ] **步骤 2：写 HDF5 结构和变量长度测试** + +使用 `tmp_path` 创建 3 个合成样本，验证： + +```python +def test_episode_store_writes_act_core_schema(tmp_path): + store = EpisodeStore.create(tmp_path / "episode_0.partial.hdf5", _metadata()) + store.append(_episode_sample(seq=100)) + store.append(_episode_sample(seq=103)) + store.append(_episode_sample(seq=106)) + store.close() + + with h5py.File(store.path, "r") as root: + assert root.attrs["sim"] == np.bool_(False) + assert root.attrs["action_alignment"] == "same_step_causal" + assert root["observations/qpos"].shape == (3, 8) + assert root["observations/qpos"].dtype == np.float32 + assert root["action"].shape == (3, 8) + assert root["action"].dtype == np.float32 + assert root["observations/images/cam_high"].shape == (3, 480, 640, 3) + assert root["observations/images/cam_high"].dtype == np.uint8 + assert root["observations/images/cam_right_wrist"].shape == (3, 480, 640, 3) + assert "observations/qvel" not in root + assert "observations/effort" not in root + assert "compress_len" not in root +``` + +另测 `truncate(2)` 后所有时间轴数据集长度都是 2。 + +- [ ] **步骤 3：运行 HDF5 测试并确认失败** + +```bash +cd /home/robot/WS_xr +source /opt/ros/humble/setup.bash +/home/robot/miniconda3/envs/xr/bin/python -m pytest \ + src/xr_rm_teleop/test/test_act_episode_recorder.py -k "episode_store" -v +``` + +预期：失败，因为 `EpisodeStore` 尚未实现。 + +- [ ] **步骤 4：实现固定数据布局和流式追加** + +定义所有数据集，不允许调用方任意创建路径。核心布局： + +```python +CORE_LAYOUT = { + "observations/qpos": (np.float32, (8,), (256, 8)), + "action": (np.float32, (8,), (256, 8)), + "observations/images/cam_high": ( + np.uint8, (480, 640, 3), (1, 480, 640, 3) + ), + "observations/images/cam_right_wrist": ( + np.uint8, (480, 640, 3), (1, 480, 640, 3) + ), +} +``` + +Debug 布局完整包含规格中的时间戳、控制状态、QP、TCP、PICO、夹爪，并增加质量 +检查所需的两个相机帧号： + +```python +DEBUG_LAYOUT = { + "debug/timestamps/control_monotonic_ns": (np.int64, (), (256,)), + "debug/timestamps/feedback_monotonic_ns": (np.int64, (), (256,)), + "debug/timestamps/action_monotonic_ns": (np.int64, (), (256,)), + "debug/timestamps/cam_high_host_monotonic_ns": (np.int64, (), (256,)), + "debug/timestamps/cam_wrist_host_monotonic_ns": (np.int64, (), (256,)), + "debug/timestamps/cam_high_hardware_ms": (np.float64, (), (256,)), + "debug/timestamps/cam_wrist_hardware_ms": (np.float64, (), (256,)), + "debug/timestamps/cam_high_age_ms": (np.float32, (), (256,)), + "debug/timestamps/cam_wrist_age_ms": (np.float32, (), (256,)), + "debug/timestamps/inter_camera_skew_ms": (np.float32, (), (256,)), + "debug/cameras/cam_high_frame_number": (np.uint64, (), (256,)), + "debug/cameras/cam_wrist_frame_number": (np.uint64, (), (256,)), + "debug/control/control_seq": (np.uint64, (), (256,)), + "debug/control/teleop_active": (np.uint8, (), (256,)), + "debug/control/action_valid": (np.uint8, (), (256,)), + "debug/control/command_sent": (np.uint8, (), (256,)), + "debug/control/target_clamped": (np.uint8, (), (256,)), + "debug/control/control_fault": (np.uint8, (), (256,)), + "debug/qp/raw_target": (np.float32, (7,), (256, 7)), + "debug/qp/attempted": (np.uint8, (), (256,)), + "debug/qp/success": (np.uint8, (), (256,)), + "debug/qp/duration_ms": (np.float32, (), (256,)), + "debug/tcp/current_pose": (np.float32, (7,), (256, 7)), + "debug/tcp/raw_target_pose": (np.float32, (7,), (256, 7)), + "debug/tcp/final_target_pose": (np.float32, (7,), (256, 7)), + "debug/tcp/command_velocity": (np.float32, (6,), (256, 6)), + "debug/pico/right_pose": (np.float32, (7,), (256, 7)), + "debug/pico/right_inputs": (np.float32, (6,), (256, 6)), + "debug/pico/left_secondary": (np.uint8, (), (256,)), + "debug/gripper/target_open": (np.uint8, (), (256,)), + "debug/gripper/state_open": (np.uint8, (), (256,)), + "debug/gripper/command_pending": (np.uint8, (), (256,)), + "debug/gripper/command_failed": (np.uint8, (), (256,)), +} +``` + +每个数据集使用 `shape=(0, *sample_shape)`、`maxshape=(None, *sample_shape)`， +`append` 先校验 key、shape 和有限值，再统一 resize 到 `count+1` 并写入。图像不 +压缩。`truncate(count)` 对所有数据集使用同一个长度。 + +有限值扫描只用于数值状态、动作和 debug 浮点数据；图像只检查 `uint8` 和固定 +shape，避免每帧重复扫描约 1.8 MB 图像。元数据类型固定为： + +```python +@dataclass(frozen=True) +class EpisodeMetadata: + joint_names: tuple[str, ...] + joint_lower_limits: np.ndarray + joint_upper_limits: np.ndarray +``` + +`EpisodeStore.create` 必须一次写入： + +```python +root.attrs["sim"] = False +root.attrs["task_name"] = "tomato_pick" +root.attrs["sample_rate_hz"] = 30 +root.attrs["action_alignment"] = "same_step_causal" +root.attrs["arm"] = "right_rm75" +root.attrs["episode_status"] = "recording" +root.attrs["camera_high_serial"] = "234222303366" +root.attrs["camera_right_wrist_serial"] = "412622272532" +root.attrs["joint_names"] = metadata.joint_names +root.attrs["joint_lower_limits"] = metadata.joint_lower_limits +root.attrs["joint_upper_limits"] = metadata.joint_upper_limits +root.attrs["pose_order"] = "x,y,z,qx,qy,qz,qw" +root.attrs["right_input_order"] = "grip,trigger,primary,secondary,axis_x,axis_y" +root.attrs["interrupted"] = False +``` + +保存或拒绝前把 `episode_status` 更新为 `saved` 或 `rejected`；拒绝文件同时写入 +`reject_reason`，中断/崩溃恢复文件把 `interrupted` 更新为 `true`。 + +- [ ] **步骤 5：写编号、锁、正式发布和恢复测试** + +```python +def test_next_index_uses_max_saved_episode_and_ignores_rejected(tmp_path): + (tmp_path / "episode_2.hdf5").touch() + (tmp_path / "episode_9.hdf5").touch() + rejected = tmp_path / "rejected" + rejected.mkdir() + (rejected / "episode_20_bad_20260810.hdf5").touch() + + assert next_episode_index(tmp_path) == 10 + + +def test_publish_never_overwrites_existing_episode(tmp_path): + partial = tmp_path / "episode_1.partial.hdf5" + partial.write_bytes(b"new") + final = tmp_path / "episode_1.hdf5" + final.write_bytes(b"old") + + with pytest.raises(FileExistsError): + publish_without_overwrite(partial, final) + + assert final.read_bytes() == b"old" +``` + +再测可读 partial 被标记 `crash_recovered` 并移入拒绝目录、不可读 partial 改名保留、 +手动丢弃只删除当前 partial、拒绝不改变下一正式编号。 + +- [ ] **步骤 6：实现任务目录和不覆盖发布** + +使用 `fcntl.flock(lock_file, LOCK_EX | LOCK_NB)` 持有任务目录锁。编号只匹配： + +```python +EPISODE_PATTERN = re.compile(r"^episode_(\d+)\.hdf5$") +``` + +临时文件关闭后使用同文件系统硬链接实现不覆盖发布： + +```python +def publish_without_overwrite(partial: Path, destination: Path) -> None: + os.link(partial, destination) + partial.unlink() +``` + +目标存在时 `os.link` 必须抛出 `FileExistsError`，原 partial 保留。恢复可读文件时用 +`h5py.File(path, "r+")` 写 `episode_status="rejected"`、 +`reject_reason="crash_recovered"`、`interrupted=true`，再发布到带原因和时间戳的 +拒绝文件。不可读文件只改成唯一带时间戳的 `.partial.hdf5` 名称。 + +- [ ] **步骤 7：运行 HDF5、编号与恢复测试** + +```bash +cd /home/robot/WS_xr +source /opt/ros/humble/setup.bash +/home/robot/miniconda3/envs/xr/bin/python -m pytest \ + src/xr_rm_teleop/test/test_act_episode_recorder.py \ + -k "episode_store or index or publish or recover" -v +``` + +预期：全部通过，且测试只操作 pytest 临时目录。 + +- [ ] **步骤 8：提交任务 6** + +```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 HDF5数据" +``` + +### 任务 7：实现 episode 质量检查和拒绝原因 + +**文件：** + +- 修改：`xr_rm_teleop/xr_rm_teleop/act_episode_recorder.py` +- 修改：`xr_rm_teleop/test/test_act_episode_recorder.py` + +- [ ] **步骤 1：写通过样本和逐项失败参数化测试** + +先创建一个 60 样本的最小合法文件，再逐项破坏副本。参数化断言至少包含： + +```python +@pytest.mark.parametrize( + ("mutation", "reason"), + [ + ("short_episode", "too_few_samples"), + ("control_seq_gap", "control_sequence_gap"), + ("nonfinite_qpos", "nonfinite_qpos"), + ("joint_limit", "joint_limit_violation"), + ("invalid_gripper", "invalid_gripper_state"), + ("feedback_age", "feedback_too_old"), + ("action_invalid", "invalid_action"), + ("control_fault", "control_fault"), + ("camera_fps", "camera_fps"), + ("camera_drop", "camera_drop_ratio"), + ("camera_age", "camera_frame_too_old"), + ("camera_skew", "camera_skew"), + ("final_gripper_closed", "final_gripper_not_open"), + ], +) +def test_validate_episode_reports_stable_reason(valid_episode, mutation, reason): + mutate_episode(valid_episode, mutation) + report = validate_episode(valid_episode, _quality_limits()) + assert not report.accepted + assert report.reason == reason +``` + +另测多个 QP 失败样本仍通过，并正确得到失败次数、占比和最长连续失败数。 + +- [ ] **步骤 2：运行质量测试并确认失败** + +```bash +cd /home/robot/WS_xr +source /opt/ros/humble/setup.bash +/home/robot/miniconda3/envs/xr/bin/python -m pytest \ + src/xr_rm_teleop/test/test_act_episode_recorder.py -k validate_episode -v +``` + +预期：失败，因为最终质量报告尚未实现。 + +- [ ] **步骤 3：实现质量配置和稳定报告类型** + +```python +@dataclass(frozen=True) +class QualityLimits: + min_samples: int = 60 + max_samples: int = 1800 + min_control_hz: float = 27.0 + max_control_gap_ms: float = 100.0 + min_camera_fps: float = 27.0 + max_drop_ratio: float = 0.01 + max_feedback_age_ms: float = 50.0 + max_camera_age_ms: float = 50.0 + max_camera_skew_ms: float = 50.0 + + +@dataclass(frozen=True) +class QualityReport: + accepted: bool + reason: str | None + metrics: dict[str, int | float] +``` + +`validate_episode` 按固定顺序返回第一个硬失败，保证拒绝原因可测试、可检索。校验 +所有时间轴长度相同；核心 shape/dtype；有限值；根属性中的关节上下限；夹爪 +`0/1`；控制序号差为 3；控制/反馈时间差；图像时间、shape 和 dtype；相机帧号； +平均控制频率和最大控制间隔；最终夹爪打开；根属性中的相机采集 FPS 和硬件丢帧率。 + +- [ ] **步骤 4：实现 QP 和限位汇总** + +只在 `debug/qp/attempted==1` 的样本中统计失败： + +```python +failed = attempted & ~success +metrics["qp_failure_count"] = int(failed.sum()) +metrics["qp_failure_ratio"] = float(failed.sum() / max(1, attempted.sum())) +metrics["qp_longest_failure_streak"] = longest_true_run(failed) +metrics["target_clamped_count"] = int(target_clamped.sum()) +``` + +这些指标不进入拒绝判断。`longest_true_run` 使用一次线性循环，不引入 pandas。 + +- [ ] **步骤 5：运行全部质量测试** + +```bash +cd /home/robot/WS_xr +source /opt/ros/humble/setup.bash +/home/robot/miniconda3/envs/xr/bin/python -m pytest \ + src/xr_rm_teleop/test/test_act_episode_recorder.py -k validate_episode -v +``` + +预期：合法文件通过，各破坏样本返回预期稳定原因，QP 回退文件仍通过。 + +- [ ] **步骤 6：提交任务 7** + +```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 episode数据质量" +``` + +### 任务 8：集成 ROS 采集节点、写入线程和完整操作流程 + +**文件：** + +- 修改：`xr_rm_teleop/xr_rm_teleop/act_episode_recorder.py` +- 修改：`xr_rm_teleop/test/test_act_episode_recorder.py` +- 修改：`xr_rm_teleop/setup.py` + +- [ ] **步骤 1：写预检和节点流程测试** + +使用假相机、假目录、假 publisher 和直接调用回调的方法，覆盖： + +```python +def test_preflight_requires_open_gripper_fresh_inputs_and_disk_space(): + recorder = _recorder_for_test() + recorder.latest_control.gripper_state_known = True + recorder.latest_control.gripper_state_open = True + recorder.right_controller_age_ms = 10.0 + recorder.left_controller_age_ms = 10.0 + recorder.free_space_bytes = 5 * 1024**3 + + assert recorder._run_preflight() is None + + +def test_end_to_end_fake_episode_saves_and_returns_idle(tmp_path): + recorder = _recorder_for_test(tmp_path=tmp_path) + recorder._on_right_b(grip=False) + assert recorder.state is RecordingState.ARMED + + for message in _valid_control_messages(90 * 3): + recorder._on_control_sample(message) + recorder._request_finish() + recorder._on_control_sample(_released_message(seq=271)) + recorder._wait_for_writer() + + assert (tmp_path / "tomato_pick" / "episode_0.hdf5").is_file() + assert recorder.state is RecordingState.IDLE +``` + +另测：Y 删除 partial；A 生成拒绝文件；最大时长、相机故障、队列满、写盘异常和 +Ctrl+C 均生成对应原因；拒绝不调用任何机器人 API。 + +- [ ] **步骤 2：运行集成测试并确认失败** + +```bash +cd /home/robot/WS_xr +source /opt/ros/humble/setup.bash +source install/setup.bash +/home/robot/miniconda3/envs/xr/bin/python -m pytest \ + src/xr_rm_teleop/test/test_act_episode_recorder.py -k "preflight or end_to_end" -v +``` + +预期：失败，因为 ROS 节点和工作线程尚未连接现有纯逻辑。 + +- [ ] **步骤 3：实现节点参数、订阅和状态发布** + +`ActEpisodeRecorder(Node)` 声明 YAML 中的全部参数，创建： + +```python +self._status_pub = self.create_publisher( + String, + self._status_topic, + 10, +) +self.create_subscription( + ActControlSample, + self._control_sample_topic, + self._on_control_sample, + sensor_data_qos, +) +self.create_subscription( + XrController, + self._right_controller_topic, + self._on_right_controller, + 10, +) +self.create_subscription( + XrController, + self._left_controller_topic, + self._on_left_controller, + 10, +) +``` + +节点启动顺序固定为：验证参数和 h5py → 创建并锁定任务目录 → 恢复 partial → 打开 +两台相机并开始预热 → 创建 ROS 订阅。相机启动失败时节点记录错误并保持不可录制， +不触发 launch 全局关闭。 + +- [ ] **步骤 4：实现样本构造和有界写入线程** + +收到被状态机选中的控制消息时： + +```python +qpos = np.asarray( + [*message.q_actual, float(message.gripper_state_open)], + dtype=np.float32, +) +action = np.asarray( + [*message.q_target, float(message.gripper_target_open)], + dtype=np.float32, +) +``` + +然后按控制单调时间选择两路帧、计算年龄和 skew、构造全部核心/debug 值，并调用 +`EpisodeWriter.submit(sample)`。`EpisodeWriter` 使用 `queue.Queue(maxsize=8)` 和单 +写线程；队列满立即以 `writer_backlog` 拒绝，不在 ROS 回调中等待磁盘。结束时： + +```text +停止接受新样本 +→ queue.join() +→ 检查写线程异常 +→ truncate(candidate_end_count) +→ 等待夹爪命令完成（最多 3 秒） +→ 关闭 HDF5 +→ validate_episode +→ 发布正式文件或拒绝文件 +``` + +`EpisodeWriter` 的线程异常保存在一个受锁保护的字段中，ROS 节点每次控制回调和 +保存前检查；异常原因固定为 `disk_write_error`。 + +进入 `ARMED` 时保存两台相机的累计帧数、首末主机时间和累计硬件丢帧数；结束时 +用差值计算本 episode 的采集 FPS 与硬件丢帧率，并写入根属性。这样预热阶段不会 +稀释本次 episode 的质量指标。 + +- [ ] **步骤 5：实现预检、状态结果和中断处理** + +预检使用 `shutil.disk_usage`、PICO/控制消息接收单调时间、相机连续 5 秒统计和 +夹爪状态。可用空间要求 `>=4*1024**3`。结果状态方法统一： + +```python +def _publish_state(self, state: RecordingState, reason: str = "") -> None: + message = String() + message.data = state.value if not reason else f"{state.value}:{reason}" + self._status_pub.publish(message) +``` + +`SAVED`、`DISCARDED`、`REJECTED` 发布后立即发布 `IDLE`。`main()` 捕获 +`KeyboardInterrupt` 后先调用 `node.interrupt_recording("interrupted")`，再停止 +相机、关闭 writer、释放目录锁和销毁节点。正常 IDLE 退出不生成文件。 + +- [ ] **步骤 6：安装 console script 且保持普通遥操无 h5py 强依赖** + +在 `setup.py` 增加： + +```python +"act_episode_recorder = xr_rm_teleop.act_episode_recorder:main", +``` + +不要把 h5py 或 pyrealsense2 加进系统 `package.xml` 依赖；它们只属于现有 XR Conda +运行环境。`single_arm_velocity_teleop` 不导入采集模块，因此 `record_act=false` +不会加载相机或 HDF5。 + +- [ ] **步骤 7：运行采集节点全部合成测试** + +```bash +cd /home/robot/WS_xr +source /opt/ros/humble/setup.bash +source install/setup.bash +/home/robot/miniconda3/envs/xr/bin/python -m pytest \ + src/xr_rm_teleop/test/test_act_episode_recorder.py -v +``` + +预期：所有测试通过，测试进程没有访问 RealSense 或 RealMan。 + +- [ ] **步骤 8：提交任务 8** + +```bash +git add src/xr_rm_teleop/xr_rm_teleop/act_episode_recorder.py \ + src/xr_rm_teleop/test/test_act_episode_recorder.py \ + src/xr_rm_teleop/setup.py +git commit -m "feat: 集成ACT episode采集节点" +``` + +### 任务 9：接入统一 launch 和番茄采摘配置 + +**文件：** + +- 新建：`xr_rm_bringup/config/act_tomato_pick.yaml` +- 修改：`xr_rm_bringup/launch/arm_debug.launch.py` +- 修改：`xr_rm_bringup/test/test_arm_debug_launch.py` + +- [ ] **步骤 1：写 launch 默认值和组合校验测试** + +在现有测试中增加： + +```python +def test_act_recording_is_disabled_by_default() -> None: + description = arm_debug_launch.generate_launch_description() + arguments = { + entity.name: entity + for entity in description.entities + if isinstance(entity, DeclareLaunchArgument) + } + assert perform_substitutions( + LaunchContext(), + arguments["record_act"].default_value, + ) == "false" + + +@pytest.mark.parametrize( + ("arm", "use_mock"), + [("left", False), ("both", False), ("right", True)], +) +def test_act_recording_rejects_unsupported_modes(arm, use_mock) -> None: + with pytest.raises(ValueError, match="arm:=right use_mock:=false"): + arm_debug_launch._validate_act_mode(arm, use_mock, True) + + +def test_act_recording_accepts_right_real_mode() -> None: + arm_debug_launch._validate_act_mode("right", False, True) + arm_debug_launch._validate_act_mode("both", True, False) +``` + +- [ ] **步骤 2：运行 launch 测试并确认失败** + +```bash +cd /home/robot/WS_xr +source /opt/ros/humble/setup.bash +pytest src/xr_rm_bringup/test/test_arm_debug_launch.py -v +``` + +预期：失败，因为 `record_act` 和 `_validate_act_mode` 尚不存在。 + +- [ ] **步骤 3：添加完整采集配置** + +创建 `act_tomato_pick.yaml`： + +```yaml +act_episode_recorder: + ros__parameters: + output_root: /home/robot/ACT_Data + task_name: tomato_pick + control_sample_topic: /xr_rm/right_rm75/act_control_sample + right_controller_topic: /xr/right_controller + left_controller_topic: /xr/left_controller + status_topic: /act/recording_status + + cam_high_serial: "234222303366" + cam_high_model: D455 + cam_right_wrist_serial: "412622272532" + cam_right_wrist_model: D405 + image_width: 640 + image_height: 480 + camera_fps: 30 + camera_warmup_sec: 5.0 + + control_rate_hz: 90.0 + sample_rate_hz: 30.0 + min_samples: 60 + max_samples: 1800 + min_control_hz: 27.0 + max_control_gap_ms: 100.0 + min_camera_fps: 27.0 + max_drop_ratio: 0.01 + max_feedback_age_ms: 50.0 + max_camera_age_ms: 50.0 + max_camera_skew_ms: 50.0 + min_free_space_gib: 4.0 + y_hold_sec: 1.0 + gripper_completion_timeout_sec: 3.0 + writer_queue_size: 8 +``` + +参数校验明确要求控制频率可以被采样频率整除且比值为 3，图像尺寸和通道必须与 +固定 HDF5 schema 一致。 + +- [ ] **步骤 4：增加 launch 分支** + +声明默认关闭参数并增加校验： + +```python +def _validate_act_mode(arm: str, use_mock: bool, record_act: bool) -> None: + if record_act and (arm != "right" or use_mock): + raise ValueError( + "record_act:=true requires arm:=right use_mock:=false" + ) +``` + +`_act_recorder_node()` 使用现有 `XR_PYTHON` prefix、专用 YAML 和固定节点名 +`act_episode_recorder`。只有 `record_act=true` 才把该节点加入列表。不要为采集节点 +设置 `on_exit=Shutdown`；UDP 接收器原有退出联动保持不变。 + +- [ ] **步骤 5：运行 launch 测试和参数展示** + +```bash +cd /home/robot/WS_xr +source /opt/ros/humble/setup.bash +pytest src/xr_rm_bringup/test/test_arm_debug_launch.py -v +colcon build --symlink-install --packages-select \ + xr_rm_interfaces xr_rm_teleop xr_rm_bringup +source install/setup.bash +ros2 launch xr_rm_bringup arm_debug.launch.py --show-args +``` + +预期：测试和构建通过，参数列表显示 `record_act` 默认 `false`。该命令只展示参数， +不启动真机节点。 + +- [ ] **步骤 6：提交任务 9** + +```bash +git add src/xr_rm_bringup/config/act_tomato_pick.yaml \ + src/xr_rm_bringup/launch/arm_debug.launch.py \ + src/xr_rm_bringup/test/test_arm_debug_launch.py +git commit -m "feat: 接入番茄采摘ACT采集启动项" +``` + +### 任务 10：完整回归验证和真机验收交接 + +**文件：** + +- 验证：本计划涉及的全部文件 +- 不修改：训练代码、系统 Python、真机安全配置 + +- [ ] **步骤 1：运行格式和变更范围检查** + +```bash +cd /home/robot/WS_xr/src +git diff --check +git status --short +``` + +预期：`git diff --check` 无输出；状态只包含本规格和计划列出的文件，不包含 +`build/`、`install/`、`log/` 或无关格式化改动。 + +- [ ] **步骤 2：运行全部相关 Python 测试** + +```bash +cd /home/robot/WS_xr +source /opt/ros/humble/setup.bash +source install/setup.bash +/home/robot/miniconda3/envs/xr/bin/python -m pytest \ + src/xr_rm_teleop/test/test_act_control_sample.py \ + src/xr_rm_teleop/test/test_act_episode_recorder.py \ + src/xr_rm_teleop/test/test_initial_joint_pose.py \ + src/xr_rm_teleop/test/test_joint_control.py \ + src/xr_rm_teleop/test/test_orientation_control.py \ + src/xr_rm_teleop/test/test_placo_transforms.py \ + src/xr_rm_bringup/test/test_arm_debug_launch.py -v +``` + +预期：全部通过。若发现任务开始前就存在的失败，记录完整命令和失败输出，不能修改 +无关逻辑掩盖它。 + +- [ ] **步骤 3：从工作空间根目录完成全量构建** + +```bash +cd /home/robot/WS_xr +source /opt/ros/humble/setup.bash +colcon build --symlink-install +``` + +预期：所有包构建成功，源码目录中不生成 `build/`、`install/`、`log/`。 + +- [ ] **步骤 4：运行不连接硬件的接口与启动检查** + +```bash +cd /home/robot/WS_xr +source /opt/ros/humble/setup.bash +source install/setup.bash +ros2 interface show xr_rm_interfaces/msg/ActControlSample +ros2 launch xr_rm_bringup arm_debug.launch.py --show-args +``` + +预期：接口字段完整，`record_act` 默认关闭。不要在自动验证中运行 +`use_mock:=false`。 + +- [ ] **步骤 5：复核 HDF5 合成产物** + +使用测试生成的临时文件或单独的 `tempfile.TemporaryDirectory()` 调用 +`EpisodeStore` 写 60 帧，再用 h5py 断言： + +```python +assert root["observations/qpos"].shape == (60, 8) +assert root["action"].shape == (60, 8) +assert root["observations/images/cam_high"].shape == (60, 480, 640, 3) +assert root["observations/images/cam_right_wrist"].shape == (60, 480, 640, 3) +assert root.attrs["sim"] == np.bool_(False) +assert root.attrs["action_alignment"] == "same_step_causal" +``` + +预期：质量报告接受该文件；不存在 qvel、effort 和压缩字段。 + +- [ ] **步骤 6：提交最终验证修正（仅在确有必要时）** + +如果步骤 1 至 5 暴露了本功能范围内的问题，先补失败测试、做最小修正并重跑对应 +验证，再提交： + +```bash +git add src/xr_rm_interfaces/msg/ActControlSample.msg \ + src/xr_rm_interfaces/CMakeLists.txt \ + src/xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py \ + src/xr_rm_teleop/xr_rm_teleop/fun_peripheral.py \ + src/xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py \ + src/xr_rm_teleop/xr_rm_teleop/act_episode_recorder.py \ + src/xr_rm_teleop/setup.py \ + src/xr_rm_bringup/config/peripherals_rm75.yaml \ + src/xr_rm_bringup/config/act_tomato_pick.yaml \ + src/xr_rm_bringup/launch/arm_debug.launch.py \ + src/xr_rm_teleop/test/test_initial_joint_pose.py \ + src/xr_rm_teleop/test/test_joint_control.py \ + src/xr_rm_teleop/test/test_orientation_control.py \ + src/xr_rm_teleop/test/test_placo_transforms.py \ + src/xr_rm_teleop/test/test_act_control_sample.py \ + src/xr_rm_teleop/test/test_act_episode_recorder.py \ + src/xr_rm_bringup/test/test_arm_debug_launch.py +git commit -m "fix: 修正ACT采集集成问题" +``` + +如果没有产生修正，不创建空提交。 + +- [ ] **步骤 7：向用户交接真机手工验收命令，不自行执行** + +只有用户明确授权连接真机后，才可运行： + +```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 +``` + +手工验收顺序固定为：确认夹爪初始化打开 → 确认两路相机角色 → B 进入 ARMED → +Grip 完成采摘/释放 → 确认夹爪逻辑 open → 松 Grip → B 保存 → 等待 SAVED/IDLE → +A 回初始位姿。另行验证 Y 丢弃、A 误触拒绝、QP 短暂失败、重启编号延续和 Ctrl+C +中断文件。任何真机异常继续由现有安全停止流程处理。 diff --git a/docs/superpowers/specs/2026-08-10-act-tomato-pick-data-collection-design.md b/docs/superpowers/specs/2026-08-10-act-tomato-pick-data-collection-design.md new file mode 100644 index 0000000..71b972d --- /dev/null +++ b/docs/superpowers/specs/2026-08-10-act-tomato-pick-data-collection-design.md @@ -0,0 +1,657 @@ +# 右臂番茄采摘 ACT 数据采集适配设计 + +## 背景与目标 + +当前项目已经通过 ROS2 Humble、PICO 手柄、Placo QP 和 RealMan Python API2 +完成 RM75 遥操作。右臂遥操节点以名义 `90 Hz` 读取实际关节反馈，根据 PICO +相对位姿生成 TCP 目标，经过工作空间限制、QP、关节速度与加速度限制后，通过 +`rm_movej_canfd` 下发最终关节目标。 + +本次变更在不重写现有遥操链路的前提下，增加一个独立的 ALOHA/ACT 风格数据采集 +节点。第一阶段只采集右臂番茄采摘任务，每个 episode 覆盖从初始位姿出发、抓取 +番茄、搬运至 RM75 下方收集篮并释放番茄的完整过程。 + +核心目标如下： + +- 以右臂 7 个实际关节角和夹爪逻辑状态作为 `observations/qpos`； +- 以实际成功下发或零阶保持的 7 个最终关节目标和夹爪目标作为 `action`； +- 同步采集一台全局 D455 和一台右腕 D405 的 RGB 图像； +- 以 `30 Hz` 形成同周期因果对齐的数据； +- 每个完整任务流式保存为一个 ALOHA/ACT 核心结构兼容的 HDF5 文件； +- 支持手柄开始、结束、丢弃、拒绝、质量检查、崩溃恢复和编号防覆盖； +- 保留足够的 PICO、TCP、QP、夹爪和时间戳调试数据，但不让采集节点进入机器人 + 控制链路。 + +参考实现为 ALOHA 官方仓库中的 +[`record_episodes.py`](https://github.com/tonyzhaozh/aloha/blob/master/aloha_scripts/record_episodes.py)。 +官方双臂数据使用 14 维状态和动作；本项目第一阶段采用右臂 `7+1=8` 维，因此只 +保证 HDF5 核心组织方式兼容，不声称官方旧训练加载器可以不修改直接训练。 + +## 非目标 + +第一阶段明确不实现： + +- 左臂或双臂 ACT 采集； +- ACT 训练代码、数据加载器、策略部署或自动完成判定； +- 深度图、红外图、点云、图像压缩或 ROS 图像话题； +- rosbag、中间格式、离线转换工具、GUI、声音或手柄震动反馈； +- 让 ACT 学习 A 键触发的回初始位姿运动； +- 修改现有工作空间限制、圆柱限制、速度限制、指令超时、安全停止或 + `move_to_initial_pose_on_connect` 默认值； +- 新建重复的 ROS2 包或第二个 RealMan 连接。 + +后续训练时建议将 ACT `chunk_size` 设为 `60`，对应约 2 秒动作长度；该参数属于 +训练配置，不写死在采集逻辑中。 + +## 现有控制链路与关键约束 + +现有 `single_arm_velocity_teleop` 在每个控制周期内依次完成： + +```text +读取最新 RM75 反馈 +→ 同步 Placo 状态 +→ 读取 PICO 状态并生成 TCP 目标 +→ 工作空间、位姿步长与速度限制 +→ Placo QP +→ 关节速度与加速度限制 +→ rm_movej_canfd 下发 +→ 发布 joint_target 调试话题 +``` + +当前 `joint_states`、TCP 调试话题和 `joint_target` 分别发布并各自取时间戳。如果 +采集节点仅订阅这些分散话题，即使时间接近，也可能把第 100 个控制周期的 +`q_actual` 与第 99 个控制周期的 `q_target` 拼在一起。这里的“第 100 个周期”是 +控制序号，不是 `100 Hz`；现有控制频率仍是名义 `90 Hz`。 + +此外，A 键回位调用的是一次阻塞式 `rm_movej(initial_joint_pose)`。轨迹由 RM75 +控制器内部生成，项目不能获得每个控制周期的中间目标，也不经过当前 QP 和 +`rm_movej_canfd` 链路，因此不能与遥操动作混用同一种标签语义。 + +## 总体架构 + +采用一个自定义原子控制采样消息和一个独立 ACT 采集节点： + +```text +PICO 输入 + RM75 反馈 + ↓ +single_arm_velocity_teleop（90 Hz） + ↓ +QP → 关节限速/限加速度 → RM75 下发 + ↓ +ActControlSample（同周期、同时间戳、同序号） + ↓ +act_episode_recorder（每 3 个控制周期取 1 个） + ├──────────────┐ + ↓ ↓ + 全局 D455 RGB 右腕 D405 RGB + └──────┬───────┘ + ↓ + 30 Hz 流式写入临时 HDF5 + ↓ + 裁剪 → 质量检查 → 保存/拒绝/丢弃 +``` + +“原子”表示消息是一个逻辑上不可拆分的控制周期快照。订阅者要么收到该周期完整的 +反馈、求解结果、最终动作和状态，要么该周期整体缺失；不会自行拼接多个异步话题。 + +职责边界如下： + +- 遥操节点继续唯一负责 RM75 连接、反馈、QP、限位、动作下发和安全停止； +- 遥操节点只增加原子消息发布和夹爪逻辑状态记录，不读取相机、不写 HDF5； +- 采集节点只订阅控制/PICO 数据、独占两台相机并写文件，不连接或控制 RM75； +- 采集节点异常、退出或写盘过慢不得阻塞遥操发布或改变机器人动作； +- 相机由采集节点通过 `pyrealsense2` 直接打开，不再发布和重新订阅 ROS 图像。 + +原子消息使用本机低延迟、非阻塞的 best-effort QoS。采集节点检测任何需要保留的 +控制周期丢失并拒绝 episode，而不是让 DDS 反压影响遥操控制。 + +## 原子控制采样消息 + +在 `xr_rm_interfaces` 中新增 `ActControlSample.msg`，发布话题为： + +```text +/xr_rm/right_rm75/act_control_sample +``` + +消息至少表达以下内容： + +| 类别 | 字段语义 | +|---|---| +| 周期标识 | ROS header、`control_seq`、控制周期单调时间戳 | +| 关节反馈 | `q_actual[7]`、当前 URDF 关节上下限、反馈接收单调时间戳、反馈年龄、反馈有效状态 | +| QP | QP 原始输出或失败时的保持目标 `q_qp_raw[7]`、是否尝试、成功状态、耗时 | +| 最终动作 | 经关节限速后的 `q_target[7]`、动作时间戳、是否当前周期成功下发 | +| TCP | 当前 TCP、PICO 映射前的原始目标 TCP、最终受限目标 TCP、命令速度 | +| PICO | 当前右手位姿、Grip、Trigger、A、B 和摇杆值 | +| 夹爪 | 请求目标、已确认逻辑状态、命令是否处理中、命令是否失败 | +| 控制状态 | `teleop_active`、`action_valid`、QP 回退、目标限位和控制故障状态 | + +数值型关节字段在 ROS 消息中保持双精度，写入 HDF5 核心数据时显式转换成 +`float32`。位姿使用位置加四元数，不保存完整 Placo 对象、Hessian、约束矩阵或 +其他大体积求解器内部状态。 + +消息在每个实际执行的 90 Hz 控制回调中发布，包括 Grip 松开和安全停止状态： + +- 当前周期成功发送动作时，`command_sent=true`； +- 当前周期没有发送，但此前存在成功目标时，`q_target` 零阶保持上一个成功目标， + `command_sent=false`、`action_valid=true`； +- 尚未形成任何有效目标或当前动作发送失败时，`action_valid=false`； +- QP 失败但成功重发上次有效目标时，`qp_success=false`、`action_valid=true`； +- 发送失败必须发布失败状态，并由采集节点拒绝当前 episode。 + +相机数据不放进该消息。相机时间戳和帧号由采集节点在同一主机的单调时钟域中补充。 + +## 夹爪数据语义 + +右臂使用 Modbus 电动夹爪。由于不同番茄尺寸会导致实际停止开度不同，而本项目只 +关心抓取意图，第一阶段不读取或估算实际开度。 + +统一约定： + +```text +0 = closed +1 = open +``` + +两类状态必须分开： + +- `action[7]` 是目标状态，在 Trigger 产生开合请求的控制周期立即改变； +- `qpos[7]` 是已确认逻辑状态，只有 Modbus 命令正常返回后才改变； +- 命令失败时 `qpos[7]` 保持原值，并拒绝当前 episode； +- 采集前右臂夹爪必须成功初始化为完全打开 `1.0`，之后才允许把初始 + `qpos[7]` 设为 `1`； +- 不再使用原先含义不明确的 `0.75 → 0.15` 初始化序列。 + +结束一个有效番茄采摘 episode 前，操作者应先请求打开夹爪，等待日志/状态确认 +逻辑状态已经变为 `open`，再松开 Grip 并按 B。结束时夹爪命令仍在执行，保存状态 +最多等待 3 秒；失败或超时则拒绝。最终裁剪后的数据若没有包含已确认的打开状态， +同样拒绝，不能把保存后的成功状态回填到更早样本中。 + +## 相机配置与采集 + +第一阶段固定使用两台已确定序列号的 RealSense： + +| ACT 名称 | 型号与位置 | 序列号 | +|---|---|---| +| `cam_high` | 全局 D455 | `234222303366` | +| `cam_right_wrist` | 右臂腕部 D405 | `412622272532` | + +两路图像参数统一为： + +```text +分辨率：640 × 480 +帧率：30 FPS +格式：RGB uint8 +HDF5 形状：(T, 480, 640, 3) +``` + +不采集深度、红外和点云，不使用 JPEG 压缩。采集线程直接请求 RealSense RGB8， +避免为颜色通道转换引入 OpenCV 依赖。 + +每台相机使用独立采集线程和一个很小的 `deque` 帧缓冲。每帧保存： + +- RealSense 帧号； +- RealSense 硬件时间戳； +- `wait_for_frames` 返回后立即读取的主机单调时间戳； +- RGB 数组。 + +两台设备的硬件时钟不能默认视为同一时钟域，因此正式对齐只使用同一主机的单调 +时钟；硬件时间戳只用于发现设备重启、帧号跳变和采集异常。 + +采集节点独占相机。指定设备缺失、型号/序列号不匹配、流配置失败或已经被其他 +进程占用时，预检失败并停留在 `IDLE`，不自动替换成其他相机。 + +## 30 Hz 采样与因果对齐 + +正式采样不使用独立的 30 Hz ROS 定时器。采集节点在首次有效 Grip 控制周期记录 +`sample_origin_seq`，随后只选择： + +```text +(control_seq - sample_origin_seq) % 3 == 0 +``` + +因此 90 Hz 控制消息按 `0、3、6、9...` 的相对序号形成名义 30 Hz 数据，同时保证 +第一个正式样本就是首次有效动作，而不是等待一个全局取模相位。 + +每个样本的定义为： + +```text +observation[t] + = 当前控制周期开始时读取的 q_actual + + 对每台相机选择主机时间戳不晚于该控制周期的最新帧 + +action[t] + = 同一控制周期经 QP、关节限速后成功发送的 q_target + 或 Grip 暂停/QP 回退时明确定义的上次成功目标 +``` + +采集节点收到消息时，相机缓冲中可能已经存在晚于控制周期的帧，因此不能简单取 +“回调时最新帧”，必须按 `host_monotonic_ns <= control_monotonic_ns` 选择最近帧。 +不存在满足条件且年龄不超过 50 ms 的帧时，当前 episode 拒绝。 + +不采用官方旧加载器中的 `action[t-1]` 补丁。HDF5 根属性写入： + +```text +action_alignment = "same_step_causal" +``` + +控制、反馈、动作和图像源时间戳全部保存在 `/debug`，未来只有在真实延迟测量证明 +存在稳定偏移时，才在训练加载器中调整；原始 HDF5 不进行不可逆移位。 + +## Episode 边界与手柄状态机 + +### 按键映射 + +- 右手 B，即右手 `secondary` 单击：开始准备或结束保存； +- 左手 Y，即左手 `secondary` 长按 1 秒：丢弃当前准备/录制； +- 右手 A，即右手 `primary`：继续保持现有右臂回初始位姿功能； +- Grip：继续只控制遥操离合，不作为“只在按下时才记录”的采集开关。 + +右手 B 在右手 Grip 按下时始终忽略，避免运动中误触开始或结束。左手 Y 仅在 +`ARMED` 或 `RECORDING` 中长按有效，在 `IDLE` 中无作用，且永远不删除上一个已经 +保存的 episode。 + +### 状态机 + +```text +IDLE + └─ Grip 松开时单击右手 B + ├─ 预检失败 → IDLE + └─ 预检通过 → ARMED + +ARMED + ├─ 第一次 Grip 有效动作 → RECORDING + ├─ 再次单击右手 B → 取消 → IDLE + ├─ 长按左手 Y 1 秒 → DISCARDED → IDLE + └─ 按右手 A → 取消 → IDLE + +RECORDING + ├─ Grip 松开后单击右手 B → SAVING + ├─ 长按左手 Y 1 秒 → DISCARDED → IDLE + ├─ 按右手 A → REJECTED → IDLE + └─ 硬质量故障/60 秒上限/Ctrl+C → REJECTED → IDLE + +SAVING + ├─ 质量检查通过 → SAVED → IDLE + └─ 质量检查失败 → REJECTED → IDLE +``` + +`IDLE`、`ARMED`、`RECORDING`、`SAVING` 是运行状态；`SAVED`、`DISCARDED`、 +`REJECTED` 是短暂结果状态，发布一次结果并输出日志后回到 `IDLE`。`SAVING` +期间忽略 B/Y 录制按键，A 键仍属于原有遥操逻辑，但不会再进入已经结束的数据。 + +状态通过 `std_msgs/msg/String` 话题 `/act/recording_status` 和终端日志报告，不增加 +新状态消息、声音或震动接口。 + +### 连续记录与 Grip 暂停 + +正式时间轴从 `ARMED` 后 Grip 按下且第一次 +`action_valid=true、command_sent=true` 的控制周期开始。Grip 刚按下的建基准周期 +尚未向 RM75 发送新的 CANFD 目标，因此不作为第一个训练样本。 +录制过程中临时松开 Grip 时仍以 30 Hz 保存图像和 `qpos`，`action` 零阶保持上次 +成功目标，并记录 `teleop_active=false`： + +- 松开后重新按 Grip：暂停区间保留，继续同一个 episode； +- 最后一次松开后按 B：将该次松开至 B 之间的纯操作等待数据裁掉，episode 结束在 + 最后一次 Grip 松开附近； +- 夹爪打开确认必须已经包含在裁剪终点之前，否则拒绝该 episode。 + +只在 Grip 按下时保存数据会丢失接近任务开始、暂停恢复和完整视觉上下文，因此不 +采用该方案。 + +### Episode 是否包含 A 键回位 + +一个正式 episode 只包含： + +```text +初始位姿、夹爪打开 +→ 接近番茄 +→ 闭合夹爪 +→ 搬运至收集篮 +→ 打开夹爪并确认成功 +→ 松开 Grip +→ 按 B 结束 +``` + +A 键的 `rm_movej(initial_joint_pose)` 必须在成功结束 episode 后执行，不写进 +episode。这样 ACT 始终学习同一种逐周期 `q_target` 动作语义。未来实时推理若要 +连续采摘，应由上层状态机执行： + +```text +ACT 完成一次采摘 → 完成判定 → 固定 rm_movej 复位 → 下一次 ACT 采摘 +``` + +若在 `RECORDING` 中误按 A，当前文件转入拒绝目录，原因写为： + +```text +initial_pose_command_during_episode +``` + +拒绝数据不会拦截 A 键原有回位动作，也不会额外控制机器人。 + +## HDF5 核心结构 + +数据根目录和任务目录固定为： + +```text +/home/robot/ACT_Data +/home/robot/ACT_Data/tomato_pick +``` + +正式文件核心结构： + +```text +/observations/qpos float32 (T, 8) +/observations/images/cam_high uint8 (T, 480, 640, 3) +/observations/images/cam_right_wrist uint8 (T, 480, 640, 3) +/action float32 (T, 8) +/debug/... +``` + +`T` 是该次任务的实际样本数，不要求所有 episode 等长，不进行文件内 padding。 +最短有效 episode 为 `60` 个样本，即 2 秒；最长为 `1800` 个样本，即 60 秒。 + +8 维字段顺序固定为： + +```text +qpos[0:7] = RM75 实际反馈关节角，单位 rad +qpos[7] = 已确认夹爪逻辑状态，0 closed、1 open + +action[0:7] = 最终成功下发或明确定义为保持的关节目标，单位 rad +action[7] = 夹爪请求目标，0 closed、1 open +``` + +根属性至少包括： + +| 属性 | 值或语义 | +|---|---| +| `sim` | `false`，与 ALOHA 真实数据约定一致 | +| `task_name` | `tomato_pick` | +| `sample_rate_hz` | `30` | +| `action_alignment` | `same_step_causal` | +| `arm` | `right_rm75` | +| `episode_status` | `saved` 或 `rejected` | +| `camera_high_serial` | `234222303366` | +| `camera_right_wrist_serial` | `412622272532` | +| `joint_names` | 7 个 RM75 关节名和 `gripper` 的固定顺序 | +| `joint_lower_limits` | 来自当前 Placo/URDF 的 7 关节下限 | +| `joint_upper_limits` | 来自当前 Placo/URDF 的 7 关节上限 | +| `reject_reason` | 仅拒绝文件存在 | +| `interrupted` | 正常文件为 `false`，Ctrl+C/异常恢复为 `true` | + +图像不压缩，每帧使用一个 HDF5 chunk；数值数据使用可扩展的一维时间轴并分块 +写入。按两路 `640×480×3×30` 计算，图像数据约为 3.3 GB/分钟，因此不能把完整 +episode 先缓存到内存再一次性保存。 + +不创建 `/observations/qvel`、`/observations/effort`、压缩标记或 `compress_len`； +也不使用零值、有限差分或其他伪数据填充缺失字段。后续训练加载器按存在的核心 +字段读取。 + +## Debug 结构 + +自定义消息是运行时传输载体，进程退出后不会保留；HDF5 `/debug` 是永久诊断记录。 +ACT 训练默认不读取该组。 + +建议使用以下精简结构，布尔状态以 `uint8` 保存： + +```text +/debug/timestamps/control_monotonic_ns int64 (T,) +/debug/timestamps/feedback_monotonic_ns int64 (T,) +/debug/timestamps/action_monotonic_ns int64 (T,) +/debug/timestamps/cam_high_host_monotonic_ns int64 (T,) +/debug/timestamps/cam_wrist_host_monotonic_ns int64 (T,) +/debug/timestamps/cam_high_hardware_ms float64 (T,) +/debug/timestamps/cam_wrist_hardware_ms float64 (T,) +/debug/timestamps/cam_high_age_ms float32 (T,) +/debug/timestamps/cam_wrist_age_ms float32 (T,) +/debug/timestamps/inter_camera_skew_ms float32 (T,) + +/debug/cameras/cam_high_frame_number uint64 (T,) +/debug/cameras/cam_wrist_frame_number uint64 (T,) + +/debug/control/control_seq uint64 (T,) +/debug/control/teleop_active uint8 (T,) +/debug/control/action_valid uint8 (T,) +/debug/control/command_sent uint8 (T,) +/debug/control/target_clamped uint8 (T,) +/debug/control/control_fault uint8 (T,) + +/debug/qp/raw_target float32 (T, 7) +/debug/qp/attempted uint8 (T,) +/debug/qp/success uint8 (T,) +/debug/qp/duration_ms float32 (T,) + +/debug/tcp/current_pose float32 (T, 7) +/debug/tcp/raw_target_pose float32 (T, 7) +/debug/tcp/final_target_pose float32 (T, 7) +/debug/tcp/command_velocity float32 (T, 6) + +/debug/pico/right_pose float32 (T, 7) +/debug/pico/right_inputs float32 (T, 6) +/debug/pico/left_secondary uint8 (T,) + +/debug/gripper/target_open uint8 (T,) +/debug/gripper/state_open uint8 (T,) +/debug/gripper/command_pending uint8 (T,) +/debug/gripper/command_failed uint8 (T,) +``` + +位姿顺序统一为 `[x, y, z, qx, qy, qz, qw]`，TCP 速度顺序统一为 +`[vx, vy, vz, wx, wy, wz]`。`right_inputs` 顺序在文件属性中写明。当前周期没有发送 +动作时，`action_monotonic_ns=-1`，并以 `command_sent=false` 消除歧义。 + +episode 根属性额外保存 QP 失败次数、失败占比、最长连续失败次数、目标限位次数、 +相机帧率、丢帧率和最大时间偏差等汇总指标。 + +## 数据质量规则 + +### 开始前预检 + +Grip 松开时单击 B 后，采集节点检查： + +- 输出目录存在或可以创建且可写； +- 可用空间不少于 4 GiB，约为 60 秒原始图像估算值的 1.2 倍； +- 两台指定相机均在线、已经连续预热 5 秒且当前帧率合格； +- 最近 `q_actual` 合法，反馈年龄不超过 50 ms，RM75 无掉使能或控制故障； +- 右臂夹爪初始化打开命令已经成功； +- 左右 PICO 话题均在现有手柄超时范围内保持新鲜； +- 没有第二个采集进程持有任务目录锁或相机设备； +- 启动组合是 `arm:=right use_mock:=false record_act:=true`。 + +任一预检失败时输出明确原因并停留在 `IDLE`，不生成空文件，也不改变机器人状态。 + +### 硬拒绝条件 + +以下任一情况使当前 episode 进入 `REJECTED`： + +- 样本少于 60 或达到 60 秒上限； +- 控制周期序列缺失、有效平均采样率低于 27 Hz 或相邻样本间隔超过 100 ms； +- `qpos/action` 不是 `(T,8)`、包含 NaN/Inf、违反配置关节限制或夹爪值不是 + `0/1`； +- `q_actual` 年龄超过 50 ms，反馈超时、掉使能或出现控制故障； +- 最终关节动作发送失败，或消息表示的反馈和动作不属于同一控制周期； +- 夹爪命令失败、超时，或最终裁剪数据没有包含已确认的打开状态； +- 任一路相机平均帧率低于 27 FPS； +- 任一路相机硬件帧号丢失率超过 1%，或采样后的重复/跳帧比例超过 1%； +- 任一采样图像年龄超过 50 ms，或两路图像主机时间差超过 50 ms； +- 图像形状、数据类型或 RGB 通道约定错误； +- 录制中按 A； +- HDF5 写入失败、磁盘空间不足或有界写入队列持续积压； +- Ctrl+C、采集节点异常退出或启动时恢复崩溃残留文件。 + +采集节点的拒绝只处理数据，不额外发送停止或运动命令。若原因来自控制故障，仍由 +现有遥操安全链路执行原有安全停止。 + +### 允许但记录告警的情况 + +QP 求解偶发失败时，现有逻辑保留上次有效关节目标。只要该保持目标最终成功发送、 +反馈和其他质量规则正常，就不自动拒绝 episode，而是保存： + +- QP 失败样本数； +- 失败占比； +- 最长连续失败样本数； +- 每个样本的 `qp_success`。 + +工作空间限位、TCP 步长限制或关节速度/加速度限制生效同样只记录，不自动拒绝。 +这些限制是正常安全控制的一部分。 + +## 文件编号、保存、拒绝与恢复 + +正式编号只扫描任务目录根部的 `episode_<数字>.hdf5`，取最大编号加一： + +```text +已有 episode_0.hdf5 ... episode_9.hdf5 +重启后下一个正式文件仍为 episode_10.hdf5 +``` + +不填补编号空洞，绝不覆盖已有正式文件。任务目录使用标准库文件锁保证同一时刻只有 +一个采集进程分配编号和写入；临时文件与目标文件位于同一文件系统，检查通过后使用 +不覆盖已有目标的原子发布方式。 + +文件生命周期： + +```text +录制中： +/home/robot/ACT_Data/tomato_pick/episode_10.partial.hdf5 + +检查通过： +/home/robot/ACT_Data/tomato_pick/episode_10.hdf5 + +检查失败： +/home/robot/ACT_Data/tomato_pick/rejected/ + episode_10__.hdf5 +``` + +- 只有正式保存成功才消耗编号； +- 手动长按 Y 丢弃时关闭并删除当前临时文件，不生成拒绝文件； +- 普通质量拒绝转入 `rejected/`，不消耗正式编号； +- Ctrl+C 时尽力关闭可读 HDF5，根属性写入 `interrupted=true`，文件名原因使用 + `interrupted`； +- 下次启动先处理遗留临时文件：可读文件转入 `rejected/` 并标记 + `crash_recovered`，不可读文件改成带时间戳的 `.partial.hdf5` 保留； +- 拒绝原因同时写入文件名和根属性； +- 任一步出现目标文件冲突时停止保存并报警，不能覆盖或自动删除已有 episode。 + +写入采用单独工作线程和有界队列，ROS 回调只完成取样、对齐和入队。队列容量只需 +覆盖短暂磁盘抖动，不能无限增长掩盖磁盘吞吐不足；持续积压时拒绝数据。 + +## 启动、配置与依赖 + +继续使用唯一入口 `xr_rm_bringup/launch/arm_debug.launch.py`，新增参数： + +```text +record_act:=false +``` + +默认 `false`，现有 mock、单臂、双臂和 MuJoCo 启动行为不变。第一阶段唯一允许的 +采集组合为： + +```bash +ros2 launch xr_rm_bringup arm_debug.launch.py \ + arm:=right use_mock:=false record_act:=true +``` + +`record_act:=true` 配合 `arm:=left|both` 或 `use_mock:=true` 时，在 launch 参数校验 +阶段明确拒绝。ACT 采集节点退出不触发整个 launch 的 `Shutdown`，保证数据进程故障 +不会终止遥操；终端必须清晰显示采集已经不可用。 + +新增一份专用 YAML，集中保存： + +- 数据根目录和任务名； +- 两个相机序列号、分辨率和帧率； +- 30 Hz 采样率、2 秒最短时长和 60 秒最长时长； +- 相机、反馈、磁盘和时间对齐质量阈值； +- PICO 左右话题、原子采样话题和状态话题。 + +硬件相关阈值保留为配置项，核心 HDF5 字段顺序和 8 维语义固定，不为未来可能的 +变体增加插件或通用框架。 + +采集节点继续由 `/home/robot/miniconda3/envs/xr/bin/python` 启动。复用该环境已有的 +`pyrealsense2` 和 NumPy，只在该环境增加 HDF5 必需依赖 `h5py`；不修改系统 Python， +不新增 Conda 环境，也不增加 OpenCV 依赖。依赖缺失时节点应在打开相机或创建文件前 +给出明确错误。 + +## 最小代码范围 + +预计只修改或新增以下位置： + +- `xr_rm_interfaces/msg/ActControlSample.msg`：原子消息； +- `xr_rm_interfaces/CMakeLists.txt`、`package.xml`：生成并导出消息； +- `xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py`：发布原子样本、跟踪夹爪 + 请求/成功状态和 A 键事件； +- `xr_rm_teleop/xr_rm_teleop/fun_peripheral.py`：把请求的初始工具状态明确改为完全 + 打开； +- `xr_rm_teleop/xr_rm_teleop/act_episode_recorder.py`：独立采集节点； +- `xr_rm_teleop/setup.py`、`package.xml`：安装入口和 ROS 运行依赖声明； +- `xr_rm_bringup/config/act_tomato_pick.yaml`：采集与硬件参数； +- `xr_rm_bringup/config/peripherals_rm75.yaml`：只为右臂启用初始化打开； +- `xr_rm_bringup/launch/arm_debug.launch.py`：默认关闭的 `record_act` 启动分支； +- 现有测试目录中的最小相关测试。 + +不改左臂/双臂运动参数，不改变 `left_arm_teleop`、`right_arm_teleop` 节点名，不创建 +第二个相机包、训练包或数据工具包。 + +## 测试与验收 + +### 自动化验证 + +测试全部使用 mock、假适配器、合成相机帧和临时目录，不连接真机、不移动机械臂、 +不操作真实夹爪： + +- 原子消息中的 `q_actual`、QP 输出和最终 `q_target` 来自同一控制周期； +- QP 失败时记录失败并保持上次目标，不自动拒绝； +- 发送失败、夹爪失败和 A 键误触触发拒绝； +- Trigger 请求立即改变 `action[7]`，成功返回后才改变 `qpos[7]`； +- B/Y/Grip 的边沿、长按、忽略条件和所有状态转换正确； +- 从首次有效样本开始按控制序号每 3 个周期取 1 个； +- 相机只选择不晚于控制时间的最新帧，并能发现过期、偏斜和帧号异常； +- Grip 中途暂停保留，最终松开到 B 的等待段正确裁剪； +- HDF5 核心路径、形状、dtype、8 维顺序、根属性和 debug 字段正确； +- 变量长度、最短/最长限制、质量拒绝、手动丢弃和 Ctrl+C 正确； +- 编号从最大正式编号加一，拒绝不占号，已有文件不被覆盖； +- 可读和不可读崩溃残留分别按设计恢复； +- `record_act` 默认关闭，非法 arm/mock 组合被 launch 拒绝； +- mock 模式不会导入或调用 RealMan SDK、RealSense 或 HDF5 采集链路。 + +按照仓库要求，构建和测试从工作空间根目录执行： + +```bash +cd /home/robot/WS_xr +source /opt/ros/humble/setup.bash +colcon build --symlink-install +pytest src/xr_rm_teleop/test/test_orientation_control.py +``` + +实施时还应运行新增测试及受影响的现有关节控制、初始位姿和外设配置测试。未看到 +实际通过输出前不得声称验证通过。 + +### 真机手工验收 + +真机验收必须由用户明确授权并在现有安全检查完成后进行，至少验证： + +1. 启动后右臂夹爪完全打开，状态未在命令成功前提前标记； +2. 两台相机序列号和画面角色正确，持续 30 FPS 左右； +3. B 开始、Grip 激活、B 结束、Y 丢弃和 A 误触拒绝符合状态机； +4. 正常采摘文件包含完整抓取、搬运和释放，不包含 A 键回位； +5. `qpos/action` 是有限的 `(T,8)` `float32`，图像是两路 + `(T,480,640,3)` `uint8`； +6. QP 短暂失败只增加 debug 计数，成功调整后仍可完成 episode； +7. 重启后编号继续递增，丢弃和拒绝不会覆盖或占用正式编号； +8. Ctrl+C、相机断流和磁盘不足产生带明确原因的拒绝文件； +9. ACT 采集节点退出后，遥操安全链路仍按现有行为运行。 + +## 后续训练与推理影响 + +本设计生成 ALOHA/ACT 风格的核心数据，但原始 ACT 代码通常把状态维度硬编码为 +双臂 14 维，并可能假设固定 episode 长度。训练阶段需要单独适配： + +- `state_dim=8`； +- 两个相机名 `cam_high`、`cam_right_wrist`； +- 变量长度 episode 的 padding 和 mask； +- `chunk_size≈60`； +- 不使用 `action[t-1]` 旧补丁； +- 忽略 `/debug`，除非用于筛选或诊断。 + +实时推理只负责从初始位姿执行一次采摘到释放。回初始位姿继续调用当前确定性的 +`rm_movej`，由未来的上层任务状态机协调，避免让一个低层策略混合两种动作接口和 +任务阶段。