# 右臂番茄采摘 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
中断文件。任何真机异常继续由现有安全停止流程处理。