28 KiB
右臂番茄采摘 ACT 数据采集适配设计
背景与目标
当前项目已经通过 ROS2 Humble、PICO 手柄、Placo QP 和 RealMan Python API2
完成 RM75 遥操作。右臂遥操节点以名义 90 Hz 读取实际关节反馈，根据 PICO
相对位姿生成 TCP 目标，经过工作空间限制、QP、关节速度与加速度限制后，通过
rm_movej_canfd 下发最终关节目标。
本次变更在不重写现有遥操链路的前提下，增加一个独立的 ALOHA/ACT 风格数据采集 节点。第一阶段只采集右臂番茄采摘任务，每个 episode 覆盖从初始位姿出发、抓取 番茄、搬运至 RM75 下方收集篮并释放番茄的完整过程。
核心目标如下：
- 以右臂 7 个实际关节角和夹爪逻辑状态作为
observations/qpos； - 以实际成功下发或零阶保持的 7 个最终关节目标和夹爪目标作为
action； - 同步采集一台全局 D455 和一台右腕 D405 的 RGB 图像；
- 以
30 Hz形成同周期因果对齐的数据； - 每个完整任务流式保存为一个 ALOHA/ACT 核心结构兼容的 HDF5 文件；
- 支持手柄开始、结束、丢弃、拒绝、质量检查、崩溃恢复和编号防覆盖；
- 保留足够的 PICO、TCP、QP、夹爪和时间戳调试数据，但不让采集节点进入机器人 控制链路。
参考实现为 ALOHA 官方仓库中的
record_episodes.py。
官方双臂数据使用 14 维状态和动作；本项目第一阶段采用右臂 7+1=8 维，因此只
保证 HDF5 核心组织方式兼容，不声称官方旧训练加载器可以不修改直接训练。
非目标
第一阶段明确不实现：
- 左臂或双臂 ACT 采集；
- ACT 训练代码、数据加载器、策略部署或自动完成判定；
- 深度图、红外图、点云、图像压缩或 ROS 图像话题；
- rosbag、中间格式、离线转换工具、GUI、声音或手柄震动反馈；
- 让 ACT 学习 A 键触发的回初始位姿运动；
- 修改现有工作空间限制、圆柱限制、速度限制、指令超时、安全停止或
move_to_initial_pose_on_connect默认值； - 新建重复的 ROS2 包或第二个 RealMan 连接。
后续训练时建议将 ACT chunk_size 设为 60，对应约 2 秒动作长度；该参数属于
训练配置，不写死在采集逻辑中。
现有控制链路与关键约束
现有 single_arm_velocity_teleop 在每个控制周期内依次完成：
读取最新 RM75 反馈
→ 同步 Placo 状态
→ 读取 PICO 状态并生成 TCP 目标
→ 工作空间、位姿步长与速度限制
→ Placo QP
→ 关节速度与加速度限制
→ rm_movej_canfd 下发
→ 发布 joint_target 调试话题
当前 joint_states、TCP 调试话题和 joint_target 分别发布并各自取时间戳。如果
采集节点仅订阅这些分散话题，即使时间接近，也可能把第 100 个控制周期的
q_actual 与第 99 个控制周期的 q_target 拼在一起。这里的“第 100 个周期”是
控制序号，不是 100 Hz；现有控制频率仍是名义 90 Hz。
此外，A 键回位调用的是一次阻塞式 rm_movej(initial_joint_pose)。轨迹由 RM75
控制器内部生成，项目不能获得每个控制周期的中间目标，也不经过当前 QP 和
rm_movej_canfd 链路，因此不能与遥操动作混用同一种标签语义。
总体架构
采用一个自定义原子控制采样消息和一个独立 ACT 采集节点：
PICO 输入 + RM75 反馈
↓
single_arm_velocity_teleop（90 Hz）
↓
QP → 关节限速/限加速度 → RM75 下发
↓
ActControlSample（同周期、同时间戳、同序号）
↓
act_episode_recorder（每 3 个控制周期取 1 个）
├──────────────┐
↓ ↓
全局 D455 RGB 右腕 D405 RGB
└──────┬───────┘
↓
30 Hz 流式写入临时 HDF5
↓
裁剪 → 质量检查 → 保存/拒绝/丢弃
“原子”表示消息是一个逻辑上不可拆分的控制周期快照。订阅者要么收到该周期完整的 反馈、求解结果、最终动作和状态，要么该周期整体缺失；不会自行拼接多个异步话题。
职责边界如下：
- 遥操节点继续唯一负责 RM75 连接、反馈、QP、限位、动作下发和安全停止；
- 遥操节点只增加原子消息发布和夹爪逻辑状态记录，不读取相机、不写 HDF5；
- 采集节点只订阅控制/PICO 数据、独占两台相机并写文件，不连接或控制 RM75；
- 采集节点异常、退出或写盘过慢不得阻塞遥操发布或改变机器人动作；
- 相机由采集节点通过
pyrealsense2直接打开，不再发布和重新订阅 ROS 图像。
原子消息使用本机低延迟、非阻塞的 best-effort QoS。采集节点检测任何需要保留的 控制周期丢失并拒绝 episode，而不是让 DDS 反压影响遥操控制。
原子控制采样消息
在 xr_rm_interfaces 中新增 ActControlSample.msg，发布话题为：
/xr_rm/right_rm75/act_control_sample
消息至少表达以下内容：
| 类别 | 字段语义 |
|---|---|
| 周期标识 | ROS header、control_seq、控制周期单调时间戳 |
| 关节反馈 | q_actual[7]、当前 URDF 关节上下限、反馈接收单调时间戳、反馈年龄、反馈有效状态 |
| QP | QP 原始输出或失败时的保持目标 q_qp_raw[7]、是否尝试、成功状态、耗时 |
| 最终动作 | 经关节限速后的 q_target[7]、动作时间戳、是否当前周期成功下发 |
| TCP | 当前 TCP、PICO 映射前的原始目标 TCP、最终受限目标 TCP、命令速度 |
| PICO | 当前右手位姿、Grip、Trigger、A、B 和摇杆值 |
| 夹爪 | 请求目标、已确认逻辑状态、命令是否处理中、命令是否失败 |
| 控制状态 | teleop_active、action_valid、QP 回退、目标限位和控制故障状态 |
数值型关节字段在 ROS 消息中保持双精度，写入 HDF5 核心数据时显式转换成
float32。位姿使用位置加四元数，不保存完整 Placo 对象、Hessian、约束矩阵或
其他大体积求解器内部状态。
消息在每个实际执行的 90 Hz 控制回调中发布，包括 Grip 松开和安全停止状态：
- 当前周期成功发送动作时，
command_sent=true； - 当前周期没有发送，但此前存在成功目标时，
q_target零阶保持上一个成功目标，command_sent=false、action_valid=true； - 尚未形成任何有效目标或当前动作发送失败时，
action_valid=false； - QP 失败但成功重发上次有效目标时，
qp_success=false、action_valid=true； - 发送失败必须发布失败状态，并由采集节点拒绝当前 episode。
相机数据不放进该消息。相机时间戳和帧号由采集节点在同一主机的单调时钟域中补充。
夹爪数据语义
右臂使用 Modbus 电动夹爪。由于不同番茄尺寸会导致实际停止开度不同，而本项目只 关心抓取意图，第一阶段不读取或估算实际开度。
统一约定：
0 = closed
1 = open
两类状态必须分开：
action[7]是目标状态，在 Trigger 产生开合请求的控制周期立即改变；qpos[7]是已确认逻辑状态，只有 Modbus 命令正常返回后才改变；- 命令失败时
qpos[7]保持原值，并拒绝当前 episode； - 采集前右臂夹爪必须成功初始化为完全打开
1.0，之后才允许把初始qpos[7]设为1； - 不再使用原先含义不明确的
0.75 → 0.15初始化序列。
结束一个有效番茄采摘 episode 前，操作者应先请求打开夹爪，等待日志/状态确认
逻辑状态已经变为 open，再松开 Grip 并按 B。结束时夹爪命令仍在执行，保存状态
最多等待 3 秒；失败或超时则拒绝。最终裁剪后的数据若没有包含已确认的打开状态，
同样拒绝，不能把保存后的成功状态回填到更早样本中。
相机配置与采集
第一阶段固定使用两台已确定序列号的 RealSense：
| ACT 名称 | 型号与位置 | 序列号 |
|---|---|---|
cam_high |
全局 D455 | 234222303366 |
cam_right_wrist |
右臂腕部 D405 | 412622272532 |
两路图像参数统一为：
分辨率：640 × 480
帧率：30 FPS
格式：RGB uint8
HDF5 形状：(T, 480, 640, 3)
不采集深度、红外和点云，不使用 JPEG 压缩。采集线程直接请求 RealSense RGB8， 避免为颜色通道转换引入 OpenCV 依赖。
每台相机使用独立采集线程和一个很小的 deque 帧缓冲。每帧保存：
- RealSense 帧号；
- RealSense 硬件时间戳；
wait_for_frames返回后立即读取的主机单调时间戳；- RGB 数组。
两台设备的硬件时钟不能默认视为同一时钟域，因此正式对齐只使用同一主机的单调 时钟；硬件时间戳只用于发现设备重启、帧号跳变和采集异常。
采集节点独占相机。指定设备缺失、型号/序列号不匹配、流配置失败或已经被其他
进程占用时，预检失败并停留在 IDLE，不自动替换成其他相机。
30 Hz 采样与因果对齐
正式采样不使用独立的 30 Hz ROS 定时器。采集节点在首次有效 Grip 控制周期记录
sample_origin_seq，随后只选择：
(control_seq - sample_origin_seq) % 3 == 0
因此 90 Hz 控制消息按 0、3、6、9... 的相对序号形成名义 30 Hz 数据，同时保证
第一个正式样本就是首次有效动作，而不是等待一个全局取模相位。
每个样本的定义为：
observation[t]
= 当前控制周期开始时读取的 q_actual
+ 对每台相机选择主机时间戳不晚于该控制周期的最新帧
action[t]
= 同一控制周期经 QP、关节限速后成功发送的 q_target
或 Grip 暂停/QP 回退时明确定义的上次成功目标
采集节点收到消息时，相机缓冲中可能已经存在晚于控制周期的帧，因此不能简单取
“回调时最新帧”，必须按 host_monotonic_ns <= control_monotonic_ns 选择最近帧。
不存在满足条件且年龄不超过 50 ms 的帧时，当前 episode 拒绝。
不采用官方旧加载器中的 action[t-1] 补丁。HDF5 根属性写入：
action_alignment = "same_step_causal"
控制、反馈、动作和图像源时间戳全部保存在 /debug，未来只有在真实延迟测量证明
存在稳定偏移时，才在训练加载器中调整；原始 HDF5 不进行不可逆移位。
Episode 边界与手柄状态机
按键映射
- 右手 B，即右手
secondary单击：开始准备或结束保存； - 左手 Y，即左手
secondary长按 1 秒：丢弃当前准备/录制； - 右手 A，即右手
primary：继续保持现有右臂回初始位姿功能； - Grip：继续只控制遥操离合，不作为“只在按下时才记录”的采集开关。
右手 B 在右手 Grip 按下时始终忽略，避免运动中误触开始或结束。左手 Y 仅在
ARMED 或 RECORDING 中长按有效，在 IDLE 中无作用，且永远不删除上一个已经
保存的 episode。
状态机
IDLE
└─ Grip 松开时单击右手 B
├─ 预检失败 → IDLE
└─ 预检通过 → ARMED
ARMED
├─ 第一次 Grip 有效动作 → RECORDING
├─ 再次单击右手 B → 取消 → IDLE
├─ 长按左手 Y 1 秒 → DISCARDED → IDLE
└─ 按右手 A → 取消 → IDLE
RECORDING
├─ Grip 松开后单击右手 B → SAVING
├─ 长按左手 Y 1 秒 → DISCARDED → IDLE
├─ 按右手 A → REJECTED → IDLE
└─ 硬质量故障/60 秒上限/Ctrl+C → REJECTED → IDLE
SAVING
├─ 质量检查通过 → SAVED → IDLE
└─ 质量检查失败 → REJECTED → IDLE
IDLE、ARMED、RECORDING、SAVING 是运行状态；SAVED、DISCARDED、
REJECTED 是短暂结果状态，发布一次结果并输出日志后回到 IDLE。SAVING
期间忽略 B/Y 录制按键，A 键仍属于原有遥操逻辑，但不会再进入已经结束的数据。
状态通过 std_msgs/msg/String 话题 /act/recording_status 和终端日志报告，不增加
新状态消息、声音或震动接口。
连续记录与 Grip 暂停
正式时间轴从 ARMED 后 Grip 按下且第一次
action_valid=true、command_sent=true 的控制周期开始。Grip 刚按下的建基准周期
尚未向 RM75 发送新的 CANFD 目标，因此不作为第一个训练样本。
录制过程中临时松开 Grip 时仍以 30 Hz 保存图像和 qpos，action 零阶保持上次
成功目标，并记录 teleop_active=false：
- 松开后重新按 Grip：暂停区间保留，继续同一个 episode；
- 最后一次松开后按 B：将该次松开至 B 之间的纯操作等待数据裁掉，episode 结束在 最后一次 Grip 松开附近；
- 夹爪打开确认必须已经包含在裁剪终点之前，否则拒绝该 episode。
只在 Grip 按下时保存数据会丢失接近任务开始、暂停恢复和完整视觉上下文，因此不 采用该方案。
Episode 是否包含 A 键回位
一个正式 episode 只包含：
初始位姿、夹爪打开
→ 接近番茄
→ 闭合夹爪
→ 搬运至收集篮
→ 打开夹爪并确认成功
→ 松开 Grip
→ 按 B 结束
A 键的 rm_movej(initial_joint_pose) 必须在成功结束 episode 后执行，不写进
episode。这样 ACT 始终学习同一种逐周期 q_target 动作语义。未来实时推理若要
连续采摘，应由上层状态机执行：
ACT 完成一次采摘 → 完成判定 → 固定 rm_movej 复位 → 下一次 ACT 采摘
若在 RECORDING 中误按 A，当前文件转入拒绝目录，原因写为：
initial_pose_command_during_episode
拒绝数据不会拦截 A 键原有回位动作，也不会额外控制机器人。
HDF5 核心结构
数据根目录和任务目录固定为：
/home/robot/ACT_Data
/home/robot/ACT_Data/tomato_pick
正式文件核心结构：
/observations/qpos float32 (T, 8)
/observations/images/cam_high uint8 (T, 480, 640, 3)
/observations/images/cam_right_wrist uint8 (T, 480, 640, 3)
/action float32 (T, 8)
/debug/...
T 是该次任务的实际样本数，不要求所有 episode 等长，不进行文件内 padding。
最短有效 episode 为 60 个样本，即 2 秒；最长为 1800 个样本，即 60 秒。
8 维字段顺序固定为：
qpos[0:7] = RM75 实际反馈关节角，单位 rad
qpos[7] = 已确认夹爪逻辑状态，0 closed、1 open
action[0:7] = 最终成功下发或明确定义为保持的关节目标，单位 rad
action[7] = 夹爪请求目标，0 closed、1 open
根属性至少包括：
| 属性 | 值或语义 |
|---|---|
sim |
false，与 ALOHA 真实数据约定一致 |
task_name |
tomato_pick |
sample_rate_hz |
30 |
action_alignment |
same_step_causal |
arm |
right_rm75 |
episode_status |
saved 或 rejected |
camera_high_serial |
234222303366 |
camera_right_wrist_serial |
412622272532 |
joint_names |
7 个 RM75 关节名和 gripper 的固定顺序 |
joint_lower_limits |
来自当前 Placo/URDF 的 7 关节下限 |
joint_upper_limits |
来自当前 Placo/URDF 的 7 关节上限 |
reject_reason |
仅拒绝文件存在 |
interrupted |
正常文件为 false，Ctrl+C/异常恢复为 true |
图像不压缩，每帧使用一个 HDF5 chunk；数值数据使用可扩展的一维时间轴并分块
写入。按两路 640×480×3×30 计算，图像数据约为 3.3 GB/分钟，因此不能把完整
episode 先缓存到内存再一次性保存。
不创建 /observations/qvel、/observations/effort、压缩标记或 compress_len；
也不使用零值、有限差分或其他伪数据填充缺失字段。后续训练加载器按存在的核心
字段读取。
Debug 结构
自定义消息是运行时传输载体，进程退出后不会保留；HDF5 /debug 是永久诊断记录。
ACT 训练默认不读取该组。
建议使用以下精简结构，布尔状态以 uint8 保存：
/debug/timestamps/control_monotonic_ns int64 (T,)
/debug/timestamps/feedback_monotonic_ns int64 (T,)
/debug/timestamps/action_monotonic_ns int64 (T,)
/debug/timestamps/cam_high_host_monotonic_ns int64 (T,)
/debug/timestamps/cam_wrist_host_monotonic_ns int64 (T,)
/debug/timestamps/cam_high_hardware_ms float64 (T,)
/debug/timestamps/cam_wrist_hardware_ms float64 (T,)
/debug/timestamps/cam_high_age_ms float32 (T,)
/debug/timestamps/cam_wrist_age_ms float32 (T,)
/debug/timestamps/inter_camera_skew_ms float32 (T,)
/debug/cameras/cam_high_frame_number uint64 (T,)
/debug/cameras/cam_wrist_frame_number uint64 (T,)
/debug/control/control_seq uint64 (T,)
/debug/control/teleop_active uint8 (T,)
/debug/control/action_valid uint8 (T,)
/debug/control/command_sent uint8 (T,)
/debug/control/target_clamped uint8 (T,)
/debug/control/control_fault uint8 (T,)
/debug/qp/raw_target float32 (T, 7)
/debug/qp/attempted uint8 (T,)
/debug/qp/success uint8 (T,)
/debug/qp/duration_ms float32 (T,)
/debug/tcp/current_pose float32 (T, 7)
/debug/tcp/raw_target_pose float32 (T, 7)
/debug/tcp/final_target_pose float32 (T, 7)
/debug/tcp/command_velocity float32 (T, 6)
/debug/pico/right_pose float32 (T, 7)
/debug/pico/right_inputs float32 (T, 6)
/debug/pico/left_secondary uint8 (T,)
/debug/gripper/target_open uint8 (T,)
/debug/gripper/state_open uint8 (T,)
/debug/gripper/command_pending uint8 (T,)
/debug/gripper/command_failed uint8 (T,)
位姿顺序统一为 [x, y, z, qx, qy, qz, qw]，TCP 速度顺序统一为
[vx, vy, vz, wx, wy, wz]。right_inputs 顺序在文件属性中写明。当前周期没有发送
动作时，action_monotonic_ns=-1，并以 command_sent=false 消除歧义。
episode 根属性额外保存 QP 失败次数、失败占比、最长连续失败次数、目标限位次数、 相机帧率、丢帧率和最大时间偏差等汇总指标。
数据质量规则
开始前预检
Grip 松开时单击 B 后，采集节点检查：
- 输出目录存在或可以创建且可写；
- 可用空间不少于 4 GiB，约为 60 秒原始图像估算值的 1.2 倍；
- 两台指定相机均在线、已经连续预热 5 秒且当前帧率合格；
- 最近
q_actual合法，反馈年龄不超过 50 ms，RM75 无掉使能或控制故障； - 右臂夹爪初始化打开命令已经成功；
- 左右 PICO 话题均在现有手柄超时范围内保持新鲜；
- 没有第二个采集进程持有任务目录锁或相机设备；
- 启动组合是
arm:=right use_mock:=false record_act:=true。
任一预检失败时输出明确原因并停留在 IDLE，不生成空文件，也不改变机器人状态。
硬拒绝条件
以下任一情况使当前 episode 进入 REJECTED：
- 样本少于 60 或达到 60 秒上限；
- 控制周期序列缺失、有效平均采样率低于 27 Hz 或相邻样本间隔超过 100 ms；
qpos/action不是(T,8)、包含 NaN/Inf、违反配置关节限制或夹爪值不是0/1；q_actual年龄超过 50 ms，反馈超时、掉使能或出现控制故障；- 最终关节动作发送失败，或消息表示的反馈和动作不属于同一控制周期；
- 夹爪命令失败、超时，或最终裁剪数据没有包含已确认的打开状态；
- 任一路相机平均帧率低于 27 FPS；
- 任一路相机硬件帧号丢失率超过 1%，或采样后的重复/跳帧比例超过 1%；
- 任一采样图像年龄超过 50 ms，或两路图像主机时间差超过 50 ms；
- 图像形状、数据类型或 RGB 通道约定错误；
- 录制中按 A；
- HDF5 写入失败、磁盘空间不足或有界写入队列持续积压；
- Ctrl+C、采集节点异常退出或启动时恢复崩溃残留文件。
采集节点的拒绝只处理数据，不额外发送停止或运动命令。若原因来自控制故障，仍由 现有遥操安全链路执行原有安全停止。
允许但记录告警的情况
QP 求解偶发失败时，现有逻辑保留上次有效关节目标。只要该保持目标最终成功发送、 反馈和其他质量规则正常，就不自动拒绝 episode，而是保存：
- QP 失败样本数；
- 失败占比；
- 最长连续失败样本数；
- 每个样本的
qp_success。
工作空间限位、TCP 步长限制或关节速度/加速度限制生效同样只记录，不自动拒绝。 这些限制是正常安全控制的一部分。
文件编号、保存、拒绝与恢复
正式编号只扫描任务目录根部的 episode_<数字>.hdf5，取最大编号加一：
已有 episode_0.hdf5 ... episode_9.hdf5
重启后下一个正式文件仍为 episode_10.hdf5
不填补编号空洞，绝不覆盖已有正式文件。任务目录使用标准库文件锁保证同一时刻只有 一个采集进程分配编号和写入；临时文件与目标文件位于同一文件系统，检查通过后使用 不覆盖已有目标的原子发布方式。
文件生命周期：
录制中：
/home/robot/ACT_Data/tomato_pick/episode_10.partial.hdf5
检查通过：
/home/robot/ACT_Data/tomato_pick/episode_10.hdf5
检查失败：
/home/robot/ACT_Data/tomato_pick/rejected/
episode_10_<reason>_<timestamp>.hdf5
- 只有正式保存成功才消耗编号；
- 手动长按 Y 丢弃时关闭并删除当前临时文件，不生成拒绝文件；
- 普通质量拒绝转入
rejected/，不消耗正式编号； - Ctrl+C 时尽力关闭可读 HDF5，根属性写入
interrupted=true，文件名原因使用interrupted； - 下次启动先处理遗留临时文件：可读文件转入
rejected/并标记crash_recovered，不可读文件改成带时间戳的.partial.hdf5保留； - 拒绝原因同时写入文件名和根属性；
- 任一步出现目标文件冲突时停止保存并报警，不能覆盖或自动删除已有 episode。
写入采用单独工作线程和有界队列，ROS 回调只完成取样、对齐和入队。队列容量只需 覆盖短暂磁盘抖动，不能无限增长掩盖磁盘吞吐不足；持续积压时拒绝数据。
启动、配置与依赖
继续使用唯一入口 xr_rm_bringup/launch/arm_debug.launch.py，新增参数：
record_act:=false
默认 false，现有 mock、单臂、双臂和 MuJoCo 启动行为不变。第一阶段唯一允许的
采集组合为：
ros2 launch xr_rm_bringup arm_debug.launch.py \
arm:=right use_mock:=false record_act:=true
record_act:=true 配合 arm:=left|both 或 use_mock:=true 时，在 launch 参数校验
阶段明确拒绝。ACT 采集节点退出不触发整个 launch 的 Shutdown，保证数据进程故障
不会终止遥操；终端必须清晰显示采集已经不可用。
新增一份专用 YAML，集中保存：
- 数据根目录和任务名；
- 两个相机序列号、分辨率和帧率；
- 30 Hz 采样率、2 秒最短时长和 60 秒最长时长；
- 相机、反馈、磁盘和时间对齐质量阈值；
- PICO 左右话题、原子采样话题和状态话题。
硬件相关阈值保留为配置项，核心 HDF5 字段顺序和 8 维语义固定，不为未来可能的 变体增加插件或通用框架。
采集节点继续由 /home/robot/miniconda3/envs/xr/bin/python 启动。复用该环境已有的
pyrealsense2 和 NumPy，只在该环境增加 HDF5 必需依赖 h5py；不修改系统 Python，
不新增 Conda 环境，也不增加 OpenCV 依赖。依赖缺失时节点应在打开相机或创建文件前
给出明确错误。
最小代码范围
预计只修改或新增以下位置：
xr_rm_interfaces/msg/ActControlSample.msg：原子消息；xr_rm_interfaces/CMakeLists.txt、package.xml：生成并导出消息；xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py：发布原子样本、跟踪夹爪 请求/成功状态和 A 键事件；xr_rm_teleop/xr_rm_teleop/fun_peripheral.py：把请求的初始工具状态明确改为完全 打开；xr_rm_teleop/xr_rm_teleop/act_episode_recorder.py：独立采集节点；xr_rm_teleop/setup.py、package.xml：安装入口和 ROS 运行依赖声明；xr_rm_bringup/config/act_tomato_pick.yaml：采集与硬件参数；xr_rm_bringup/config/peripherals_rm75.yaml：只为右臂启用初始化打开；xr_rm_bringup/launch/arm_debug.launch.py：默认关闭的record_act启动分支；- 现有测试目录中的最小相关测试。
不改左臂/双臂运动参数，不改变 left_arm_teleop、right_arm_teleop 节点名，不创建
第二个相机包、训练包或数据工具包。
测试与验收
自动化验证
测试全部使用 mock、假适配器、合成相机帧和临时目录，不连接真机、不移动机械臂、 不操作真实夹爪：
- 原子消息中的
q_actual、QP 输出和最终q_target来自同一控制周期； - QP 失败时记录失败并保持上次目标，不自动拒绝；
- 发送失败、夹爪失败和 A 键误触触发拒绝；
- Trigger 请求立即改变
action[7]，成功返回后才改变qpos[7]； - B/Y/Grip 的边沿、长按、忽略条件和所有状态转换正确；
- 从首次有效样本开始按控制序号每 3 个周期取 1 个；
- 相机只选择不晚于控制时间的最新帧，并能发现过期、偏斜和帧号异常；
- Grip 中途暂停保留，最终松开到 B 的等待段正确裁剪；
- HDF5 核心路径、形状、dtype、8 维顺序、根属性和 debug 字段正确；
- 变量长度、最短/最长限制、质量拒绝、手动丢弃和 Ctrl+C 正确；
- 编号从最大正式编号加一，拒绝不占号，已有文件不被覆盖；
- 可读和不可读崩溃残留分别按设计恢复；
record_act默认关闭，非法 arm/mock 组合被 launch 拒绝；- mock 模式不会导入或调用 RealMan SDK、RealSense 或 HDF5 采集链路。
按照仓库要求，构建和测试从工作空间根目录执行：
cd /home/robot/WS_xr
source /opt/ros/humble/setup.bash
colcon build --symlink-install
pytest src/xr_rm_teleop/test/test_orientation_control.py
实施时还应运行新增测试及受影响的现有关节控制、初始位姿和外设配置测试。未看到 实际通过输出前不得声称验证通过。
真机手工验收
真机验收必须由用户明确授权并在现有安全检查完成后进行，至少验证：
- 启动后右臂夹爪完全打开，状态未在命令成功前提前标记；
- 两台相机序列号和画面角色正确，持续 30 FPS 左右；
- B 开始、Grip 激活、B 结束、Y 丢弃和 A 误触拒绝符合状态机；
- 正常采摘文件包含完整抓取、搬运和释放，不包含 A 键回位；
qpos/action是有限的(T,8)float32，图像是两路(T,480,640,3)uint8；- QP 短暂失败只增加 debug 计数，成功调整后仍可完成 episode；
- 重启后编号继续递增，丢弃和拒绝不会覆盖或占用正式编号；
- Ctrl+C、相机断流和磁盘不足产生带明确原因的拒绝文件；
- ACT 采集节点退出后，遥操安全链路仍按现有行为运行。
后续训练与推理影响
本设计生成 ALOHA/ACT 风格的核心数据，但原始 ACT 代码通常把状态维度硬编码为 双臂 14 维，并可能假设固定 episode 长度。训练阶段需要单独适配：
state_dim=8；- 两个相机名
cam_high、cam_right_wrist； - 变量长度 episode 的 padding 和 mask；
chunk_size≈60；- 不使用
action[t-1]旧补丁； - 忽略
/debug，除非用于筛选或诊断。
实时推理只负责从初始位姿执行一次采摘到释放。回初始位姿继续调用当前确定性的
rm_movej，由未来的上层任务状态机协调，避免让一个低层策略混合两种动作接口和
任务阶段。