YikaiFu-cart/acRealman_xr
1
1
Fork 1
Code Issues Pull Requests Packages Projects Releases Wiki Activity
Files
xr_rm_qp
acRealman_xr/docs/superpowers/plans/2026-08-10-act-tomato-pick-data-collection.md
T

52 KiB
Raw Permalink Blame History

右臂番茄采摘 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 保持单文件，因为这些逻辑只服务一个节点；只提取可独立测试的小型数据类和纯函数，不创建通用采集框架、工厂或插件层。

统一执行约定

所有构建和测试命令必须从工作空间根目录执行：

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 使用该文件现有的求解器构造辅助方式，增加：

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：运行测试并确认失败

运行：

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 的现有属性旁增加：

@property
def joint_position_limits(self) -> np.ndarray:
    return self._joint_limits.copy()
  • 步骤 4：定义原子消息

创建 ActControlSample.msg，内容固定为：

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：构建接口并检查生成结果

运行：

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
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 增加对部署配置的断言：

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 调用，核心断言为：

assert calls == [(1.0, 1, 1)]

其中三项依次表示 percent、device、scissorgripper，不能出现 0.75 或 0.15。

  • 步骤 2：运行测试并确认失败
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 的初始化分支替换成一次调用：

if set_initial_tool_state:
    set_tool_position(
        robot,
        percent=1.0,
        device=1,
        scissorgripper=scissorgripper,
    )

在 peripherals_rm75.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 初始化的遥操对象和假适配器，验证：

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：运行夹爪状态测试并确认失败
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：在线程锁内记录夹爪状态

在遥操节点初始化时增加：

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 中完成。

提供一个只读快照方法，后续原子消息复用：

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：运行相关测试
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
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，覆盖三个关键行为：

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：运行测试并确认失败
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：增加仅供单周期使用的上下文数据类

在遥操模块内增加私有数据类，默认值确保任何提前返回路径也能发布完整消息：

@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)，包装器只负责周期号和最终发布：

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 方法改为返回目标和成功标志：

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。构造方法必须：

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 发布异常中断机器人控制：

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：运行原子样本和受影响控制测试
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
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 秒上限。核心测试：

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：运行测试并确认失败
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：实现最小状态类型和控制决策

在新模块中定义：

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 边沿，最后用：

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：

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：运行状态机测试
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
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：写帧选择和相机质量测试

增加纯数据测试：

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：运行测试并确认失败
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：实现线程安全的小缓冲和选择函数
@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 流：

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：运行相机纯逻辑测试
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
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

/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 个合成样本，验证：

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 测试并确认失败
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：实现固定数据布局和流式追加

定义所有数据集，不允许调用方任意创建路径。核心布局：

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、夹爪，并增加质量 检查所需的两个相机帧号：

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 图像。元数据类型固定为：

@dataclass(frozen=True)
class EpisodeMetadata:
    joint_names: tuple[str, ...]
    joint_lower_limits: np.ndarray
    joint_upper_limits: np.ndarray

EpisodeStore.create 必须一次写入：

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：写编号、锁、正式发布和恢复测试
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) 持有任务目录锁。编号只匹配：

EPISODE_PATTERN = re.compile(r"^episode_(\d+)\.hdf5$")

临时文件关闭后使用同文件系统硬链接实现不覆盖发布：

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、编号与恢复测试
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
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 样本的最小合法文件，再逐项破坏副本。参数化断言至少包含：

@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：运行质量测试并确认失败
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：实现质量配置和稳定报告类型
@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 的样本中统计失败：

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：运行全部质量测试
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
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 和直接调用回调的方法，覆盖：

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：运行集成测试并确认失败
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 中的全部参数，创建：

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：实现样本构造和有界写入线程

收到被状态机选中的控制消息时：

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 回调中等待磁盘。结束时：

停止接受新样本
→ 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。结果状态方法统一：

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 增加：

"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：运行采集节点全部合成测试
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
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 默认值和组合校验测试

在现有测试中增加：

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 测试并确认失败
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：

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 分支

声明默认关闭参数并增加校验：

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 测试和参数展示
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
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：运行格式和变更范围检查

cd /home/robot/WS_xr/src
git diff --check
git status --short

预期：git diff --check 无输出；状态只包含本规格和计划列出的文件，不包含 build/、install/、log/ 或无关格式化改动。

  • 步骤 2：运行全部相关 Python 测试
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：从工作空间根目录完成全量构建
cd /home/robot/WS_xr
source /opt/ros/humble/setup.bash
colcon build --symlink-install

预期：所有包构建成功，源码目录中不生成 build/、install/、log/。

  • 步骤 4：运行不连接硬件的接口与启动检查
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 断言：

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 暴露了本功能范围内的问题，先补失败测试、做最小修正并重跑对应 验证，再提交：

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：向用户交接真机手工验收命令，不自行执行

只有用户明确授权连接真机后，才可运行：

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

Reference in New Issue View Git Blame Copy Permalink