fix: 拒绝ACT episode编号冲突

docs/superpowers/specs/2026-08-10-act-tomato-pick-data-collection-design.md

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

xr_rm_bringup/config/act_tomato_pick.yaml

@@ -0,0 +1,33 @@
+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

xr_rm_bringup/config/peripherals_rm75.yaml

@@ -27,3 +27,4 @@ arms:
     scissorgripper: 0
   right:
     scissorgripper: 1
+    set_initial_tool_state: true

xr_rm_bringup/launch/arm_debug.launch.py

@@ -80,6 +80,29 @@ def _validate_mujoco_mode(arm: str, use_mujoco: bool) -> None:
         raise ValueError("use_mujoco:=true requires arm:=both")
 
 
+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"
+        )
+
+
+def _act_recorder_node() -> Node:
+    """创建独立 ACT 数据采集节点；退出时不终止遥操作。"""
+    return Node(
+        package="xr_rm_teleop",
+        executable="act_episode_recorder",
+        name="act_episode_recorder",
+        output="screen",
+        prefix=[XR_PYTHON],
+        parameters=[_config_file("act_tomato_pick.yaml")],
+    )
+
+
 def _single_arm_node(
     arm: str,
     use_mock: bool,
@@ -164,10 +187,14 @@ def _launch_setup(context, *args, **kwargs):
     use_mujoco = _as_bool(
         LaunchConfiguration("use_mujoco").perform(context)
     )
+    record_act = _as_bool(
+        LaunchConfiguration("record_act").perform(context)
+    )
 
     if arm not in ("left", "right", "both"):
         raise ValueError("arm must be one of: left, right, both")
     _validate_mujoco_mode(arm, use_mujoco)
+    _validate_act_mode(arm, use_mock, record_act)
 
     nodes = [_udp_receiver_node()]
     if arm == "both":
@@ -176,6 +203,8 @@ def _launch_setup(context, *args, **kwargs):
         nodes.append(_single_arm_node(arm, use_mock))
     if use_mujoco:
         nodes.append(_mujoco_node())
+    if record_act:
+        nodes.append(_act_recorder_node())
     return nodes
 
 
@@ -187,6 +216,8 @@ def generate_launch_description() -> LaunchDescription:
         DeclareLaunchArgument("use_mock", default_value="true"),
         # true 时额外启动只读 MuJoCo 双臂显示，默认不改变现有启动行为。
         DeclareLaunchArgument("use_mujoco", default_value="false"),
+        # true 时只允许右臂真机，并启动独立 ACT 数据采集节点。
+        DeclareLaunchArgument("record_act", default_value="false"),
         # UDP 监听参数，需要与 PICO 端或 sample_udp_sender 保持一致。
         DeclareLaunchArgument("udp_host", default_value="0.0.0.0"),
         DeclareLaunchArgument("udp_port", default_value="15000"),

xr_rm_bringup/test/test_arm_debug_launch.py

@@ -41,3 +41,37 @@ def test_udp_receiver_exit_shuts_down_launch() -> None:
     receiver = arm_debug_launch._udp_receiver_node()
 
     assert isinstance(receiver._ExecuteLocal__on_exit, Shutdown)
+
+
+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)
+
+
+def test_act_recorder_exit_does_not_shutdown_teleoperation() -> None:
+    recorder = arm_debug_launch._act_recorder_node()
+
+    assert recorder._ExecuteLocal__on_exit is None

xr_rm_interfaces/CMakeLists.txt

@@ -11,6 +11,7 @@ find_package(rosidl_default_generators REQUIRED)
 find_package(std_msgs REQUIRED)
 
 rosidl_generate_interfaces(${PROJECT_NAME}
+  "msg/ActControlSample.msg"
   "msg/XrController.msg"
   DEPENDENCIES geometry_msgs std_msgs
 )

xr_rm_interfaces/msg/ActControlSample.msg

@@ -0,0 +1,41 @@
+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

xr_rm_teleop/setup.py

@@ -55,7 +55,9 @@ setup(
     tests_require=["pytest"],
     entry_points={
         "console_scripts": [
-            "single_arm_velocity_teleop = xr_rm_teleop.single_arm_velocity_teleop:main",
+            "act_episode_recorder = xr_rm_teleop.act_episode_recorder:main",
+            "single_arm_velocity_teleop = "
+            "xr_rm_teleop.single_arm_velocity_teleop:main",
         ],
     },
 )

xr_rm_teleop/test/test_act_control_sample.py

@@ -0,0 +1,194 @@
+from types import SimpleNamespace
+
+import numpy as np
+import pytest
+from builtin_interfaces.msg import Time as TimeMsg
+
+from xr_rm_interfaces.msg import XrController
+from xr_rm_teleop.single_arm_velocity_teleop import (
+    SingleArmVelocityTeleop,
+    _ActCycleContext,
+)
+
+
+class FakePublisher:
+    def __init__(self, error=None) -> None:
+        self.error = error
+        self.messages = []
+
+    def publish(self, message) -> None:
+        if self.error is not None:
+            raise self.error
+        self.messages.append(message)
+
+
+class FakeLogger:
+    def __init__(self) -> None:
+        self.warnings = []
+
+    def warn(self, message, **kwargs) -> None:
+        del kwargs
+        self.warnings.append(message)
+
+
+def _controller(*, grip=True) -> XrController:
+    message = XrController()
+    message.hand = "right"
+    message.grip = grip
+    message.trigger = 0.25
+    message.primary = False
+    message.secondary = False
+    message.axis = [0.1, -0.2]
+    message.pose.orientation.w = 1.0
+    return message
+
+
+def _teleop(*, last_target=None, tool_state=True):
+    teleop = object.__new__(SingleArmVelocityTeleop)
+    teleop._arm_name = "right_rm75"
+    teleop._last_msg = _controller()
+    teleop._active = True
+    teleop._control_fault_latched = False
+    teleop._last_current_pose = np.eye(4)
+    teleop._robot_start_transform = None
+    teleop._last_sent_target = None
+    teleop._last_sent_orientation = None
+    teleop._last_successful_action_target = last_target
+    teleop._ik_solver = SimpleNamespace(
+        joint_position_limits=np.asarray([[-1.0, 1.0]] * 7)
+    )
+    teleop._tool_state_snapshot = lambda: (
+        True,
+        tool_state,
+        False,
+        False,
+    )
+    teleop.get_clock = lambda: SimpleNamespace(
+        now=lambda: SimpleNamespace(to_msg=lambda: TimeMsg())
+    )
+    teleop.get_logger = lambda: FakeLogger()
+    return teleop
+
+
+def _cycle(**overrides) -> _ActCycleContext:
+    values = {
+        "control_seq": 100,
+        "control_monotonic_ns": 1_000_000_000,
+        "feedback_monotonic_ns": 990_000_000,
+        "action_monotonic_ns": 1_005_000_000,
+        "feedback_age_ms": 10.0,
+        "q_actual": [0.1] * 7,
+        "q_qp_raw": [0.3] * 7,
+        "q_target": [0.2] * 7,
+        "current_pose": np.eye(4),
+        "raw_target_pose": np.eye(4),
+        "target_pose": np.eye(4),
+        "command_velocity": [0.0] * 6,
+        "feedback_valid": True,
+        "command_sent": True,
+        "qp_attempted": True,
+        "qp_success": True,
+    }
+    values.update(overrides)
+    return _ActCycleContext(**values)
+
+
+def test_act_sample_uses_feedback_and_limited_target_from_one_cycle() -> None:
+    teleop = _teleop(last_target=[0.2] * 7)
+
+    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.joint_lower_limits == pytest.approx([-1.0] * 7)
+    assert message.joint_upper_limits == pytest.approx([1.0] * 7)
+    assert message.command_sent
+    assert message.action_valid
+    assert message.qp_attempted
+    assert message.qp_success
+
+
+def test_act_sample_marks_qp_fallback_as_valid_held_action() -> None:
+    teleop = _teleop(last_target=[0.2] * 7)
+    cycle = _cycle(
+        q_qp_raw=[0.2] * 7,
+        q_target=[0.2] * 7,
+        qp_success=False,
+    )
+
+    message = teleop._build_act_control_sample(cycle)
+
+    assert message.q_target == pytest.approx([0.2] * 7)
+    assert message.action_valid
+    assert message.qp_attempted
+    assert not message.qp_success
+
+
+def test_act_sample_holds_last_action_while_grip_is_released() -> None:
+    teleop = _teleop(last_target=[0.4] * 7)
+    teleop._last_msg = _controller(grip=False)
+    teleop._active = False
+    cycle = _cycle(
+        q_qp_raw=None,
+        q_target=None,
+        command_sent=False,
+        qp_attempted=False,
+        qp_success=False,
+        action_monotonic_ns=-1,
+    )
+
+    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
+    assert not message.teleop_active
+
+
+def test_act_sample_marks_send_failure_invalid() -> None:
+    teleop = _teleop(last_target=[0.4] * 7)
+
+    message = teleop._build_act_control_sample(
+        _cycle(send_failed=True, command_sent=False)
+    )
+
+    assert not message.action_valid
+
+
+def test_act_sample_marks_unknown_gripper_state() -> None:
+    teleop = _teleop(last_target=[0.2] * 7, tool_state=None)
+
+    message = teleop._build_act_control_sample(_cycle())
+
+    assert not message.gripper_state_known
+
+
+def test_act_sample_publish_failure_does_not_escape_control_path() -> None:
+    teleop = _teleop(last_target=[0.2] * 7)
+    logger = FakeLogger()
+    teleop._act_sample_pub = FakePublisher(RuntimeError("dds failed"))
+    teleop.get_logger = lambda: logger
+
+    teleop._publish_act_control_sample(_cycle())
+
+    assert logger.warnings == [
+        "right_rm75 ACT原子样本发布失败：dds failed"
+    ]
+
+
+def test_control_tick_wraps_one_impl_call_in_one_atomic_sample() -> None:
+    teleop = object.__new__(SingleArmVelocityTeleop)
+    cycles = []
+    published = []
+    teleop._act_control_seq = 7
+    teleop._control_tick_impl = lambda cycle: cycles.append(cycle)
+    teleop._publish_act_control_sample = lambda cycle: published.append(cycle)
+
+    teleop._control_tick()
+
+    assert len(cycles) == 1
+    assert published == cycles
+    assert cycles[0].control_seq == 7
+    assert teleop._act_control_seq == 8

xr_rm_teleop/test/test_act_episode_recorder.py

@@ -0,0 +1,882 @@
+import queue
+import threading
+from types import SimpleNamespace
+
+import numpy as np
+import pytest
+
+try:
+    import h5py
+except ImportError:
+    h5py = None
+
+from xr_rm_interfaces.msg import ActControlSample, XrController
+from xr_rm_teleop.act_episode_recorder import (
+    NO_ACTION,
+    ActEpisodeRecorder,
+    ButtonTracker,
+    CameraBuffer,
+    CameraFrame,
+    EpisodeMetadata,
+    EpisodeStore,
+    EpisodeWriter,
+    QualityError,
+    QualityLimits,
+    RecordingSession,
+    RecordingState,
+    TaskDirectoryLock,
+    discard_partial,
+    next_episode_index,
+    publish_without_overwrite,
+    recover_partial_files,
+    select_camera_pair,
+    select_frame,
+    validate_episode,
+)
+
+
+def _recording_session(*, origin_seq=10, max_samples=1800):
+    session = RecordingSession(max_samples=max_samples)
+    session.arm()
+    decision = session.on_control(
+        origin_seq,
+        grip=True,
+        action_valid=True,
+        command_sent=True,
+    )
+    assert decision.record_sample
+    return session
+
+
+def test_recording_starts_on_first_sent_grip_action_and_downsamples():
+    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,
+    )
+    session.on_control(
+        104,
+        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(11, grip=True, action_valid=True, command_sent=True)
+    session.on_control(12, grip=True, action_valid=True, command_sent=True)
+    session.on_control(13, grip=False, action_valid=True, command_sent=False)
+    first_crop = session.candidate_end_count
+
+    assert first_crop == 2
+
+    session.on_control(14, grip=True, action_valid=True, command_sent=False)
+    assert session.candidate_end_count is None
+    session.on_control(15, grip=True, action_valid=True, command_sent=True)
+    session.on_control(16, grip=False, action_valid=True, command_sent=False)
+
+    assert session.candidate_end_count == 3
+
+
+def test_finish_waits_for_control_sample_after_b_request():
+    session = _recording_session(origin_seq=10)
+    session.on_control(11, grip=False, action_valid=True, command_sent=False)
+    session.request_finish()
+
+    decision = session.on_control(
+        12,
+        grip=False,
+        action_valid=True,
+        command_sent=False,
+    )
+
+    assert decision.finish
+    assert session.candidate_end_count == 1
+
+
+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"
+
+
+def test_max_samples_rejects_without_stopping_robot():
+    session = _recording_session(origin_seq=10, max_samples=2)
+    session.on_control(11, grip=True, action_valid=True, command_sent=True)
+    session.on_control(12, 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 == "max_duration"
+
+
+def test_right_b_is_ignored_while_grip_is_pressed():
+    tracker = ButtonTracker(hold_ns=1_000_000_000)
+    tracker.on_right(False, False, False, 0, RecordingState.IDLE)
+
+    events = tracker.on_right(
+        False,
+        True,
+        True,
+        1,
+        RecordingState.IDLE,
+    )
+
+    assert not events.right_b
+
+
+def test_left_y_requires_new_press_inside_active_recording_state():
+    tracker = ButtonTracker(hold_ns=1_000_000_000)
+    tracker.on_left(True, 0, RecordingState.IDLE)
+    assert not tracker.on_left(
+        True,
+        2_000_000_000,
+        RecordingState.RECORDING,
+    ).discard
+    tracker.on_left(False, 2_100_000_000, RecordingState.RECORDING)
+    tracker.on_left(True, 3_000_000_000, RecordingState.RECORDING)
+
+    assert not tracker.on_left(
+        True,
+        3_999_999_999,
+        RecordingState.RECORDING,
+    ).discard
+    assert tracker.on_left(
+        True,
+        4_000_000_000,
+        RecordingState.RECORDING,
+    ).discard
+    assert not tracker.on_left(
+        True,
+        5_000_000_000,
+        RecordingState.RECORDING,
+    ).discard
+
+
+def test_right_a_reports_recording_rejection_event():
+    tracker = ButtonTracker(hold_ns=1_000_000_000)
+    tracker.on_right(False, False, False, 0, RecordingState.RECORDING)
+
+    events = tracker.on_right(
+        True,
+        False,
+        False,
+        1,
+        RecordingState.RECORDING,
+    )
+
+    assert events.reject_reason == "initial_pose_command_during_episode"
+
+
+def _image(value, *, shape=(480, 640, 3), dtype=np.uint8):
+    return np.full(shape, value, dtype=dtype)
+
+
+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_missing_old_and_invalid_images():
+    with pytest.raises(QualityError, match="camera_frame_missing"):
+        select_frame((), 1_000_000_000, 50.0)
+    with pytest.raises(QualityError, match="camera_frame_too_old"):
+        select_frame(
+            (CameraFrame(_image(1), 10, 100.0, 900_000_000),),
+            1_000_000_000,
+            50.0,
+        )
+    with pytest.raises(QualityError, match="camera_frame_format"):
+        select_frame(
+            (
+                CameraFrame(
+                    _image(1, shape=(10, 10, 3)),
+                    10,
+                    100.0,
+                    990_000_000,
+                ),
+            ),
+            1_000_000_000,
+            50.0,
+        )
+
+
+def test_select_camera_pair_rejects_inter_camera_skew():
+    high = (CameraFrame(_image(1), 10, 100.0, 990_000_000),)
+    wrist = (CameraFrame(_image(2), 20, 100.0, 930_000_000),)
+
+    with pytest.raises(QualityError, match="camera_skew"):
+        select_camera_pair(
+            high,
+            wrist,
+            1_000_000_000,
+            max_age_ms=100.0,
+            max_skew_ms=50.0,
+        )
+
+
+def test_camera_buffer_is_bounded_and_counts_dropped_frames():
+    buffer = CameraBuffer(maxlen=4)
+    for frame_number in (10, 11, 13, 14, 15):
+        buffer.push(
+            CameraFrame(
+                _image(frame_number),
+                frame_number,
+                float(frame_number),
+                frame_number * 1_000_000,
+            )
+        )
+
+    stats = buffer.stats()
+
+    assert [frame.frame_number for frame in buffer.snapshot()] == [
+        11,
+        13,
+        14,
+        15,
+    ]
+    assert stats.frame_count == 5
+    assert stats.dropped_frames == 1
+    assert stats.drop_ratio == pytest.approx(1.0 / 6.0)
+    assert stats.fps == pytest.approx(800.0)
+
+
+requires_h5py = pytest.mark.skipif(
+    h5py is None,
+    reason="h5py is not installed",
+)
+
+
+def _metadata():
+    return EpisodeMetadata(
+        joint_names=tuple(f"joint_{index}" for index in range(1, 8))
+        + ("gripper",),
+        joint_lower_limits=np.full(7, -3.0, dtype=np.float64),
+        joint_upper_limits=np.full(7, 3.0, dtype=np.float64),
+    )
+
+
+def _episode_sample(seq):
+    control_ns = seq * 11_111_111
+    qpos = np.zeros(8, dtype=np.float32)
+    qpos[-1] = 1.0
+    action = qpos.copy()
+    return {
+        "observations/qpos": qpos,
+        "action": action,
+        "observations/images/cam_high": _image(seq % 255),
+        "observations/images/cam_right_wrist": _image((seq + 1) % 255),
+        "debug/timestamps/control_monotonic_ns": control_ns,
+        "debug/timestamps/feedback_monotonic_ns": control_ns - 1_000_000,
+        "debug/timestamps/action_monotonic_ns": control_ns,
+        "debug/timestamps/cam_high_host_monotonic_ns": control_ns - 2_000_000,
+        "debug/timestamps/cam_wrist_host_monotonic_ns": control_ns - 3_000_000,
+        "debug/timestamps/cam_high_hardware_ms": float(seq),
+        "debug/timestamps/cam_wrist_hardware_ms": float(seq),
+        "debug/timestamps/cam_high_age_ms": 2.0,
+        "debug/timestamps/cam_wrist_age_ms": 3.0,
+        "debug/timestamps/inter_camera_skew_ms": 1.0,
+        "debug/cameras/cam_high_frame_number": seq,
+        "debug/cameras/cam_wrist_frame_number": seq,
+        "debug/control/control_seq": seq,
+        "debug/control/teleop_active": 1,
+        "debug/control/action_valid": 1,
+        "debug/control/command_sent": 1,
+        "debug/control/target_clamped": 0,
+        "debug/control/control_fault": 0,
+        "debug/qp/raw_target": np.zeros(7, dtype=np.float32),
+        "debug/qp/attempted": 1,
+        "debug/qp/success": 1,
+        "debug/qp/duration_ms": 1.0,
+        "debug/tcp/current_pose": np.zeros(7, dtype=np.float32),
+        "debug/tcp/raw_target_pose": np.zeros(7, dtype=np.float32),
+        "debug/tcp/final_target_pose": np.zeros(7, dtype=np.float32),
+        "debug/tcp/command_velocity": np.zeros(6, dtype=np.float32),
+        "debug/pico/right_pose": np.zeros(7, dtype=np.float32),
+        "debug/pico/right_inputs": np.zeros(6, dtype=np.float32),
+        "debug/pico/left_secondary": 0,
+        "debug/gripper/target_open": 1,
+        "debug/gripper/state_open": 1,
+        "debug/gripper/command_pending": 0,
+        "debug/gripper/command_failed": 0,
+    }
+
+
+@requires_h5py
+def test_episode_store_writes_act_core_schema(tmp_path):
+    store = EpisodeStore.create(
+        tmp_path / "episode_0.partial.hdf5",
+        _metadata(),
+    )
+    for seq in (100, 103, 106):
+        store.append(_episode_sample(seq))
+    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
+
+
+@requires_h5py
+def test_episode_store_truncates_every_time_axis_dataset(tmp_path):
+    store = EpisodeStore.create(
+        tmp_path / "episode_0.partial.hdf5",
+        _metadata(),
+    )
+    for seq in (100, 103, 106):
+        store.append(_episode_sample(seq))
+
+    store.truncate(2)
+    store.close()
+
+    lengths = []
+    with h5py.File(store.path, "r") as root:
+        root.visititems(
+            lambda _name, item: lengths.append(item.shape[0])
+            if isinstance(item, h5py.Dataset)
+            else None
+        )
+    assert lengths
+    assert set(lengths) == {2}
+
+
+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"
+    assert partial.read_bytes() == b"new"
+
+
+@requires_h5py
+def test_recover_marks_readable_and_preserves_unreadable_partial(tmp_path):
+    readable = tmp_path / "episode_1.partial.hdf5"
+    store = EpisodeStore.create(readable, _metadata())
+    store.append(_episode_sample(100))
+    store.close()
+    unreadable = tmp_path / "episode_2.partial.hdf5"
+    unreadable.write_bytes(b"not hdf5")
+
+    recovered = recover_partial_files(tmp_path, timestamp="20260810T120000")
+
+    assert len(recovered) == 2
+    rejected = tmp_path / "rejected" / (
+        "episode_1_crash_recovered_20260810T120000.hdf5"
+    )
+    assert rejected in recovered
+    with h5py.File(rejected, "r") as root:
+        assert root.attrs["episode_status"] == "rejected"
+        assert root.attrs["reject_reason"] == "crash_recovered"
+        assert root.attrs["interrupted"] == np.bool_(True)
+    assert not readable.exists()
+    assert any(path.name.endswith(".partial.hdf5") for path in recovered)
+    assert unreadable not in recovered
+    assert not unreadable.exists()
+
+
+def test_discard_partial_removes_only_current_file(tmp_path):
+    current = tmp_path / "episode_1.partial.hdf5"
+    current.write_bytes(b"current")
+    saved = tmp_path / "episode_0.hdf5"
+    saved.write_bytes(b"saved")
+
+    discard_partial(current)
+
+    assert not current.exists()
+    assert saved.read_bytes() == b"saved"
+    assert next_episode_index(tmp_path) == 1
+
+
+def test_task_directory_lock_rejects_second_recorder(tmp_path):
+    first = TaskDirectoryLock(tmp_path)
+    second = TaskDirectoryLock(tmp_path)
+    first.acquire()
+    try:
+        with pytest.raises(BlockingIOError):
+            second.acquire()
+    finally:
+        first.release()
+
+
+def _quality_limits():
+    return QualityLimits(min_samples=3, max_samples=100)
+
+
+def _valid_episode(tmp_path):
+    path = tmp_path / "episode_0.partial.hdf5"
+    store = EpisodeStore.create(path, _metadata())
+    for index, seq in enumerate((100, 103, 106)):
+        sample = _episode_sample(seq)
+        sample["debug/cameras/cam_high_frame_number"] = 200 + index
+        sample["debug/cameras/cam_wrist_frame_number"] = 300 + index
+        store.append(sample)
+    store.close()
+    with h5py.File(path, "r+") as root:
+        root.attrs["camera_high_fps"] = 30.0
+        root.attrs["camera_right_wrist_fps"] = 30.0
+        root.attrs["camera_high_drop_ratio"] = 0.0
+        root.attrs["camera_right_wrist_drop_ratio"] = 0.0
+    return path
+
+
+@requires_h5py
+def test_validate_episode_accepts_valid_file(tmp_path):
+    report = validate_episode(_valid_episode(tmp_path), _quality_limits())
+
+    assert report.accepted
+    assert report.reason is None
+    assert report.metrics["control_hz"] == pytest.approx(30.0, rel=1e-5)
+
+
+def _mutate_episode(path, mutation):
+    with h5py.File(path, "r+") as root:
+        if mutation == "short_episode":
+            root.visititems(
+                lambda _name, item: item.resize(2, axis=0)
+                if isinstance(item, h5py.Dataset)
+                else None
+            )
+        elif mutation == "control_seq_gap":
+            root["debug/control/control_seq"][1] = 104
+        elif mutation == "nonfinite_qpos":
+            root["observations/qpos"][1, 0] = np.nan
+        elif mutation == "joint_limit":
+            root["action"][1, 0] = 4.0
+        elif mutation == "invalid_gripper":
+            root["action"][1, 7] = 0.5
+        elif mutation == "feedback_age":
+            control_ns = root["debug/timestamps/control_monotonic_ns"][1]
+            root["debug/timestamps/feedback_monotonic_ns"][1] = (
+                control_ns - 60_000_000
+            )
+        elif mutation == "action_invalid":
+            root["debug/control/action_valid"][1] = 0
+        elif mutation == "control_fault":
+            root["debug/control/control_fault"][1] = 1
+        elif mutation == "camera_fps":
+            root.attrs["camera_high_fps"] = 20.0
+        elif mutation == "camera_drop":
+            root.attrs["camera_right_wrist_drop_ratio"] = 0.02
+        elif mutation == "camera_age":
+            root["debug/timestamps/cam_high_age_ms"][1] = 60.0
+        elif mutation == "camera_skew":
+            root["debug/timestamps/inter_camera_skew_ms"][1] = 60.0
+        elif mutation == "final_gripper_closed":
+            root["observations/qpos"][-1, 7] = 0.0
+        else:
+            raise AssertionError(f"unknown mutation: {mutation}")
+
+
+@requires_h5py
+@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(
+    tmp_path,
+    mutation,
+    reason,
+):
+    path = _valid_episode(tmp_path)
+    _mutate_episode(path, mutation)
+
+    report = validate_episode(path, _quality_limits())
+
+    assert not report.accepted
+    assert report.reason == reason
+
+
+@requires_h5py
+def test_validate_episode_reports_qp_failures_without_rejecting(tmp_path):
+    path = _valid_episode(tmp_path)
+    with h5py.File(path, "r+") as root:
+        root["debug/qp/attempted"][:] = (1, 1, 1)
+        root["debug/qp/success"][:] = (0, 0, 1)
+        root["debug/control/target_clamped"][:] = (1, 0, 1)
+
+    report = validate_episode(path, _quality_limits())
+
+    assert report.accepted
+    assert report.metrics["qp_failure_count"] == 2
+    assert report.metrics["qp_failure_ratio"] == pytest.approx(2.0 / 3.0)
+    assert report.metrics["qp_longest_failure_streak"] == 2
+    assert report.metrics["target_clamped_count"] == 2
+
+
+class _StatusPublisher:
+    def __init__(self):
+        self.messages = []
+
+    def publish(self, message):
+        self.messages.append(message.data)
+
+
+class _Logger:
+    def info(self, *_args, **_kwargs):
+        pass
+
+    def warn(self, *_args, **_kwargs):
+        pass
+
+    def error(self, *_args, **_kwargs):
+        pass
+
+
+def _control_message(seq, control_ns, *, grip=True):
+    message = ActControlSample()
+    message.control_seq = seq
+    message.control_monotonic_ns = control_ns
+    message.feedback_monotonic_ns = control_ns - 1_000_000
+    message.action_monotonic_ns = control_ns + 1_000_000 if grip else -1
+    message.q_actual = [0.0] * 7
+    message.q_qp_raw = [0.0] * 7
+    message.q_target = [0.0] * 7
+    message.joint_lower_limits = [-3.0] * 7
+    message.joint_upper_limits = [3.0] * 7
+    message.tcp_current.orientation.w = 1.0
+    message.tcp_raw_target.orientation.w = 1.0
+    message.tcp_target.orientation.w = 1.0
+    message.pico_pose.orientation.w = 1.0
+    message.pico_grip = grip
+    message.gripper_target_open = True
+    message.gripper_state_open = True
+    message.gripper_state_known = True
+    message.teleop_active = grip
+    message.feedback_valid = True
+    message.action_valid = True
+    message.command_sent = grip
+    message.qp_attempted = grip
+    message.qp_success = grip
+    return message
+
+
+def _seed_camera(start_ns, first_number):
+    camera = SimpleNamespace(
+        buffer=CameraBuffer(maxlen=4),
+        last_error=None,
+    )
+    image = _image(1)
+    for index in range(151):
+        camera.buffer.push(
+            CameraFrame(
+                image,
+                first_number + index,
+                index * (1000.0 / 30.0),
+                start_ns
+                - 5_020_000_000
+                + round(index * 5_000_000_000 / 150),
+            )
+        )
+    return camera
+
+
+def _recorder_for_test(tmp_path):
+    now_ns = 10_000_000_000
+    recorder = object.__new__(ActEpisodeRecorder)
+    recorder._task_dir = tmp_path / "tomato_pick"
+    recorder._task_dir.mkdir(parents=True)
+    recorder._quality_limits = _quality_limits()
+    recorder._min_free_space_bytes = 4 * 1024**3
+    recorder._controller_timeout_ns = 500_000_000
+    recorder._camera_warmup_ns = 5_000_000_000
+    recorder._gripper_completion_timeout_ns = 3_000_000_000
+    recorder._writer_queue_size = 8
+    recorder._max_camera_age_ms = 50.0
+    recorder._max_camera_skew_ms = 50.0
+    recorder._session = RecordingSession(max_samples=100)
+    recorder._button_tracker = ButtonTracker(hold_ns=1_000_000_000)
+    recorder._latest_control = _control_message(99, now_ns - 1_000_000)
+    recorder._latest_control_received_ns = now_ns - 1_000_000
+    recorder._right_controller_received_ns = now_ns - 1_000_000
+    recorder._left_controller_received_ns = now_ns - 1_000_000
+    recorder._left_secondary = False
+    recorder._high_camera = _seed_camera(now_ns, 850)
+    recorder._wrist_camera = _seed_camera(now_ns, 1850)
+    recorder._camera_start_error = None
+    recorder._writer = None
+    recorder._store = None
+    recorder._partial_path = None
+    recorder._camera_baselines = None
+    recorder._saving_deadline_ns = None
+    recorder._status_pub = _StatusPublisher()
+    recorder._now_ns = lambda: now_ns
+    recorder._disk_usage = lambda _path: SimpleNamespace(free=5 * 1024**3)
+    recorder.get_logger = lambda: _Logger()
+    return recorder
+
+
+@requires_h5py
+def test_preflight_requires_open_gripper_fresh_inputs_and_disk_space(tmp_path):
+    recorder = _recorder_for_test(tmp_path)
+
+    assert recorder._run_preflight() is None
+
+    recorder._latest_control.gripper_state_open = False
+    assert recorder._run_preflight() == "gripper_not_open"
+    recorder._latest_control.gripper_state_open = True
+    recorder._right_controller_received_ns = 0
+    assert recorder._run_preflight() == "right_controller_stale"
+    recorder._right_controller_received_ns = recorder._now_ns()
+    recorder._disk_usage = lambda _path: SimpleNamespace(free=1024)
+    assert recorder._run_preflight() == "insufficient_disk_space"
+
+
+def _push_recording_frames(recorder, control_ns, frame_number):
+    recorder._high_camera.buffer.push(
+        CameraFrame(
+            _image(2),
+            frame_number,
+            float(frame_number),
+            control_ns - 2_000_000,
+        )
+    )
+    recorder._wrist_camera.buffer.push(
+        CameraFrame(
+            _image(3),
+            frame_number,
+            float(frame_number),
+            control_ns - 3_000_000,
+        )
+    )
+
+
+def _finish_fake_episode(recorder):
+    start_ns = recorder._now_ns()
+    frame_number = 1001
+    for offset, seq in enumerate(range(100, 107)):
+        control_ns = start_ns + offset * 11_111_111
+        if (seq - 100) % 3 == 0:
+            _push_recording_frames(recorder, control_ns, frame_number)
+            frame_number += 1
+        recorder._on_control_sample(_control_message(seq, control_ns))
+
+    recorder._on_control_sample(
+        _control_message(107, start_ns + 7 * 11_111_111, grip=False)
+    )
+    recorder._handle_right_b(grip=False)
+    final_ns = start_ns + 8 * 11_111_111
+    _push_recording_frames(recorder, final_ns, frame_number)
+    recorder._on_control_sample(_control_message(108, final_ns, grip=False))
+
+
+@requires_h5py
+def test_end_to_end_fake_episode_saves_and_returns_idle(tmp_path):
+    recorder = _recorder_for_test(tmp_path)
+    recorder._handle_right_b(grip=False)
+
+    assert recorder.state is RecordingState.ARMED
+
+    _finish_fake_episode(recorder)
+
+    assert (tmp_path / "tomato_pick" / "episode_0.hdf5").is_file()
+    assert recorder.state is RecordingState.IDLE
+    assert "SAVING" in recorder._status_pub.messages
+    assert recorder._status_pub.messages[-2:] == ["SAVED", "IDLE"]
+
+
+@requires_h5py
+def test_final_publish_rejects_allocated_episode_number_conflict(tmp_path):
+    recorder = _recorder_for_test(tmp_path)
+    recorder._handle_right_b(grip=False)
+    existing = recorder._task_dir / "episode_0.hdf5"
+    existing.write_bytes(b"existing")
+
+    _finish_fake_episode(recorder)
+
+    assert existing.read_bytes() == b"existing"
+    assert not (recorder._task_dir / "episode_1.hdf5").exists()
+    rejected = list((recorder._task_dir / "rejected").glob("*.hdf5"))
+    assert len(rejected) == 1
+    with h5py.File(rejected[0], "r") as root:
+        assert root.attrs["reject_reason"] == "episode_number_conflict"
+
+
+@requires_h5py
+def test_discard_and_interrupt_only_process_current_partial(tmp_path):
+    recorder = _recorder_for_test(tmp_path)
+    recorder._handle_right_b(grip=False)
+    partial = recorder._partial_path
+
+    recorder._discard_current()
+
+    assert not partial.exists()
+    assert recorder.state is RecordingState.IDLE
+
+    recorder._handle_right_b(grip=False)
+    recorder.interrupt_recording("interrupted")
+    rejected = list((recorder._task_dir / "rejected").glob("*.hdf5"))
+    assert len(rejected) == 1
+    with h5py.File(rejected[0], "r") as root:
+        assert root.attrs["reject_reason"] == "interrupted"
+        assert root.attrs["interrupted"] == np.bool_(True)
+
+
+@requires_h5py
+def test_a_button_and_camera_error_reject_without_robot_commands(tmp_path):
+    recorder = _recorder_for_test(tmp_path)
+    recorder._handle_right_b(grip=False)
+    recorder._button_tracker.on_right(
+        False,
+        False,
+        False,
+        recorder._now_ns(),
+        RecordingState.RECORDING,
+    )
+    recorder._session.state = RecordingState.RECORDING
+    controller = XrController()
+    controller.primary = True
+
+    recorder._on_right_controller(controller)
+
+    rejected = list((recorder._task_dir / "rejected").glob("*.hdf5"))
+    assert len(rejected) == 1
+    with h5py.File(rejected[0], "r") as root:
+        assert root.attrs["reject_reason"] == (
+            "initial_pose_command_during_episode"
+        )
+
+    recorder._handle_right_b(grip=False)
+    recorder._high_camera.last_error = RuntimeError("usb")
+    recorder._on_control_sample(
+        _control_message(100, recorder._now_ns())
+    )
+    rejected = list((recorder._task_dir / "rejected").glob("*.hdf5"))
+    assert len(rejected) == 2
+    camera_rejected = next(
+        path for path in rejected if "camera_error" in path.name
+    )
+    with h5py.File(camera_rejected, "r") as root:
+        assert root.attrs["reject_reason"] == "camera_error"
+
+
+def test_episode_writer_reports_queue_backlog_and_write_error():
+    started = threading.Event()
+    release = threading.Event()
+
+    class BlockingStore:
+        def append(self, _sample):
+            started.set()
+            assert release.wait(timeout=1.0)
+
+    writer = EpisodeWriter(BlockingStore(), queue_size=1)
+    writer.submit({})
+    assert started.wait(timeout=1.0)
+    writer.submit({})
+    with pytest.raises(queue.Full):
+        writer.submit({})
+    release.set()
+    writer.finish()
+    assert writer.error is None
+
+    class FailingStore:
+        def append(self, _sample):
+            raise OSError("disk full")
+
+    writer = EpisodeWriter(FailingStore(), queue_size=1)
+    writer.submit({})
+    writer.finish()
+    assert isinstance(writer.error, OSError)

xr_rm_teleop/test/test_initial_joint_pose.py

@@ -6,13 +6,14 @@ from types import ModuleType, SimpleNamespace
 import pytest
 import yaml
 
-from xr_rm_teleop import realman_adapter
+from xr_rm_teleop import fun_peripheral, realman_adapter
 from xr_rm_teleop.realman_adapter import RealManAdapter
 from xr_rm_teleop.realman_adapter import MockRealManAdapter
 from xr_rm_teleop.fun_peripheral import (
     PeripheralConfig,
     _configure_tool_frame,
     load_peripheral_config,
+    peripheral_cfg,
 )
 
 
@@ -82,6 +83,78 @@ def test_deployed_peripheral_config_selects_left_and_right_tools() -> None:
     assert right.tool_pose == [0.0, 0.0, 0.14, 0.0, 0.0, 0.0, 1.0]
 
 
+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
+
+
+def test_omnipic_initial_state_opens_fully(monkeypatch) -> None:
+    calls = []
+
+    class FakeArm:
+        def rm_set_voltage(self, *args):
+            del args
+
+        def rm_set_io_mode(self, *args):
+            del args
+
+        def rm_algo_quaternion2euler(self, quaternion):
+            del quaternion
+            return [0.0, 0.0, 0.0]
+
+        def rm_get_total_tool_frame(self):
+            return {"return_code": 0, "tool_names": []}
+
+        def rm_set_manual_tool_frame(self, *, frame):
+            del frame
+            return 0
+
+        def rm_change_tool_frame(self, tool_name):
+            del tool_name
+            return 0
+
+        def rm_set_modbus_mode(self, **kwargs):
+            del kwargs
+            return 0
+
+        def rm_write_single_register(self, params, value):
+            del params, value
+            return 0
+
+    sdk = ModuleType("Robotic_Arm.rm_robot_interface")
+    sdk.rm_frame_t = lambda *args: object()
+    sdk.rm_peripheral_read_write_params_t = lambda *args: object()
+    package = ModuleType("Robotic_Arm")
+    package.rm_robot_interface = sdk
+    monkeypatch.setitem(sys.modules, "Robotic_Arm", package)
+    monkeypatch.setitem(sys.modules, "Robotic_Arm.rm_robot_interface", sdk)
+    monkeypatch.setattr(fun_peripheral.time, "sleep", lambda seconds: None)
+    monkeypatch.setattr(
+        fun_peripheral,
+        "set_tool_position",
+        lambda robot, percent, device, scissorgripper: calls.append(
+            (percent, device, scissorgripper)
+        ),
+    )
+    tools = {
+        "scissor": [[0.0] * 7, [0.0] * 7],
+        "omnipic": [[0.0] * 7, [0.0] * 7],
+    }
+
+    peripheral_cfg(
+        FakeArm(),
+        1,
+        tools,
+        set_initial_tool_state=True,
+    )
+
+    assert calls == [(1.0, 1, 1)]
+
+
 @pytest.mark.parametrize(
     ("config_name", "node_names"),
     [

xr_rm_teleop/test/test_joint_control.py

@@ -1,4 +1,5 @@
 import math
+import threading
 import time
 from types import SimpleNamespace
 
@@ -42,6 +43,69 @@ class FakePublisher:
         self.messages.append(message)
 
 
+def _tool_state_teleop(*, command_error=None):
+    started = threading.Event()
+    release = threading.Event()
+
+    class Adapter:
+        def set_tool_enabled(self, open_tool):
+            del open_tool
+            started.set()
+            assert release.wait(timeout=1.0)
+            if command_error is not None:
+                raise command_error
+
+    teleop = object.__new__(SingleArmVelocityTeleop)
+    teleop._arm_name = "right_rm75"
+    teleop._adapter = Adapter()
+    teleop._tool_command_queue = None
+    teleop._tool_worker_stop = threading.Event()
+    teleop._tool_worker_thread = None
+    teleop._tool_state_lock = threading.Lock()
+    teleop._tool_target_open = True
+    teleop._tool_state_open = True
+    teleop._tool_command_pending = False
+    teleop._tool_command_failed = False
+    teleop.get_logger = lambda: FakeLogger()
+    teleop._start_tool_worker()
+    return teleop, started, release
+
+
+def test_tool_state_changes_only_after_command_succeeds() -> None:
+    teleop, started, release = _tool_state_teleop()
+    try:
+        teleop._enqueue_tool_command(False, "test")
+        assert started.wait(timeout=1.0)
+
+        assert teleop._tool_state_snapshot() == (False, True, True, False)
+
+        release.set()
+        assert teleop._tool_command_queue is not None
+        teleop._tool_command_queue.join()
+
+        assert teleop._tool_state_snapshot() == (False, False, False, False)
+    finally:
+        release.set()
+        teleop._shutdown_tool_worker()
+
+
+def test_tool_failure_keeps_previous_state_and_is_reported() -> None:
+    teleop, started, release = _tool_state_teleop(
+        command_error=RuntimeError("modbus failed")
+    )
+    try:
+        teleop._enqueue_tool_command(False, "test")
+        assert started.wait(timeout=1.0)
+        release.set()
+        assert teleop._tool_command_queue is not None
+        teleop._tool_command_queue.join()
+
+        assert teleop._tool_state_snapshot() == (False, True, False, True)
+    finally:
+        release.set()
+        teleop._shutdown_tool_worker()
+
+
 def _joint_publishing_teleop() -> SingleArmVelocityTeleop:
     names = [f"omnipic_joint_{index}" for index in range(1, 8)]
     teleop = object.__new__(SingleArmVelocityTeleop)
@@ -622,9 +686,10 @@ def test_qp_failure_returns_last_known_good_target() -> None:
     teleop._arm_name = "right_rm75"
     teleop.get_logger = lambda: FakeLogger()
 
-    target = teleop._solve_joint_target(np.eye(4))
+    target, qp_success = teleop._solve_joint_target(np.eye(4))
 
     assert target == pytest.approx([0.1] * 7)
+    assert not qp_success
     assert teleop._last_valid_joint_target == pytest.approx([0.1] * 7)
 
 
@@ -640,9 +705,10 @@ def test_qp_success_updates_last_known_good_target() -> None:
     teleop._arm_name = "left_rm75"
     teleop.get_logger = lambda: FakeLogger()
 
-    target = teleop._solve_joint_target(np.eye(4))
+    target, qp_success = teleop._solve_joint_target(np.eye(4))
 
     assert target == pytest.approx([0.2] * 7)
+    assert qp_success
     assert teleop._last_valid_joint_target == pytest.approx([0.2] * 7)
 
 

xr_rm_teleop/test/test_placo_transforms.py

@@ -272,6 +272,20 @@ def test_validated_transform_rejects_invalid_se3(transform: np.ndarray) -> None:
         _validated_transform(transform)
 
 
+def test_joint_position_limits_returns_a_copy() -> None:
+    solver = object.__new__(PlacoIkSolver)
+    solver._joint_limits = np.asarray([[-1.0, 1.0]] * 7)
+
+    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
+
+
 def test_qp_result_rejects_nan_position_and_velocity_violations() -> None:
     solver = object.__new__(PlacoIkSolver)
     solver._joint_limits = np.asarray([[-1.0, 1.0]] * 7)

xr_rm_teleop/xr_rm_teleop/fun_peripheral.py

@@ -225,9 +225,12 @@ def peripheral_cfg(
             time.sleep(0.5)
 
         if set_initial_tool_state:
-            set_tool_position(robot, percent=0.75, device=1, scissorgripper=scissorgripper)
+            set_tool_position(
-            time.sleep(1.5)
+                robot,
-            set_tool_position(robot, percent=0.15, device=1, scissorgripper=scissorgripper)
+                percent=1.0,
+                device=1,
+                scissorgripper=scissorgripper,
+            )
 
     elif scissorgripper == 2:
         # 小型剪刀夹爪通过控制器 DO3/DO4 控制。

xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py

@@ -157,6 +157,10 @@ class PlacoIkSolver:
     def base_configuration(self) -> list[float]:
         return self._robot.state.q[:7].tolist()
 
+    @property
+    def joint_position_limits(self) -> np.ndarray:
+        return self._joint_limits.copy()
+
     def update_joint_state(self, joints: list[float]) -> np.ndarray:
         values = np.asarray(joints, dtype=float)
         if values.shape != (7,) or not np.isfinite(values).all():

xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py

@@ -10,17 +10,19 @@ import math
 import queue
 import threading
 import time
+from dataclasses import dataclass
 from typing import Iterable
 
 import numpy as np
 import rclpy
 from geometry_msgs.msg import PoseStamped, TwistStamped
 from rclpy.node import Node
+from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy
 from rclpy.time import Time
 from sensor_msgs.msg import JointState
 from std_msgs.msg import Bool
 
-from xr_rm_interfaces.msg import XrController
+from xr_rm_interfaces.msg import ActControlSample, XrController
 
 from .fun_peripheral import load_peripheral_config
 from .placo_ik_solver import PlacoIkSolver
@@ -31,6 +33,30 @@ from .realman_adapter import (
 )
 
 
+@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
+    qp_duration_ms: float = 0.0
+    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
+
+
 def _norm(values: Iterable[float]) -> float:
     return math.sqrt(sum(value * value for value in values))
 
@@ -291,12 +317,19 @@ class SingleArmVelocityTeleop(Node):
         self._latest_joint_positions: list[float] | None = None
         self._last_joint_command_target: list[float] | None = None
         self._last_joint_command_velocity: list[float] | None = None
+        self._last_successful_action_target: list[float] | None = None
+        self._act_control_seq = 0
         self._joint_feedback_ready = False
         self._grip_rearm_required = False
         self._feedback_resync_attempted = False
         self._control_fault_latched = False
         self._stop_sent = True
         self._trigger_tool_open = True
+        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
         self._last_primary_pressed: bool | None = None
         self._last_trigger_pressed: bool | None = None
         self._tool_command_queue: queue.Queue[tuple[bool, str] | None] | None = None
@@ -340,6 +373,15 @@ class SingleArmVelocityTeleop(Node):
             f"{debug_ns}/joint_target",
             10,
         )
+        self._act_sample_pub = self.create_publisher(
+            ActControlSample,
+            f"{debug_ns}/act_control_sample",
+            QoSProfile(
+                history=HistoryPolicy.KEEP_LAST,
+                depth=10,
+                reliability=ReliabilityPolicy.BEST_EFFORT,
+            ),
+        )
         self._adapter = self._make_adapter()
         self._adapter.connect()
         self._initialize_joint_state()
@@ -430,11 +472,20 @@ class SingleArmVelocityTeleop(Node):
 
     def _setup_tool_control(self) -> None:
         peripheral_arm = self._peripheral_arm_name()
-        if self._bool_parameter("configure_peripheral_on_connect"):
+        configure_on_connect = self._bool_parameter(
+            "configure_peripheral_on_connect"
+        )
+        if configure_on_connect:
             self._adapter.configure_peripheral(
                 self._peripheral_config,
                 peripheral_arm,
             )
+            if self._peripheral_config.set_initial_tool_state:
+                with self._tool_state_lock:
+                    self._tool_target_open = True
+                    self._tool_state_open = True
+                    self._tool_command_pending = False
+                    self._tool_command_failed = False
 
         if not self._enable_tool_control:
             if self._enable_trigger_gripper_control:
@@ -484,6 +535,10 @@ class SingleArmVelocityTeleop(Node):
             )
             return
 
+        with self._tool_state_lock:
+            self._tool_target_open = open_tool
+            self._tool_command_pending = True
+
         item = (open_tool, source)
         while True:
             try:
@@ -512,15 +567,34 @@ class SingleArmVelocityTeleop(Node):
                 try:
                     self._adapter.set_tool_enabled(open_tool)
                 except Exception as exc:
+                    with self._tool_state_lock:
+                        self._tool_command_failed = True
                     self.get_logger().error(
                         f"{self._arm_name} tool {action} failed from {source}: {exc}"
                     )
                     continue
+                with self._tool_state_lock:
+                    self._tool_state_open = open_tool
+                    self._tool_command_failed = False
                 self.get_logger().info(
                     f"{self._arm_name} tool {action} command sent from {source}"
                 )
             finally:
                 self._tool_command_queue.task_done()
+                if self._tool_command_queue.empty():
+                    with self._tool_state_lock:
+                        self._tool_command_pending = False
+
+    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,
+            )
 
     def _peripheral_arm_name(self) -> str:
         configured = str(self.get_parameter("peripheral_arm").value).strip().lower()
@@ -582,6 +656,18 @@ class SingleArmVelocityTeleop(Node):
         self._enqueue_tool_command(self._trigger_tool_open, "trigger")
 
     def _control_tick(self) -> None:
+        control_seq = getattr(self, "_act_control_seq", 0)
+        self._act_control_seq = control_seq + 1
+        cycle = _ActCycleContext(
+            control_seq=control_seq,
+            control_monotonic_ns=time.monotonic_ns(),
+        )
+        try:
+            self._control_tick_impl(cycle)
+        finally:
+            self._publish_act_control_sample(cycle)
+
+    def _control_tick_impl(self, cycle: _ActCycleContext) -> None:
         tick_started_ns = time.perf_counter_ns()
         last_tick_started_ns = getattr(
             self,
@@ -596,10 +682,12 @@ class SingleArmVelocityTeleop(Node):
         )
         now = self.get_clock().now()
         if self._control_fault_latched:
+            cycle.control_fault = True
             return
 
         snapshot = self._adapter.get_latest_joint_state()
         if not self._joint_snapshot_is_motion_ready(snapshot):
+            cycle.control_fault = True
             self._grip_rearm_required = True
             if self._joint_feedback_ready:
                 self.get_logger().warn(
@@ -610,13 +698,18 @@ class SingleArmVelocityTeleop(Node):
             self._safe_stop(reset_active=True)
             return
         assert snapshot is not None
+        cycle.q_actual = list(snapshot.positions)
+        cycle.feedback_monotonic_ns = int(snapshot.received_at * 1e9)
         feedback_age = time.monotonic() - snapshot.received_at
+        cycle.feedback_age_ms = feedback_age * 1000.0
         if feedback_age < 0.0:
+            cycle.control_fault = True
             self._grip_rearm_required = True
             self._joint_feedback_ready = False
             self._safe_stop(reset_active=True)
             return
         if feedback_age > self._command_timeout_sec:
+            cycle.control_fault = True
             self._handle_stale_joint_feedback(feedback_age)
             return
 
@@ -624,6 +717,7 @@ class SingleArmVelocityTeleop(Node):
         try:
             current_pose = self._sync_joint_feedback(snapshot)
         except Exception as exc:
+            cycle.control_fault = True
             self.get_logger().error(
                 f"{self._arm_name} 关节反馈同步到 Placo 失败：{exc}",
                 throttle_duration_sec=1.0,
@@ -632,6 +726,8 @@ class SingleArmVelocityTeleop(Node):
             self._grip_rearm_required = True
             self._safe_stop(reset_active=True)
             return
+        cycle.current_pose = current_pose
+        cycle.feedback_valid = True
         if not self._joint_feedback_ready:
             if self._grip_rearm_required:
                 message = (
@@ -680,6 +776,7 @@ class SingleArmVelocityTeleop(Node):
         try:
             controller_quat = self._controller_quaternion(self._last_msg)
         except ValueError as exc:
+            cycle.control_fault = True
             self.get_logger().warn(
                 f"{self._arm_name} XR 手柄姿态无效，停止输出：{exc}",
                 throttle_duration_sec=1.0,
@@ -724,19 +821,35 @@ class SingleArmVelocityTeleop(Node):
             sent_target,
             sent_orientation,
         )
+        cycle.raw_target_pose = raw_target_pose
+        cycle.target_pose = target_pose
+        cycle.command_velocity = list(velocity)
+        cycle.target_clamped = target_clamped
 
         self._publish_debug(raw_target_pose, target_pose, velocity, target_clamped)
+        cycle.qp_attempted = True
         qp_started_ns = time.perf_counter_ns()
-        joint_target = self._solve_joint_target(target_pose)
+        joint_target, qp_success = self._solve_joint_target(target_pose)
         qp_ms = (time.perf_counter_ns() - qp_started_ns) * 1e-6
+        cycle.qp_duration_ms = qp_ms
+        cycle.q_qp_raw = list(joint_target)
+        cycle.qp_success = qp_success
         send_started_ns = time.perf_counter_ns()
         sent = self._send_joint_target(joint_target)
         send_ms = (time.perf_counter_ns() - send_started_ns) * 1e-6
         if sent:
+            assert self._last_joint_command_target is not None
+            final_target = list(self._last_joint_command_target)
+            self._last_successful_action_target = final_target
+            cycle.q_target = final_target
+            cycle.action_monotonic_ns = time.monotonic_ns()
+            cycle.command_sent = True
             self._last_sent_target = sent_target
             self._last_sent_orientation = sent_orientation.copy()
             self._last_command_time = now
             self._stop_sent = False
+        else:
+            cycle.send_failed = True
         total_ms = (time.perf_counter_ns() - tick_started_ns) * 1e-6
         try:
             self._record_timing_sample(
@@ -1196,7 +1309,10 @@ class SingleArmVelocityTeleop(Node):
             self._last_valid_joint_target = list(snapshot.positions)
         return current_pose
 
-    def _solve_joint_target(self, target_pose: np.ndarray) -> list[float]:
+    def _solve_joint_target(
+        self,
+        target_pose: np.ndarray,
+    ) -> tuple[list[float], bool]:
         if self._last_valid_joint_target is None:
             raise RuntimeError("valid joint feedback has not been initialized")
         try:
@@ -1206,9 +1322,9 @@ class SingleArmVelocityTeleop(Node):
                 f"{self._arm_name} QP 求解失败，保持上一组关节目标：{exc}",
                 throttle_duration_sec=1.0,
             )
-            return list(self._last_valid_joint_target)
+            return list(self._last_valid_joint_target), False
         self._last_valid_joint_target = list(result)
-        return list(result)
+        return list(result), True
 
     def _safe_stop(self, reset_active: bool) -> None:
         if not self._stop_sent:
@@ -1380,6 +1496,122 @@ class SingleArmVelocityTeleop(Node):
         self._cmd_vel_pub.publish(velocity_msg)
         self._target_clamped_pub.publish(clamped_msg)
 
+    def _build_act_control_sample(
+        self,
+        cycle: _ActCycleContext,
+    ) -> ActControlSample:
+        message = ActControlSample()
+        message.header.stamp = self.get_clock().now().to_msg()
+        message.header.frame_id = "rm_base"
+        message.control_seq = cycle.control_seq
+        message.control_monotonic_ns = cycle.control_monotonic_ns
+        message.feedback_monotonic_ns = cycle.feedback_monotonic_ns
+        message.action_monotonic_ns = cycle.action_monotonic_ns
+        message.feedback_age_ms = float(cycle.feedback_age_ms)
+        message.qp_duration_ms = float(cycle.qp_duration_ms)
+
+        q_actual = cycle.q_actual or [0.0] * 7
+        held_target = (
+            cycle.q_target
+            or self._last_successful_action_target
+            or q_actual
+        )
+        qp_target = cycle.q_qp_raw or held_target
+        limits = np.asarray(
+            self._ik_solver.joint_position_limits,
+            dtype=float,
+        )
+        if limits.shape != (7, 2) or not np.isfinite(limits).all():
+            raise ValueError("joint limits must have finite shape (7, 2)")
+        message.q_actual = [float(value) for value in q_actual]
+        message.q_qp_raw = [float(value) for value in qp_target]
+        message.q_target = [float(value) for value in held_target]
+        message.joint_lower_limits = limits[:, 0].tolist()
+        message.joint_upper_limits = limits[:, 1].tolist()
+
+        current_pose = cycle.current_pose
+        if current_pose is None:
+            current_pose = self._debug_pose_fallback()
+        if current_pose is None:
+            current_pose = np.eye(4)
+        raw_target_pose = cycle.raw_target_pose
+        if raw_target_pose is None:
+            raw_target_pose = current_pose
+        target_pose = cycle.target_pose
+        if target_pose is None:
+            target_pose = current_pose
+        message.tcp_current = self._pose_msg(
+            message.header.stamp,
+            current_pose,
+        ).pose
+        message.tcp_raw_target = self._pose_msg(
+            message.header.stamp,
+            raw_target_pose,
+        ).pose
+        message.tcp_target = self._pose_msg(
+            message.header.stamp,
+            target_pose,
+        ).pose
+        velocity = cycle.command_velocity or [0.0] * 6
+        if len(velocity) != 6:
+            raise ValueError("ACT command velocity must contain 6 values")
+        message.tcp_command_velocity.linear.x = float(velocity[0])
+        message.tcp_command_velocity.linear.y = float(velocity[1])
+        message.tcp_command_velocity.linear.z = float(velocity[2])
+        message.tcp_command_velocity.angular.x = float(velocity[3])
+        message.tcp_command_velocity.angular.y = float(velocity[4])
+        message.tcp_command_velocity.angular.z = float(velocity[5])
+
+        controller = self._last_msg
+        if controller is not None:
+            message.pico_pose = controller.pose
+            message.pico_grip = bool(controller.grip)
+            message.pico_trigger = float(controller.trigger)
+            message.pico_primary = bool(controller.primary)
+            message.pico_secondary = bool(controller.secondary)
+            message.pico_axis = [float(value) for value in controller.axis]
+
+        tool_target, tool_state, tool_pending, tool_failed = (
+            self._tool_state_snapshot()
+        )
+        message.gripper_target_open = tool_target
+        message.gripper_state_known = tool_state is not None
+        message.gripper_state_open = bool(tool_state)
+        message.gripper_command_pending = tool_pending
+        message.gripper_command_failed = tool_failed
+
+        message.teleop_active = bool(self._active)
+        message.feedback_valid = cycle.feedback_valid
+        message.action_valid = bool(
+            self._last_successful_action_target is not None
+            and cycle.feedback_valid
+            and not cycle.send_failed
+            and not cycle.control_fault
+        )
+        message.command_sent = cycle.command_sent
+        message.qp_attempted = cycle.qp_attempted
+        message.qp_success = cycle.qp_success
+        message.target_clamped = cycle.target_clamped
+        message.control_fault = bool(
+            cycle.control_fault or self._control_fault_latched
+        )
+        return message
+
+    def _publish_act_control_sample(
+        self,
+        cycle: _ActCycleContext,
+    ) -> None:
+        publisher = getattr(self, "_act_sample_pub", None)
+        if publisher is None:
+            return
+        try:
+            publisher.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,
+            )
+
     @staticmethod
     def _pose_msg(stamp, pose: np.ndarray) -> PoseStamped:
         transform = _make_transform(pose[:3, 3], pose[:3, :3])