feat: 添加ACT双相机实时预览

xr_rm_teleop/test/test_act_episode_recorder.py

@@ -326,6 +326,22 @@ def test_sample_frame_metrics_separate_repeats_skips_and_regressions():
     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",
@@ -763,6 +779,48 @@ def _recorder_for_test(tmp_path):
     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)
@@ -791,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,
         )
     )

xr_rm_teleop/xr_rm_teleop/act_episode_recorder.py

@@ -638,6 +638,7 @@ class CameraStats:
     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:
@@ -655,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:
@@ -668,6 +683,7 @@ class CameraBuffer:
         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:
@@ -683,6 +699,13 @@ class CameraBuffer:
             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, ...]:
@@ -699,6 +722,9 @@ class CameraBuffer:
                 ),
                 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
+                ),
             )
 
 
@@ -1049,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 = {
@@ -1147,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"]),
@@ -1163,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,
@@ -1226,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,
@@ -1729,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()