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