fix: 调整peripheral_cfg函数中的目标力矩设置值

README.md

@@ -150,6 +150,14 @@ ros2 launch xr_rm_bringup arm_debug.launch.py \
   arm:=right use_mock:=false record_act:=true
 ```
 
+`record_act:=true` 启动后默认显示全局 D455 和右腕 D405 双路画面，并显示实时
+FPS、真实丢帧率、帧龄、双相机时间差、录制状态、episode 编号和样本数。按
+`Q`、`Esc` 或关闭窗口只会停止预览，ACT 相机采集和录制继续运行；没有桌面环境
+或 OpenCV 显示失败时也不会影响录制。
+
+相机采集线程观察到的真实掉帧仍会拒绝 episode。独立 `30 Hz` 控制和相机时钟
+造成的 ACT 样本重复/跨帧只写入 HDF5 质量指标，不再误报为相机丢包。
+
 默认配置位于 `xr_rm_bringup/config/act_tomato_pick.yaml`：D455 序列号
 `234222303366`，右腕 D405 序列号 `412622272532`，90 Hz 控制数据下采样为
 30 Hz。输出位于 `/home/robot/ACT_Data/tomato_pick/`，状态发布到

docs/superpowers/plans/2026-08-13-act-camera-preview-alignment.md

@@ -0,0 +1,756 @@
+# ACT 双相机预览与采样质量判定修正实施计划
+
+> **供代理执行者使用：** 必须使用 `superpowers:subagent-driven-development`（推荐）或 `superpowers:executing-plans`，逐项执行本计划。所有步骤使用复选框（`- [ ]`）跟踪。
+
+**目标：** 保持 ACT 当前因果时间对齐，同时把真实相机丢帧与软件采样相位漂移分开判定，并在 ACT 数采启动时默认显示全局 D455 和右腕 D405 实时画面。
+
+**架构：** 继续由 `act_episode_recorder` 独占两台 RealSense，并按每三个 `90 Hz` 控制周期选择不晚于控制时刻的最新图像。`CameraBuffer` 负责真实采集质量，HDF5 校验只记录 ACT 样本重复/跨帧率；同一节点内新增一个只读 OpenCV 预览线程，复用现有帧缓冲且不进入机器人控制链路。
+
+**技术栈：** Ubuntu 22.04、ROS2 Humble、Python 3、rclpy、NumPy、h5py、pyrealsense2、OpenCV、pytest、HDF5。
+
+---
+
+## 文件结构
+
+本次不新建 ROS 包或运行进程，文件职责保持如下：
+
+- 修改 `xr_rm_teleop/xr_rm_teleop/act_episode_recorder.py`：真实采集统计、ACT 采样相位指标、质量判定、双路预览及关闭顺序；
+- 修改 `xr_rm_teleop/test/test_act_episode_recorder.py`：指标口径、误拒绝复现、帧号回退和预览隔离测试；
+- 修改 `README.md`：说明 ACT 数采默认预览、显示内容和关闭行为。
+
+不修改 `arm_debug.launch.py`、`launcher_ui.py`、ROS 消息、机器人控制节点和相机 YAML。工作区已有的 `xr_rm_teleop/xr_rm_teleop/fun_peripheral.py` 未提交改动属于用户，不加入本任务提交。
+
+所有构建和测试命令从工作空间根目录执行：
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+```
+
+自动化测试不得启动真机 launch、移动机械臂或操作夹爪。
+
+### 任务 1：区分真实丢帧与 ACT 采样相位漂移
+
+**文件：**
+
+- 修改：`xr_rm_teleop/xr_rm_teleop/act_episode_recorder.py:408-579`
+- 修改：`xr_rm_teleop/xr_rm_teleop/act_episode_recorder.py:595-657`
+- 修改：`xr_rm_teleop/xr_rm_teleop/act_episode_recorder.py:1472-1511`
+- 测试：`xr_rm_teleop/test/test_act_episode_recorder.py:273-296`
+- 测试：`xr_rm_teleop/test/test_act_episode_recorder.py:491-607`
+
+- [ ] **步骤 1：写异步采样不拒绝和指标测试**
+
+在测试导入中加入 `sample_frame_metrics`，并增加：
+
+```python
+def test_sample_frame_metrics_separate_repeats_skips_and_regressions():
+    repeat, skip, regression = sample_frame_metrics(
+        np.asarray((986, 986, 988), dtype=np.uint64)
+    )
+
+    assert repeat == pytest.approx(0.5)
+    assert skip == pytest.approx(0.5)
+    assert regression == 0
+
+
+@requires_h5py
+def test_validate_episode_accepts_async_camera_phase_drift(tmp_path):
+    path = _valid_episode(tmp_path)
+    with h5py.File(path, "r+") as root:
+        root["debug/cameras/cam_high_frame_number"][:] = (986, 986, 988)
+
+    report = validate_episode(path, _quality_limits())
+
+    assert report.accepted
+    assert report.metrics["cam_high_sample_repeat_ratio"] == pytest.approx(0.5)
+    assert report.metrics["cam_high_sample_skip_ratio"] == pytest.approx(0.5)
+```
+
+在 `_valid_episode()` 写入新增的真实采集属性：
+
+```python
+root.attrs["camera_high_frame_number_regression_count"] = 0
+root.attrs["camera_right_wrist_frame_number_regression_count"] = 0
+```
+
+- [ ] **步骤 2：写真实帧号回退测试**
+
+扩展现有 `CameraBuffer` 测试：
+
+```python
+def test_camera_buffer_counts_frame_number_regressions():
+    buffer = CameraBuffer(maxlen=4)
+    for frame_number in (100, 101, 1, 2):
+        buffer.push(
+            CameraFrame(
+                _image(frame_number),
+                frame_number,
+                float(frame_number),
+                time.monotonic_ns(),
+            )
+        )
+
+    stats = buffer.stats()
+
+    assert stats.frame_number_regression_count == 1
+    assert stats.dropped_frames == 0
+```
+
+测试文件顶部增加标准库导入：
+
+```python
+import time
+```
+
+并在稳定拒绝原因参数中增加真实采集帧号回退：
+
+```python
+elif mutation == "camera_frame_regression":
+    root.attrs["camera_high_frame_number_regression_count"] = 1
+```
+
+对应期望：
+
+```python
+("camera_frame_regression", "camera_frame_number_regression"),
+```
+
+- [ ] **步骤 3：运行新增测试并确认失败**
+
+运行：
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+pytest src/xr_rm_teleop/test/test_act_episode_recorder.py \
+  -k "sample_frame_metrics or async_camera_phase_drift or frame_number_regressions or camera_frame_regression" -v
+```
+
+预期：测试因 `sample_frame_metrics` 尚不存在、`CameraStats` 没有回退计数，或旧校验仍以 `camera_sample_drop_ratio` 拒绝而失败。
+
+- [ ] **步骤 4：实现最小采样相位指标**
+
+在质量校验辅助函数附近增加纯函数：
+
+```python
+def sample_frame_metrics(
+    frame_numbers: np.ndarray,
+) -> tuple[float, float, int]:
+    diffs = np.diff(np.asarray(frame_numbers, dtype=np.int64))
+    denominator = max(1, len(diffs))
+    return (
+        float(np.count_nonzero(diffs == 0) / denominator),
+        float(np.count_nonzero(diffs > 1) / denominator),
+        int(np.count_nonzero(diffs < 0)),
+    )
+```
+
+把 `validate_episode()` 中原有“所有 `diff != 1` 都拒绝”的循环替换为：
+
+```python
+for camera in ("cam_high", "cam_wrist"):
+    frame_numbers = datasets[
+        f"debug/cameras/{camera}_frame_number"
+    ][:]
+    repeat_ratio, skip_ratio, regression_count = sample_frame_metrics(
+        frame_numbers
+    )
+    metrics[f"{camera}_sample_repeat_ratio"] = repeat_ratio
+    metrics[f"{camera}_sample_skip_ratio"] = skip_ratio
+    if regression_count:
+        return _quality_failure("camera_frame_number_regression", metrics)
+```
+
+这样 `986 → 986 → 988` 只产生统计，不再触发 `camera_sample_drop_ratio`。
+
+- [ ] **步骤 5：在采集层统计帧号回退**
+
+扩展 `CameraStats`：
+
+```python
+@dataclass(frozen=True)
+class CameraStats:
+    frame_count: int
+    dropped_frames: int
+    frame_number_regression_count: int
+    first_host_monotonic_ns: int | None
+    last_host_monotonic_ns: int | None
+```
+
+在 `CameraBuffer.__init__()` 增加：
+
+```python
+self._frame_number_regression_count = 0
+```
+
+在 `CameraBuffer.push()` 更新帧号前使用互斥分支：
+
+```python
+if self._last_frame_number is not None:
+    if frame.frame_number <= self._last_frame_number:
+        self._frame_number_regression_count += 1
+    elif frame.frame_number > self._last_frame_number + 1:
+        self._dropped_frames += (
+            frame.frame_number - self._last_frame_number - 1
+        )
+```
+
+在 `stats()` 返回：
+
+```python
+frame_number_regression_count=self._frame_number_regression_count,
+```
+
+- [ ] **步骤 6：把真实采集回退纳入 episode 属性和拒绝条件**
+
+把 `_interval_camera_metrics()` 的返回值扩展为 FPS、真实丢帧率和本 episode 新增的回退数：
+
+```python
+@staticmethod
+def _interval_camera_metrics(
+    baseline: CameraStats,
+    current: CameraStats,
+) -> tuple[float, float, int]:
+    frames = max(0, current.frame_count - baseline.frame_count)
+    dropped = max(0, current.dropped_frames - baseline.dropped_frames)
+    regressions = max(
+        0,
+        current.frame_number_regression_count
+        - baseline.frame_number_regression_count,
+    )
+    if (
+        baseline.last_host_monotonic_ns is None
+        or current.last_host_monotonic_ns is None
+    ):
+        return 0.0, 1.0, regressions
+    elapsed_ns = (
+        current.last_host_monotonic_ns
+        - baseline.last_host_monotonic_ns
+    )
+    fps = frames * 1e9 / elapsed_ns if elapsed_ns > 0 else 0.0
+    expected = frames + dropped
+    drop_ratio = dropped / expected if expected else 1.0
+    return fps, drop_ratio, regressions
+```
+
+`_write_camera_metrics()` 写入：
+
+```python
+"camera_high_frame_number_regression_count": high[2],
+"camera_right_wrist_frame_number_regression_count": wrist[2],
+```
+
+`validate_episode()` 在读取 FPS 和真实丢帧率后要求两个回退属性存在且为零：
+
+```python
+for name in (
+    "camera_high_frame_number_regression_count",
+    "camera_right_wrist_frame_number_regression_count",
+):
+    if name not in root.attrs:
+        return _quality_failure("camera_stats_missing", metrics)
+    value = int(root.attrs[name])
+    metrics[name] = value
+    if value:
+        return _quality_failure("camera_frame_number_regression", metrics)
+```
+
+- [ ] **步骤 7：确保采样指标写入保存和拒绝文件**
+
+增加读取 HDF5 根节点的辅助函数：
+
+```python
+def episode_sample_frame_metrics(root: Any) -> dict[str, int | float]:
+    metrics: dict[str, int | float] = {}
+    for camera in ("cam_high", "cam_wrist"):
+        repeat, skip, regression = sample_frame_metrics(
+            root[f"debug/cameras/{camera}_frame_number"][:]
+        )
+        metrics[f"{camera}_sample_repeat_ratio"] = repeat
+        metrics[f"{camera}_sample_skip_ratio"] = skip
+        metrics[f"{camera}_sample_frame_number_regression_count"] = regression
+    return metrics
+```
+
+`validate_episode()` 复用该函数更新 `report.metrics`。在 `_reject_closed_partial()` 已打开 HDF5 后也执行：
+
+```python
+for name, value in episode_sample_frame_metrics(root).items():
+    root.attrs[name] = value
+```
+
+保存路径继续由 `_complete_save()` 把 `report.metrics` 写入属性。保存日志追加紧凑摘要：
+
+```python
+self.get_logger().info(
+    "ACT相机采样相位："
+    f"high重复={report.metrics['cam_high_sample_repeat_ratio']:.2%}, "
+    f"high跨帧={report.metrics['cam_high_sample_skip_ratio']:.2%}, "
+    f"wrist重复={report.metrics['cam_wrist_sample_repeat_ratio']:.2%}, "
+    f"wrist跨帧={report.metrics['cam_wrist_sample_skip_ratio']:.2%}"
+)
+```
+
+- [ ] **步骤 8：运行相关测试并确认通过**
+
+运行：
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+pytest src/xr_rm_teleop/test/test_act_episode_recorder.py \
+  -k "camera or validate_episode" -v
+```
+
+预期：所有选中测试通过；真实丢帧率仍使用 `camera_drop_ratio` 拒绝，异步重复/跨帧不拒绝。
+
+- [ ] **步骤 9：提交任务 1**
+
+```bash
+git add src/xr_rm_teleop/xr_rm_teleop/act_episode_recorder.py \
+  src/xr_rm_teleop/test/test_act_episode_recorder.py
+git commit -m "fix: 修正ACT相机采样质量判定"
+```
+
+### 任务 2：在录制器中增加非阻塞双路预览
+
+**文件：**
+
+- 修改：`xr_rm_teleop/xr_rm_teleop/act_episode_recorder.py:595-657`
+- 修改：`xr_rm_teleop/xr_rm_teleop/act_episode_recorder.py:1006-1152`
+- 修改：`xr_rm_teleop/xr_rm_teleop/act_episode_recorder.py:1668-1671`
+- 测试：`xr_rm_teleop/test/test_act_episode_recorder.py`
+
+- [ ] **步骤 1：写两秒滚动 FPS 测试**
+
+扩展 `CameraBuffer` 测试，使用明确的单调时间：
+
+```python
+def test_camera_buffer_reports_two_second_rolling_fps():
+    buffer = CameraBuffer(maxlen=4)
+    start_ns = 10_000_000_000
+    for index in range(61):
+        buffer.push(
+            CameraFrame(
+                _image(index),
+                index,
+                float(index),
+                start_ns + index * 33_333_333,
+            )
+        )
+
+    assert buffer.stats().rolling_fps == pytest.approx(30.0, rel=0.02)
+```
+
+- [ ] **步骤 2：写预览显示异常隔离测试**
+
+构造不经过 ROS 初始化的录制器和抛错的 `cv2` 替身：
+
+```python
+def test_preview_failure_does_not_change_recording_state():
+    recorder = object.__new__(ActEpisodeRecorder)
+    recorder._session = _recording_session()
+    recorder._preview_stop = threading.Event()
+    recorder._preview_thread = None
+    recorder.get_logger = lambda: _Logger()
+
+    class FailingCv2:
+        WINDOW_NORMAL = 0
+
+        @staticmethod
+        def namedWindow(*_args):
+            raise RuntimeError("no display")
+
+    recorder._preview_loop(FailingCv2())
+
+    assert recorder.state is RecordingState.RECORDING
+```
+
+`_Logger` 增加 `warns` 收集和 `warn()`：
+
+```python
+self.warns = []
+
+def warn(self, message):
+    self.warns.append(message)
+```
+
+- [ ] **步骤 3：写预览关闭顺序测试**
+
+验证关闭节点时先停预览，再停相机：
+
+```python
+def test_close_stops_preview_before_cameras_and_releases_lock():
+    events = []
+    recorder = object.__new__(ActEpisodeRecorder)
+    recorder._stop_preview = lambda: events.append("preview")
+    recorder._high_camera = SimpleNamespace(
+        stop=lambda: events.append("high")
+    )
+    recorder._wrist_camera = SimpleNamespace(
+        stop=lambda: events.append("wrist")
+    )
+    recorder._directory_lock = SimpleNamespace(
+        release=lambda: events.append("lock")
+    )
+
+    recorder.close()
+
+    assert events == ["preview", "high", "wrist", "lock"]
+```
+
+- [ ] **步骤 4：运行新增预览测试并确认失败**
+
+运行：
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+pytest src/xr_rm_teleop/test/test_act_episode_recorder.py \
+  -k "rolling_fps or preview or close_stops_preview" -v
+```
+
+预期：测试因 `rolling_fps`、`_preview_loop()` 和 `_stop_preview()` 尚不存在而失败。
+
+- [ ] **步骤 5：实现两秒滚动 FPS**
+
+在 `CameraBuffer.__init__()` 增加：
+
+```python
+self._recent_host_monotonic_ns: deque[int] = deque()
+```
+
+每次 `push()` 时裁剪两秒窗口：
+
+```python
+self._recent_host_monotonic_ns.append(frame.host_monotonic_ns)
+cutoff_ns = frame.host_monotonic_ns - 2_000_000_000
+while (
+    self._recent_host_monotonic_ns
+    and self._recent_host_monotonic_ns[0] < cutoff_ns
+):
+    self._recent_host_monotonic_ns.popleft()
+```
+
+`CameraStats` 增加字段和属性：
+
+```python
+recent_host_monotonic_ns: tuple[int, ...]
+
+@property
+def rolling_fps(self) -> float:
+    if len(self.recent_host_monotonic_ns) < 2:
+        return 0.0
+    elapsed_ns = (
+        self.recent_host_monotonic_ns[-1]
+        - self.recent_host_monotonic_ns[0]
+    )
+    return (
+        (len(self.recent_host_monotonic_ns) - 1) * 1e9 / elapsed_ns
+        if elapsed_ns > 0
+        else 0.0
+    )
+```
+
+`stats()` 使用：
+
+```python
+recent_host_monotonic_ns=tuple(self._recent_host_monotonic_ns),
+```
+
+- [ ] **步骤 6：实现最小预览渲染方法**
+
+在 `ActEpisodeRecorder` 增加固定窗口名：
+
+```python
+PREVIEW_WINDOW = "ACT - D455 Global / D405 Right Wrist"
+```
+
+增加生成单个画面块的方法。相机帧是 RGB，因此显示前转换成 BGR：
+
+```python
+@staticmethod
+def _preview_tile(role: str, frame, stats, cv2):
+    if frame is None:
+        tile = np.zeros((480, 640, 3), dtype=np.uint8)
+        frame_number = "-"
+    else:
+        tile = cv2.cvtColor(frame.image, cv2.COLOR_RGB2BGR)
+        frame_number = str(frame.frame_number)
+    tile = tile.copy()
+    cv2.rectangle(tile, (0, 0), (640, 74), (0, 0, 0), -1)
+    lines = (
+        f"{role}  FPS {stats.rolling_fps:.1f}  Frame {frame_number}",
+        f"Received {stats.frame_count}  Dropped "
+        f"{stats.dropped_frames} ({stats.drop_ratio:.2%})",
+    )
+    for index, line in enumerate(lines):
+        cv2.putText(
+            tile,
+            line,
+            (10, 28 + index * 30),
+            cv2.FONT_HERSHEY_SIMPLEX,
+            0.65,
+            (255, 255, 255),
+            1,
+            cv2.LINE_AA,
+        )
+    return tile
+```
+
+增加 `_compose_preview()`：读取两个缓冲最新帧，水平拼接，并在底部显示状态：
+
+```python
+def _compose_preview(self, cv2):
+    high_frames = self._high_camera.buffer.snapshot()
+    wrist_frames = self._wrist_camera.buffer.snapshot()
+    high = high_frames[-1] if high_frames else None
+    wrist = wrist_frames[-1] if wrist_frames else None
+    high_stats = self._high_camera.buffer.stats()
+    wrist_stats = self._wrist_camera.buffer.stats()
+    image = np.hstack(
+        (
+            self._preview_tile("GLOBAL D455", high, high_stats, cv2),
+            self._preview_tile("RIGHT WRIST D405", wrist, wrist_stats, cv2),
+        )
+    )
+
+    now_ns = self._now_ns()
+    high_age = (
+        f"{(now_ns - high.host_monotonic_ns) * 1e-6:.1f} ms"
+        if high is not None
+        else "-"
+    )
+    wrist_age = (
+        f"{(now_ns - wrist.host_monotonic_ns) * 1e-6:.1f} ms"
+        if wrist is not None
+        else "-"
+    )
+    skew = (
+        f"{abs(high.host_monotonic_ns - wrist.host_monotonic_ns) * 1e-6:.1f} ms"
+        if high is not None and wrist is not None
+        else "-"
+    )
+    episode = (
+        f"episode_{self._episode_index}"
+        if self._episode_index is not None
+        else "-"
+    )
+    samples = self._store.count if self._store is not None else 0
+    footer = np.zeros((80, image.shape[1], 3), dtype=np.uint8)
+    lines = (
+        f"Age high={high_age} wrist={wrist_age}  Camera skew={skew}",
+        f"ACT {self.state.value}  {episode}  Samples {samples}",
+    )
+    for index, line in enumerate(lines):
+        cv2.putText(
+            footer,
+            line,
+            (10, 30 + index * 32),
+            cv2.FONT_HERSHEY_SIMPLEX,
+            0.7,
+            (255, 255, 255),
+            1,
+            cv2.LINE_AA,
+        )
+    return np.vstack((image, footer))
+```
+
+若尚无帧，对应数值显示 `-`；该方法不修改任何录制器状态。
+
+- [ ] **步骤 7：实现预览线程生命周期和故障隔离**
+
+在相机成员创建前初始化：
+
+```python
+self._preview_stop = threading.Event()
+self._preview_thread: threading.Thread | None = None
+```
+
+两台相机成功启动后调用 `_start_preview()`：
+
+```python
+if self._camera_start_error is None:
+    self._start_preview()
+```
+
+实现：
+
+```python
+def _start_preview(self) -> None:
+    if not (os.environ.get("DISPLAY") or os.environ.get("WAYLAND_DISPLAY")):
+        self.get_logger().warn("未检测到桌面显示环境，ACT双相机预览已停用。")
+        return
+    try:
+        import cv2
+    except ImportError as exc:
+        self.get_logger().warn(f"OpenCV不可用，ACT双相机预览已停用：{exc}")
+        return
+    self._preview_stop.clear()
+    self._preview_thread = threading.Thread(
+        target=self._preview_loop,
+        args=(cv2,),
+        name="act_camera_preview",
+        daemon=True,
+    )
+    self._preview_thread.start()
+
+def _preview_loop(self, cv2) -> None:
+    try:
+        cv2.namedWindow(self.PREVIEW_WINDOW, cv2.WINDOW_NORMAL)
+        while not self._preview_stop.is_set():
+            cv2.imshow(self.PREVIEW_WINDOW, self._compose_preview(cv2))
+            key = cv2.waitKey(1) & 0xFF
+            if key in (ord("q"), ord("Q"), 27):
+                break
+            if cv2.getWindowProperty(
+                self.PREVIEW_WINDOW,
+                cv2.WND_PROP_VISIBLE,
+            ) < 1:
+                break
+            self._preview_stop.wait(0.1)
+    except Exception as exc:
+        self.get_logger().warn(f"ACT双相机预览已停用：{exc}")
+    finally:
+        try:
+            cv2.destroyWindow(self.PREVIEW_WINDOW)
+        except Exception:
+            pass
+
+def _stop_preview(self) -> None:
+    self._preview_stop.set()
+    if self._preview_thread is not None:
+        self._preview_thread.join(timeout=2.0)
+    self._preview_thread = None
+```
+
+`close()` 的第一步调用 `self._stop_preview()`，然后保持现有相机停止和目录锁释放顺序。
+
+- [ ] **步骤 8：运行预览相关测试并确认通过**
+
+运行：
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+pytest src/xr_rm_teleop/test/test_act_episode_recorder.py \
+  -k "rolling_fps or preview or close_stops_preview" -v
+```
+
+预期：所有选中测试通过，测试过程不打开真实窗口或相机。
+
+- [ ] **步骤 9：运行 ACT 录制器完整单元测试**
+
+运行：
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+pytest src/xr_rm_teleop/test/test_act_episode_recorder.py -v
+```
+
+预期：全部通过。
+
+- [ ] **步骤 10：提交任务 2**
+
+```bash
+git add src/xr_rm_teleop/xr_rm_teleop/act_episode_recorder.py \
+  src/xr_rm_teleop/test/test_act_episode_recorder.py
+git commit -m "feat: 添加ACT双相机实时预览"
+```
+
+### 任务 3：更新使用说明并完成工作空间验证
+
+**文件：**
+
+- 修改：`README.md:144-169`
+
+- [ ] **步骤 1：更新 ACT 数采说明**
+
+在 ACT episode 启动命令后补充：
+
+```markdown
+`record_act:=true` 启动后默认显示全局 D455 和右腕 D405 双路画面，并显示实时
+FPS、真实丢帧率、帧龄、双相机时间差、录制状态、episode 编号和样本数。按
+`Q`、`Esc` 或关闭窗口只会停止预览，ACT 相机采集和录制继续运行；没有桌面环境
+或 OpenCV 显示失败时也不会影响录制。
+
+相机采集线程观察到的真实掉帧仍会拒绝 episode。独立 `30 Hz` 控制和相机时钟
+造成的 ACT 样本重复/跨帧只写入 HDF5 质量指标，不再误报为相机丢包。
+```
+
+- [ ] **步骤 2：运行文档和 Python 静态检查**
+
+运行：
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+python -m py_compile \
+  src/xr_rm_teleop/xr_rm_teleop/act_episode_recorder.py \
+  src/xr_rm_teleop/test/test_act_episode_recorder.py
+git diff --check
+```
+
+预期：命令返回码为 `0`，没有语法或空白错误。
+
+- [ ] **步骤 3：运行项目要求的工作空间构建**
+
+运行：
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+colcon build --symlink-install
+```
+
+预期：所有包构建成功。不得在 `/home/robot/WS_xr/src` 中运行该命令。
+
+- [ ] **步骤 4：构建后再次运行 ACT 录制器测试**
+
+运行：
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+source install/setup.bash
+pytest src/xr_rm_teleop/test/test_act_episode_recorder.py -v
+```
+
+预期：全部通过。
+
+- [ ] **步骤 5：确认提交范围并提交任务 3**
+
+运行：
+
+```bash
+git status --short
+git diff -- README.md \
+  src/xr_rm_teleop/xr_rm_teleop/act_episode_recorder.py \
+  src/xr_rm_teleop/test/test_act_episode_recorder.py
+git add README.md
+git commit -m "docs: 更新ACT相机预览说明"
+```
+
+不得暂存或提交 `xr_rm_teleop/xr_rm_teleop/fun_peripheral.py`。
+
+## 现场验证
+
+自动化实施结束后，由用户在确认现场安全条件后运行现有 ACT 数采入口：
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+source install/setup.bash
+ros2 launch xr_rm_bringup arm_debug.launch.py \
+  arm:=right use_mock:=false record_act:=true
+```
+
+该命令会连接右臂真机，代理不得自行执行。现场确认：
+
+1. 双路画面默认出现且角色正确；
+2. FPS、真实丢帧率、帧龄、双相机时间差和 ACT 状态持续更新；
+3. 关闭窗口后录制状态和 HDF5 写入继续；
+4. 正常异步重复/跨帧的 episode 能保存；
+5. HDF5 属性包含真实采集质量与两路采样重复/跨帧指标。

docs/superpowers/specs/2026-08-13-act-camera-preview-alignment-design.md

@@ -0,0 +1,218 @@
+# ACT 双相机预览与采样质量判定修正设计
+
+## 背景
+
+右臂番茄采摘 ACT 采集当前以 `90 Hz` 接收原子控制消息，每三个控制周期生成一个
+`30 Hz` 样本，并为该样本选择不晚于控制时刻的最新全局 D455 和右腕 D405 RGB
+帧。现阶段测试暴露出两个相机相关问题：
+
+1. ACT 采样帧号偶尔出现 `986 → 986 → 988` 一类重复和跨帧，episode 因
+   `camera_sample_drop_ratio` 被拒绝；
+2. ACT 数采启动后没有现场预览，操作者不能直观看到两路画面、相机质量和录制
+   状态。
+
+已保存 HDF5 的调查结果表明，被 `camera_sample_drop_ratio` 拒绝的 episode 中，
+相机采集线程统计的 `camera_high_drop_ratio` 和
+`camera_right_wrist_drop_ratio` 均为 `0.0`。异常主要出现在独立运行的两个
+`30 Hz` 时钟之间：控制采样早于下一张相机帧时会再次选择上一帧，下一个采样点
+则可能选择更新两号的帧。相机采集线程实际收到过中间帧，因此这不是 RealSense
+传输丢帧。
+
+## 目标
+
+- 保持现有因果时间对齐，不选择控制时刻之后的图像；
+- 只用相机采集线程观察到的真实帧号缺失率判定相机丢帧；
+- 将 ACT 样本中的重复帧和跨帧改为可观测指标，不再据此拒绝 episode；
+- `record_act:=true` 启动录制器时默认显示全局 D455 和右腕 D405 实时画面；
+- 在预览中显示相机质量和 ACT 录制状态；
+- 预览关闭或显示故障不得影响相机采集、HDF5 写入和机器人控制。
+
+## 非目标
+
+本次不实现：
+
+- 原始视频流保存和离线重采样；
+- D455 与 D405 硬件同步；
+- ROS 图像话题、Web 界面或新的相机进程；
+- 深度图、点云、图像压缩或 HDF5 核心训练字段变更；
+- 关节曲线、机器人控制按钮或预览截图；
+- 夹爪实际开度反馈或 episode 终点裁剪逻辑修改；
+- 修改 `record_act` 的全局默认值；
+- 修改工作空间/圆柱限位、速度限制、指令超时、安全停止或真机连接行为。
+
+夹爪录制继续使用现有操作顺序：保持 Grip，按 Trigger 打开并等待打开命令完成，
+然后松开 Grip，最后按 B 保存。
+
+## 方案选择
+
+### 采用：保持当前因果对齐
+
+数据流保持为：
+
+```text
+90 Hz ActControlSample
+        ↓ 每三个连续控制周期选择一次
+30 Hz ACT 目标时刻
+        ↓ 分别选择 host_monotonic_ns 不晚于目标时刻的最新帧
+D455 图像 + D405 图像 + qpos + action
+        ↓
+现有 HDF5
+```
+
+该方案维持现有训练数据语义，图像不会包含控制时刻之后的未来信息。少量重复帧和
+跨帧作为异步时钟相位漂移保留在数据中，并用明确指标量化。
+
+### 未采用：以 D455 为软件主时钟
+
+该方案可以避免 D455 重复帧，但会使控制序号间隔不再固定，并需要重新定义状态、
+动作和 D405 图像的对齐语义，当前收益不足以覆盖兼容性成本。
+
+### 未采用：保存原始流并离线重采样
+
+该方案最灵活，但需要新的原始存储结构和转换工具，无压缩双路 RGB 也会显著增加
+存储开销。只有后续训练表明快速接触或释放动作受到当前单帧级时间抖动影响时，才
+考虑升级。
+
+## 相机质量指标
+
+### 真实采集质量
+
+`CameraBuffer` 在每次收到 RealSense 帧时比较相邻原始帧号。原始帧号向前跳过的
+数量计入真实丢帧数；帧号不递增单独计为回退/重启异常。episode 期间的统计写入
+现有或新增 HDF5 属性：
+
+- `camera_high_fps`；
+- `camera_right_wrist_fps`；
+- `camera_high_drop_ratio`；
+- `camera_right_wrist_drop_ratio`；
+- `camera_high_frame_number_regression_count`；
+- `camera_right_wrist_frame_number_regression_count`。
+
+以下条件继续拒绝 episode：
+
+- 任一路实际采集 FPS 小于配置的 `min_camera_fps`，当前为 `27 Hz`；
+- 任一路真实丢帧率大于配置的 `max_drop_ratio`，当前为 `1%`；
+- 任一路图像帧龄超过 `max_camera_age_ms`，当前为 `50 ms`；
+- 两路所选图像的主机单调时间差超过 `max_camera_skew_ms`，当前为 `50 ms`；
+- 任一路原始帧号发生回退或重启；
+- 相机启动、取帧或图像格式发生错误。
+
+### ACT 采样相位指标
+
+对每路写入 HDF5 的 ACT 样本帧号计算相邻差值：
+
+```text
+diff == 0：重复使用同一相机帧
+diff == 1：理想连续取样
+diff > 1：相邻 ACT 样本跨过相机帧
+diff < 0：帧号回退，仍按异常拒绝
+```
+
+对 `N` 个 ACT 样本，分母为 `max(1, N - 1)`：
+
+```text
+sample_repeat_ratio = count(diff == 0) / max(1, N - 1)
+sample_skip_ratio   = count(diff > 1)  / max(1, N - 1)
+```
+
+分别写入：
+
+- `cam_high_sample_repeat_ratio`；
+- `cam_high_sample_skip_ratio`；
+- `cam_wrist_sample_repeat_ratio`；
+- `cam_wrist_sample_skip_ratio`。
+
+这些指标写入保存/拒绝文件属性，并在保存日志中摘要输出，但不参与 episode 接受
+判定。原有 `camera_sample_drop_ratio` 拒绝路径移除，避免把软件采样相位漂移误报
+为相机传输丢帧。
+
+## 双路实时预览
+
+### 生命周期
+
+`ActEpisodeRecorder` 成功启动两台相机后，默认启动一个独立的 OpenCV 预览线程。
+该线程只读取两个现有 `CameraBuffer` 的最新帧和统计，不打开新的 RealSense
+pipeline，也不发布 ROS 图像话题。
+
+预览约以 `10 Hz` 刷新，降低显示开销；相机采集和 HDF5 录制仍保持 `30 Hz`。
+节点退出时先通知并回收预览线程，再停止两台相机。关闭预览不会重新启动。
+
+`arm_debug.launch.py` 继续保持 `record_act:=false` 的安全默认值。tools 中现有 ACT
+数采入口已经显式传入 `arm:=right use_mock:=false record_act:=true`，因此无需修改
+launch 参数或增加新的预览开关；只要录制器节点启动，预览就默认启动。
+
+### 布局与信息
+
+窗口使用左右并排的两块画面：
+
+```text
+┌──────────────────────┬──────────────────────┐
+│ 全局 D455            │ 右腕 D405            │
+│ 实时画面             │ 实时画面             │
+│ FPS / 当前帧号       │ FPS / 当前帧号       │
+│ 接收数 / 真实丢帧率  │ 接收数 / 真实丢帧率  │
+├──────────────────────┴──────────────────────┤
+│ 两路帧龄 / 两相机时间差                     │
+│ ACT 状态 / episode 编号 / 已写入样本数      │
+└─────────────────────────────────────────────┘
+```
+
+实时 FPS 使用相机采集线程最近约两秒的到帧时间计算，而不是预览刷新率。未开始
+episode 时编号和样本数显示为空或 `-`；录制过程中读取当前录制器状态。
+
+### 关闭和异常处理
+
+- 按 `Q`、`Esc` 或点击窗口关闭按钮，只停止预览线程；
+- 没有 `DISPLAY` 和 `WAYLAND_DISPLAY` 时不创建窗口，只记录一次警告；
+- `cv2` 导入失败、窗口创建失败或显示过程中抛出异常时，记录一次警告并停止
+  预览；
+- 预览异常不改变 ACT 状态，不关闭相机，不丢弃或拒绝 episode；
+- RealSense 相机本身启动或采集失败仍沿用现有预检/拒绝行为；
+- 预览线程不得调用机器人适配器、发布夹爪命令或阻塞 ROS 控制样本回调。
+
+本次复用 `xr` 运行环境中现有的 OpenCV，不新增 Python 或 ROS 依赖。
+
+## 代码范围
+
+预计只修改：
+
+- `xr_rm_teleop/xr_rm_teleop/act_episode_recorder.py`：真实丢帧与采样相位指标、
+  双路预览及生命周期；
+- `xr_rm_teleop/test/test_act_episode_recorder.py`：质量判定和预览失败隔离测试；
+- `README.md`：补充 ACT 默认双路预览及关闭方式。
+
+无需修改 `arm_debug.launch.py`、`launcher_ui.py`、ROS 消息、相机配置或机器人控制
+节点。实现继续保留在现有录制器文件中，只提取必要的纯计算/渲染辅助函数，不创建
+通用相机框架。
+
+## 测试与验证
+
+自动化测试不连接 RealSense、RM75 或真实夹爪：
+
+1. 构造 `986 → 986 → 988`，验证重复率和跨帧率均被记录，episode 不再因
+   `camera_sample_drop_ratio` 被拒绝；
+2. 在 `CameraBuffer` 原始输入中跳过帧号，验证真实丢帧数和丢帧率仍触发拒绝；
+3. 构造原始帧号回退，验证 episode 被拒绝；
+4. 验证两路采样指标分别计算，且分母在单样本时安全；
+5. 使用替代显示函数验证按键关闭、窗口关闭、无显示环境和显示异常只停用预览，
+   不改变录制状态；
+6. 运行 `xr_rm_teleop/test/test_act_episode_recorder.py`；
+7. 从 `/home/robot/WS_xr` 执行：
+
+   ```bash
+   source /opt/ros/humble/setup.bash
+   colcon build --symlink-install
+   ```
+
+现场再通过现有 ACT 数采入口验证窗口布局、画面刷新和关闭行为。该验证会连接右臂
+真机，必须由用户明确执行；自动化过程不得启动真机 launch 或移动机器人。
+
+## 完成标准
+
+- 真实相机丢帧、帧龄超限、双相机偏差和相机错误仍能拒绝不合格 episode；
+- 正常异步相位漂移造成的样本重复/跨帧不再拒绝 episode；
+- HDF5 和日志能够区分真实丢帧与 ACT 采样相位指标；
+- ACT 数采启动时默认出现全局 D455 与右腕 D405 双路预览；
+- 关闭或损坏预览不影响 ACT 采集；
+- 现有 HDF5 核心结构、机器人控制和安全行为保持不变；
+- 相关测试与工作空间构建通过。

xr_rm_bringup/config/dual_arm_rm75.yaml

@@ -45,7 +45,7 @@ left_arm_teleop:
     realtime_push_host_ip: 192.168.192.148
     realtime_push_port: 8089
     realtime_push_cycle_ms: 5
-    avoid_singularity: 1
+    avoid_singularity: 0
     follow: false
     canfd_trajectory_mode: 2
     canfd_radio: 0
@@ -102,7 +102,7 @@ right_arm_teleop:
     realtime_push_host_ip: 192.168.192.148
     realtime_push_port: 8090
     realtime_push_cycle_ms: 5
-    avoid_singularity: 1
+    avoid_singularity: 0
     follow: false
     canfd_trajectory_mode: 2
     canfd_radio: 0

xr_rm_bringup/config/left_arm_rm75.yaml

@@ -38,7 +38,7 @@ single_arm_velocity_teleop:
     realtime_push_host_ip: 192.168.192.148
     realtime_push_port: 8089
     realtime_push_cycle_ms: 5
-    avoid_singularity: 1
+    avoid_singularity: 0
     follow: false
     canfd_trajectory_mode: 2
     canfd_radio: 0

xr_rm_bringup/config/right_arm_rm75.yaml

@@ -37,7 +37,7 @@ single_arm_velocity_teleop:
     realtime_push_host_ip: 192.168.192.148
     realtime_push_port: 8090
     realtime_push_cycle_ms: 5
-    avoid_singularity: 1
+    avoid_singularity: 0
     # 厂商 MovejCANFD 示例默认低跟随；高跟随仅在验证规划轨迹后单独开启。
     follow: false
     canfd_trajectory_mode: 2

xr_rm_bringup/test/test_launcher_ui_cleanup.py

@@ -46,6 +46,14 @@ class LauncherCommandsTest(unittest.TestCase):
                 "Open ROS Topic/Node List Monitor",
                 "Open Controller Topic Monitor",
             ],
+            "ACT Data Collection": [
+                "Right Arm ACT Data Collection Launch",
+                "XRobotoolkit UDP Bridge (90 Hz)",
+                "Open ACT Recording Status",
+                "Open Right Arm ACT Control Sample Hz",
+                "Open ROS Topic/Node List Monitor",
+                "Open Controller Topic Monitor",
+            ],
             "Diagnostics": [
                 "ROS Doctor Report",
                 "XR-RM Bringup Prefix",
@@ -79,6 +87,22 @@ class LauncherCommandsTest(unittest.TestCase):
             commands["2. Dual Arm MuJoCo Real Hardware Launch"],
         )
 
+    def test_act_mode_uses_confirmed_right_hardware_topics(self) -> None:
+        commands = dict(launcher_ui.build_commands_by_mode("ACT Data Collection"))
+
+        self.assertIn(
+            "arm:=right use_mock:=false record_act:=true",
+            commands["1. Right Arm ACT Data Collection Launch"],
+        )
+        self.assertEqual(
+            commands["3. Open ACT Recording Status"],
+            "ros2 topic echo /act/recording_status",
+        )
+        self.assertEqual(
+            commands["4. Open Right Arm ACT Control Sample Hz"],
+            "ros2 topic hz /xr_rm/right_rm75/act_control_sample",
+        )
+
     def test_cmd_vel_monitor_is_completely_removed(self) -> None:
         self.assertFalse(hasattr(launcher_ui, "CMD_VEL_MONITOR_ACTION"))
         for mode in launcher_ui.MODES:

xr_rm_bringup/tools/launcher_ui.py

@@ -1,7 +1,7 @@
 #!/usr/bin/env python3
 """XR-RM 桌面调试启动器。
 
-提供 Tkinter 图形界面，按“仿真/MuJoCo/真机/诊断”组织常用 ROS2 launch、
+提供 Tkinter 图形界面，按“仿真/MuJoCo/真机/ACT采集/诊断”组织常用 ROS2 launch、
 sample_udp_sender、topic 监控和环境检查命令，降低现场调试时的命令输入成本。
 """
 
@@ -76,6 +76,7 @@ MODES = [
     "Simulation",
     "MuJoCo",
     "Real Hardware",
+    "ACT Data Collection",
     "Diagnostics",
 ]
 
@@ -275,6 +276,23 @@ def build_commands_by_mode(mode: str) -> list[tuple[str, str]]:
             ("Right Gripper Open", _tool_command("right", True)),
             ("Right Gripper Close", _tool_command("right", False)),
         ]
+    elif mode == "ACT Data Collection":
+        items = [
+            (
+                "Right Arm ACT Data Collection Launch",
+                "ros2 launch xr_rm_bringup arm_debug.launch.py "
+                "arm:=right use_mock:=false record_act:=true",
+            ),
+            ("XRobotoolkit UDP Bridge (90 Hz)", _xrobotoolkit_bridge_command()),
+            (
+                "Open ACT Recording Status",
+                "ros2 topic echo /act/recording_status",
+            ),
+            (
+                "Open Right Arm ACT Control Sample Hz",
+                "ros2 topic hz /xr_rm/right_rm75/act_control_sample",
+            ),
+        ]
     else:
         items = [
             ("ROS Doctor Report", "ros2 doctor --report"),
@@ -329,7 +347,7 @@ class LauncherApp:
             mode_frame,
             textvariable=self.mode_var,
             values=MODES,
-            width=16,
+            width=20,
             state="readonly",
         )
         self.mode_combo.grid(row=0, column=1, sticky="ew")

xr_rm_teleop/test/test_act_episode_recorder.py

@@ -1,5 +1,6 @@
 import queue
 import threading
+import time
 from types import SimpleNamespace
 
 import numpy as np
@@ -29,6 +30,7 @@ from xr_rm_teleop.act_episode_recorder import (
     next_episode_index,
     publish_without_overwrite,
     recover_partial_files,
+    sample_frame_metrics,
     select_camera_pair,
     select_frame,
     validate_episode,
@@ -296,6 +298,50 @@ def test_camera_buffer_is_bounded_and_counts_dropped_frames():
     assert stats.fps == pytest.approx(800.0)
 
 
+def test_camera_buffer_counts_frame_number_regressions():
+    buffer = CameraBuffer(maxlen=4)
+    for frame_number in (100, 101, 1, 2):
+        buffer.push(
+            CameraFrame(
+                _image(frame_number),
+                frame_number,
+                float(frame_number),
+                time.monotonic_ns(),
+            )
+        )
+
+    stats = buffer.stats()
+
+    assert stats.frame_number_regression_count == 1
+    assert stats.dropped_frames == 0
+
+
+def test_sample_frame_metrics_separate_repeats_skips_and_regressions():
+    repeat, skip, regression = sample_frame_metrics(
+        np.asarray((986, 986, 988), dtype=np.uint64)
+    )
+
+    assert repeat == pytest.approx(0.5)
+    assert skip == pytest.approx(0.5)
+    assert regression == 0
+
+
+def test_camera_buffer_reports_two_second_rolling_fps():
+    buffer = CameraBuffer(maxlen=4)
+    start_ns = 10_000_000_000
+    for index in range(61):
+        buffer.push(
+            CameraFrame(
+                _image(index),
+                index,
+                float(index),
+                start_ns + index * 33_333_333,
+            )
+        )
+
+    assert buffer.stats().rolling_fps == pytest.approx(30.0, rel=0.02)
+
+
 requires_h5py = pytest.mark.skipif(
     h5py is None,
     reason="h5py is not installed",
@@ -506,6 +552,8 @@ def _valid_episode(tmp_path):
         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
+        root.attrs["camera_high_frame_number_regression_count"] = 0
+        root.attrs["camera_right_wrist_frame_number_regression_count"] = 0
     return path
 
 
@@ -518,6 +566,19 @@ def test_validate_episode_accepts_valid_file(tmp_path):
     assert report.metrics["control_hz"] == pytest.approx(30.0, rel=1e-5)
 
 
+@requires_h5py
+def test_validate_episode_accepts_async_camera_phase_drift(tmp_path):
+    path = _valid_episode(tmp_path)
+    with h5py.File(path, "r+") as root:
+        root["debug/cameras/cam_high_frame_number"][:] = (986, 986, 988)
+
+    report = validate_episode(path, _quality_limits())
+
+    assert report.accepted
+    assert report.metrics["cam_high_sample_repeat_ratio"] == pytest.approx(0.5)
+    assert report.metrics["cam_high_sample_skip_ratio"] == pytest.approx(0.5)
+
+
 def _mutate_episode(path, mutation):
     with h5py.File(path, "r+") as root:
         if mutation == "short_episode":
@@ -547,6 +608,8 @@ def _mutate_episode(path, mutation):
             root.attrs["camera_high_fps"] = 20.0
         elif mutation == "camera_drop":
             root.attrs["camera_right_wrist_drop_ratio"] = 0.02
+        elif mutation == "camera_frame_regression":
+            root.attrs["camera_high_frame_number_regression_count"] = 1
         elif mutation == "camera_age":
             root["debug/timestamps/cam_high_age_ms"][1] = 60.0
         elif mutation == "camera_skew":
@@ -571,6 +634,7 @@ def _mutate_episode(path, mutation):
         ("control_fault", "control_fault"),
         ("camera_fps", "camera_fps"),
         ("camera_drop", "camera_drop_ratio"),
+        ("camera_frame_regression", "camera_frame_number_regression"),
         ("camera_age", "camera_frame_too_old"),
         ("camera_skew", "camera_skew"),
         ("final_gripper_closed", "final_gripper_not_open"),
@@ -616,14 +680,19 @@ class _StatusPublisher:
 
 
 class _Logger:
-    def info(self, *_args, **_kwargs):
-        pass
+    def __init__(self):
+        self.infos = []
+        self.warnings = []
+        self.errors = []
 
-    def warn(self, *_args, **_kwargs):
-        pass
+    def info(self, message, *_args, **_kwargs):
+        self.infos.append(message)
 
-    def error(self, *_args, **_kwargs):
-        pass
+    def warn(self, message, *_args, **_kwargs):
+        self.warnings.append(message)
+
+    def error(self, message, *_args, **_kwargs):
+        self.errors.append(message)
 
 
 def _control_message(seq, control_ns, *, grip=True):
@@ -705,10 +774,53 @@ def _recorder_for_test(tmp_path):
     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()
+    recorder._logger = _Logger()
+    recorder.get_logger = lambda: recorder._logger
     return recorder
 
 
+def test_preview_failure_does_not_change_recording_state():
+    recorder = object.__new__(ActEpisodeRecorder)
+    recorder._session = _recording_session()
+    recorder._preview_stop = threading.Event()
+    recorder._preview_thread = None
+    recorder._logger = _Logger()
+    recorder.get_logger = lambda: recorder._logger
+
+    class FailingCv2:
+        WINDOW_NORMAL = 0
+
+        @staticmethod
+        def namedWindow(*_args):
+            raise RuntimeError("no display")
+
+    recorder._preview_loop(FailingCv2())
+
+    assert recorder.state is RecordingState.RECORDING
+    assert recorder._logger.warnings == [
+        "ACT双相机预览已停用：no display"
+    ]
+
+
+def test_close_stops_preview_before_cameras_and_releases_lock():
+    events = []
+    recorder = object.__new__(ActEpisodeRecorder)
+    recorder._stop_preview = lambda: events.append("preview")
+    recorder._high_camera = SimpleNamespace(
+        stop=lambda: events.append("high")
+    )
+    recorder._wrist_camera = SimpleNamespace(
+        stop=lambda: events.append("wrist")
+    )
+    recorder._directory_lock = SimpleNamespace(
+        release=lambda: events.append("lock")
+    )
+
+    recorder.close()
+
+    assert events == ["preview", "high", "wrist", "lock"]
+
+
 @requires_h5py
 def test_preflight_requires_open_gripper_fresh_inputs_and_disk_space(tmp_path):
     recorder = _recorder_for_test(tmp_path)
@@ -737,8 +849,8 @@ def _push_recording_frames(recorder, control_ns, frame_number):
     recorder._wrist_camera.buffer.push(
         CameraFrame(
             _image(3),
-            frame_number,
-            float(frame_number),
+            frame_number + 1000,
+            float(frame_number + 1000),
             control_ns - 3_000_000,
         )
     )
@@ -776,6 +888,14 @@ def test_end_to_end_fake_episode_saves_and_returns_idle(tmp_path):
     assert recorder.state is RecordingState.IDLE
     assert "SAVING" in recorder._status_pub.messages
     assert recorder._status_pub.messages[-2:] == ["SAVED", "IDLE"]
+    assert any(
+        "ACT录制状态：RECORDING，episode_0" in message
+        for message in recorder._logger.infos
+    )
+    assert any(
+        "episode_0.hdf5（3 samples）" in message
+        for message in recorder._logger.infos
+    )
 
 
 @requires_h5py
@@ -791,6 +911,11 @@ def test_final_publish_rejects_allocated_episode_number_conflict(tmp_path):
     assert not (recorder._task_dir / "episode_1.hdf5").exists()
     rejected = list((recorder._task_dir / "rejected").glob("*.hdf5"))
     assert len(rejected) == 1
+    assert any(
+        str(rejected[0]) in message
+        and "原因：episode_number_conflict" in message
+        for message in recorder._logger.warnings
+    )
     with h5py.File(rejected[0], "r") as root:
         assert root.attrs["reject_reason"] == "episode_number_conflict"
 

xr_rm_teleop/xr_rm_teleop/act_episode_recorder.py

@@ -405,6 +405,32 @@ def _quality_failure(
     return QualityReport(False, reason, metrics)
 
 
+def sample_frame_metrics(
+    frame_numbers: np.ndarray,
+) -> tuple[float, float, int]:
+    diffs = np.diff(np.asarray(frame_numbers, dtype=np.int64))
+    denominator = max(1, len(diffs))
+    return (
+        float(np.count_nonzero(diffs == 0) / denominator),
+        float(np.count_nonzero(diffs > 1) / denominator),
+        int(np.count_nonzero(diffs < 0)),
+    )
+
+
+def episode_sample_frame_metrics(root: Any) -> dict[str, int | float]:
+    metrics: dict[str, int | float] = {}
+    for camera in ("cam_high", "cam_wrist"):
+        repeat, skip, regression = sample_frame_metrics(
+            root[f"debug/cameras/{camera}_frame_number"][:]
+        )
+        metrics[f"{camera}_sample_repeat_ratio"] = repeat
+        metrics[f"{camera}_sample_skip_ratio"] = skip
+        metrics[f"{camera}_sample_frame_number_regression_count"] = (
+            regression
+        )
+    return metrics
+
+
 def validate_episode(path: Path, limits: QualityLimits) -> QualityReport:
     metrics: dict[str, int | float] = {}
     try:
@@ -541,6 +567,20 @@ def validate_episode(path: Path, limits: QualityLimits) -> QualityReport:
             if reason == "camera_drop_ratio" and value > threshold:
                 return _quality_failure(reason, metrics)
 
+        for name in (
+            "camera_high_frame_number_regression_count",
+            "camera_right_wrist_frame_number_regression_count",
+        ):
+            if name not in root.attrs:
+                return _quality_failure("camera_stats_missing", metrics)
+            value = int(root.attrs[name])
+            metrics[name] = value
+            if value:
+                return _quality_failure(
+                    "camera_frame_number_regression",
+                    metrics,
+                )
+
         high_age_ms = datasets[
             "debug/timestamps/cam_high_age_ms"
         ][:]
@@ -564,15 +604,15 @@ def validate_episode(path: Path, limits: QualityLimits) -> QualityReport:
         ):
             return _quality_failure("camera_skew", metrics)
 
-        for camera in ("cam_high", "cam_wrist"):
-            frame_numbers = datasets[
-                f"debug/cameras/{camera}_frame_number"
-            ][:].astype(np.int64)
-            discontinuities = int((np.diff(frame_numbers) != 1).sum())
-            ratio = discontinuities / max(1, sample_count - 1)
-            metrics[f"{camera}_sample_discontinuity_ratio"] = float(ratio)
-            if ratio > limits.max_drop_ratio:
-                return _quality_failure("camera_sample_drop_ratio", metrics)
+        sample_metrics = episode_sample_frame_metrics(root)
+        metrics.update(sample_metrics)
+        if any(
+            sample_metrics[
+                f"{camera}_sample_frame_number_regression_count"
+            ]
+            for camera in ("cam_high", "cam_wrist")
+        ):
+            return _quality_failure("camera_frame_number_regression", metrics)
 
         if qpos[-1, 7] != 1.0:
             return _quality_failure("final_gripper_not_open", metrics)
@@ -595,8 +635,10 @@ class CameraFrame:
 class CameraStats:
     frame_count: int
     dropped_frames: int
+    frame_number_regression_count: int
     first_host_monotonic_ns: int | None
     last_host_monotonic_ns: int | None
+    recent_host_monotonic_ns: tuple[int, ...]
 
     @property
     def drop_ratio(self) -> float:
@@ -614,6 +656,20 @@ class CameraStats:
             return 0.0
         return (self.frame_count - 1) * 1e9 / elapsed_ns
 
+    @property
+    def rolling_fps(self) -> float:
+        if len(self.recent_host_monotonic_ns) < 2:
+            return 0.0
+        elapsed_ns = (
+            self.recent_host_monotonic_ns[-1]
+            - self.recent_host_monotonic_ns[0]
+        )
+        if elapsed_ns <= 0:
+            return 0.0
+        return (
+            (len(self.recent_host_monotonic_ns) - 1) * 1e9 / elapsed_ns
+        )
+
 
 class CameraBuffer:
     def __init__(self, *, maxlen: int = 4) -> None:
@@ -623,24 +679,33 @@ class CameraBuffer:
         self._lock = threading.Lock()
         self._frame_count = 0
         self._dropped_frames = 0
+        self._frame_number_regression_count = 0
         self._first_host_monotonic_ns: int | None = None
         self._last_host_monotonic_ns: int | None = None
         self._last_frame_number: int | None = None
+        self._recent_host_monotonic_ns: deque[int] = deque()
 
     def push(self, frame: CameraFrame) -> None:
         with self._lock:
-            if (
-                self._last_frame_number is not None
-                and frame.frame_number > self._last_frame_number + 1
-            ):
-                self._dropped_frames += (
-                    frame.frame_number - self._last_frame_number - 1
-                )
+            if self._last_frame_number is not None:
+                if frame.frame_number <= self._last_frame_number:
+                    self._frame_number_regression_count += 1
+                elif frame.frame_number > self._last_frame_number + 1:
+                    self._dropped_frames += (
+                        frame.frame_number - self._last_frame_number - 1
+                    )
             self._last_frame_number = frame.frame_number
             self._frame_count += 1
             if self._first_host_monotonic_ns is None:
                 self._first_host_monotonic_ns = frame.host_monotonic_ns
             self._last_host_monotonic_ns = frame.host_monotonic_ns
+            self._recent_host_monotonic_ns.append(frame.host_monotonic_ns)
+            cutoff_ns = frame.host_monotonic_ns - 2_000_000_000
+            while (
+                self._recent_host_monotonic_ns
+                and self._recent_host_monotonic_ns[0] < cutoff_ns
+            ):
+                self._recent_host_monotonic_ns.popleft()
             self._frames.append(frame)
 
     def snapshot(self) -> tuple[CameraFrame, ...]:
@@ -652,8 +717,14 @@ class CameraBuffer:
             return CameraStats(
                 frame_count=self._frame_count,
                 dropped_frames=self._dropped_frames,
+                frame_number_regression_count=(
+                    self._frame_number_regression_count
+                ),
                 first_host_monotonic_ns=self._first_host_monotonic_ns,
                 last_host_monotonic_ns=self._last_host_monotonic_ns,
+                recent_host_monotonic_ns=tuple(
+                    self._recent_host_monotonic_ns
+                ),
             )
 
 
@@ -1004,6 +1075,8 @@ def _twist_values(twist: Any) -> np.ndarray:
 
 
 class ActEpisodeRecorder(Node):
+    PREVIEW_WINDOW = "ACT - D455 Global / D405 Right Wrist"
+
     def __init__(self) -> None:
         super().__init__("act_episode_recorder")
         defaults = {
@@ -1102,6 +1175,8 @@ class ActEpisodeRecorder(Node):
         self._episode_index: int | None = None
         self._camera_baselines: tuple[CameraStats, CameraStats] | None = None
         self._saving_deadline_ns: int | None = None
+        self._preview_stop = threading.Event()
+        self._preview_thread: threading.Thread | None = None
 
         self._high_camera = RealSenseCamera(
             str(parameters["cam_high_serial"]),
@@ -1118,6 +1193,8 @@ class ActEpisodeRecorder(Node):
         except Exception as exc:
             self._camera_start_error = str(exc)
             self.get_logger().error(f"ACT相机启动失败：{exc}")
+        if self._camera_start_error is None:
+            self._start_preview()
 
         self._status_pub = self.create_publisher(
             String,
@@ -1142,6 +1219,13 @@ class ActEpisodeRecorder(Node):
             self._on_left_controller,
             10,
         )
+        self.get_logger().info(
+            f"ACT录制器已启动，输出目录：{self._task_dir}"
+        )
+        self.get_logger().info(
+            "操作提示：右手B准备/结束录制；准备后握住右手Grip开始采样；"
+            "左手Y长按1秒丢弃；录制中不要按右手A"
+        )
         self._publish_state(RecordingState.IDLE)
 
     @staticmethod
@@ -1174,6 +1258,158 @@ class ActEpisodeRecorder(Node):
     def state(self) -> RecordingState:
         return self._session.state
 
+    @staticmethod
+    def _preview_tile(
+        role: str,
+        frame: CameraFrame | None,
+        stats: CameraStats,
+        cv2: Any,
+    ) -> np.ndarray:
+        if frame is None:
+            tile = np.zeros((480, 640, 3), dtype=np.uint8)
+            frame_number = "-"
+        else:
+            tile = cv2.cvtColor(frame.image, cv2.COLOR_RGB2BGR)
+            frame_number = str(frame.frame_number)
+        tile = tile.copy()
+        cv2.rectangle(tile, (0, 0), (640, 74), (0, 0, 0), -1)
+        lines = (
+            f"{role}  FPS {stats.rolling_fps:.1f}  Frame {frame_number}",
+            f"Received {stats.frame_count}  Dropped "
+            f"{stats.dropped_frames} ({stats.drop_ratio:.2%})",
+        )
+        for index, line in enumerate(lines):
+            cv2.putText(
+                tile,
+                line,
+                (10, 28 + index * 30),
+                cv2.FONT_HERSHEY_SIMPLEX,
+                0.65,
+                (255, 255, 255),
+                1,
+                cv2.LINE_AA,
+            )
+        return tile
+
+    def _compose_preview(self, cv2: Any) -> np.ndarray:
+        high_frames = self._high_camera.buffer.snapshot()
+        wrist_frames = self._wrist_camera.buffer.snapshot()
+        high = high_frames[-1] if high_frames else None
+        wrist = wrist_frames[-1] if wrist_frames else None
+        image = np.hstack(
+            (
+                self._preview_tile(
+                    "GLOBAL D455",
+                    high,
+                    self._high_camera.buffer.stats(),
+                    cv2,
+                ),
+                self._preview_tile(
+                    "RIGHT WRIST D405",
+                    wrist,
+                    self._wrist_camera.buffer.stats(),
+                    cv2,
+                ),
+            )
+        )
+
+        now_ns = self._now_ns()
+        high_age = (
+            f"{(now_ns - high.host_monotonic_ns) * 1e-6:.1f} ms"
+            if high is not None
+            else "-"
+        )
+        wrist_age = (
+            f"{(now_ns - wrist.host_monotonic_ns) * 1e-6:.1f} ms"
+            if wrist is not None
+            else "-"
+        )
+        skew = (
+            f"{abs(high.host_monotonic_ns - wrist.host_monotonic_ns) * 1e-6:.1f} ms"
+            if high is not None and wrist is not None
+            else "-"
+        )
+        episode_index = self._episode_index
+        episode = (
+            f"episode_{episode_index}"
+            if episode_index is not None
+            else "-"
+        )
+        store = self._store
+        samples = store.count if store is not None else 0
+        footer = np.zeros((80, image.shape[1], 3), dtype=np.uint8)
+        lines = (
+            f"Age high={high_age} wrist={wrist_age}  Camera skew={skew}",
+            f"ACT {self.state.value}  {episode}  Samples {samples}",
+        )
+        for index, line in enumerate(lines):
+            cv2.putText(
+                footer,
+                line,
+                (10, 30 + index * 32),
+                cv2.FONT_HERSHEY_SIMPLEX,
+                0.7,
+                (255, 255, 255),
+                1,
+                cv2.LINE_AA,
+            )
+        return np.vstack((image, footer))
+
+    def _start_preview(self) -> None:
+        if not (
+            os.environ.get("DISPLAY") or os.environ.get("WAYLAND_DISPLAY")
+        ):
+            self.get_logger().warn(
+                "未检测到桌面显示环境，ACT双相机预览已停用。"
+            )
+            return
+        try:
+            import cv2
+        except ImportError as exc:
+            self.get_logger().warn(
+                f"OpenCV不可用，ACT双相机预览已停用：{exc}"
+            )
+            return
+        self._preview_stop.clear()
+        self._preview_thread = threading.Thread(
+            target=self._preview_loop,
+            args=(cv2,),
+            name="act_camera_preview",
+            daemon=True,
+        )
+        self._preview_thread.start()
+
+    def _preview_loop(self, cv2: Any) -> None:
+        try:
+            cv2.namedWindow(self.PREVIEW_WINDOW, cv2.WINDOW_NORMAL)
+            while not self._preview_stop.is_set():
+                cv2.imshow(
+                    self.PREVIEW_WINDOW,
+                    self._compose_preview(cv2),
+                )
+                key = cv2.waitKey(1) & 0xFF
+                if key in (ord("q"), ord("Q"), 27):
+                    break
+                if cv2.getWindowProperty(
+                    self.PREVIEW_WINDOW,
+                    cv2.WND_PROP_VISIBLE,
+                ) < 1:
+                    break
+                self._preview_stop.wait(0.1)
+        except Exception as exc:
+            self.get_logger().warn(f"ACT双相机预览已停用：{exc}")
+        finally:
+            try:
+                cv2.destroyWindow(self.PREVIEW_WINDOW)
+            except Exception:
+                pass
+
+    def _stop_preview(self) -> None:
+        self._preview_stop.set()
+        if self._preview_thread is not None:
+            self._preview_thread.join(timeout=2.0)
+        self._preview_thread = None
+
     def _publish_state(
         self,
         state: RecordingState,
@@ -1182,6 +1418,12 @@ class ActEpisodeRecorder(Node):
         message = String()
         message.data = state.value if not reason else f"{state.value}:{reason}"
         self._status_pub.publish(message)
+        episode = (
+            f"，episode_{self._episode_index}"
+            if self._episode_index is not None
+            else ""
+        )
+        self.get_logger().info(f"ACT录制状态：{message.data}{episode}")
 
     def _run_preflight(self) -> str | None:
         now_ns = self._now_ns()
@@ -1460,14 +1702,19 @@ class ActEpisodeRecorder(Node):
     def _interval_camera_metrics(
         baseline: CameraStats,
         current: CameraStats,
-    ) -> tuple[float, float]:
+    ) -> tuple[float, float, int]:
         frames = max(0, current.frame_count - baseline.frame_count)
         dropped = max(0, current.dropped_frames - baseline.dropped_frames)
+        regressions = max(
+            0,
+            current.frame_number_regression_count
+            - baseline.frame_number_regression_count,
+        )
         if (
             baseline.last_host_monotonic_ns is None
             or current.last_host_monotonic_ns is None
         ):
-            return 0.0, 1.0
+            return 0.0, 1.0, regressions
         elapsed_ns = (
             current.last_host_monotonic_ns
             - baseline.last_host_monotonic_ns
@@ -1475,7 +1722,7 @@ class ActEpisodeRecorder(Node):
         fps = frames * 1e9 / elapsed_ns if elapsed_ns > 0 else 0.0
         expected = frames + dropped
         drop_ratio = dropped / expected if expected else 1.0
-        return fps, drop_ratio
+        return fps, drop_ratio, regressions
 
     def _write_camera_metrics(self) -> None:
         assert self._store is not None
@@ -1492,8 +1739,12 @@ class ActEpisodeRecorder(Node):
             {
                 "camera_high_fps": high[0],
                 "camera_high_drop_ratio": high[1],
+                "camera_high_frame_number_regression_count": high[2],
                 "camera_right_wrist_fps": wrist[0],
                 "camera_right_wrist_drop_ratio": wrist[1],
+                "camera_right_wrist_frame_number_regression_count": (
+                    wrist[2]
+                ),
             }
         )
 
@@ -1557,6 +1808,17 @@ class ActEpisodeRecorder(Node):
         except FileExistsError:
             self._reject_closed_partial("episode_number_conflict")
             return
+        self.get_logger().info(
+            f"ACT数据已保存：{destination}"
+            f"（{report.metrics['sample_count']} samples）"
+        )
+        self.get_logger().info(
+            "ACT相机采样相位："
+            f"high重复={report.metrics['cam_high_sample_repeat_ratio']:.2%}, "
+            f"high跨帧={report.metrics['cam_high_sample_skip_ratio']:.2%}, "
+            f"wrist重复={report.metrics['cam_wrist_sample_repeat_ratio']:.2%}, "
+            f"wrist跨帧={report.metrics['cam_wrist_sample_skip_ratio']:.2%}"
+        )
         self._finish_result(RecordingState.SAVED)
 
     def _reject_closed_partial(
@@ -1570,6 +1832,9 @@ class ActEpisodeRecorder(Node):
             root.attrs["episode_status"] = "rejected"
             root.attrs["reject_reason"] = reason
             root.attrs["interrupted"] = np.bool_(interrupted)
+            for name, value in episode_sample_frame_metrics(root).items():
+                root.attrs[name] = value
+            sample_count = int(root["action"].shape[0])
         rejected = self._task_dir / "rejected"
         rejected.mkdir(exist_ok=True)
         safe_reason = re.sub(r"[^a-zA-Z0-9_-]", "_", reason)
@@ -1581,6 +1846,10 @@ class ActEpisodeRecorder(Node):
             / f"episode_{episode_index}_{safe_reason}_{timestamp}.hdf5"
         )
         publish_without_overwrite(self._partial_path, destination)
+        self.get_logger().warn(
+            f"ACT数据已拒绝：{destination}（{sample_count} samples，"
+            f"原因：{reason}）"
+        )
         self._finish_result(RecordingState.REJECTED, reason)
 
     def _reject_current(
@@ -1609,11 +1878,16 @@ class ActEpisodeRecorder(Node):
     def _discard_current(self) -> None:
         if self._partial_path is None:
             return
+        partial = self._partial_path
         if self._writer is not None:
             self._writer.finish()
+        sample_count = self._store.count if self._store is not None else 0
         if self._store is not None:
             self._store.close()
-        discard_partial(self._partial_path)
+        discard_partial(partial)
+        self.get_logger().info(
+            f"ACT数据已丢弃：{partial}（{sample_count} samples）"
+        )
         self._finish_result(RecordingState.DISCARDED)
 
     def _finish_result(
@@ -1639,6 +1913,7 @@ class ActEpisodeRecorder(Node):
             self._reject_current(reason, interrupted=True)
 
     def close(self) -> None:
+        self._stop_preview()
         self._high_camera.stop()
         self._wrist_camera.stop()
         self._directory_lock.release()

xr_rm_teleop/xr_rm_teleop/fun_peripheral.py

@@ -218,7 +218,7 @@ def peripheral_cfg(
 
         addr = 1
         # 依次设置目标速度、目标力矩、目标加速度和目标减速度。
-        reg_value = [255, 60, 255, 255]
+        reg_value = [255, 150, 255, 255]
         for i, reg_addr in enumerate([11, 12, 13, 14]):
             write_params = rm_peripheral_read_write_params_t(1, reg_addr, addr, 1)
             robot.rm_write_single_register(write_params, reg_value[i])