From 676b33bfe446eff0328d6daf868d2e167a0f4003 Mon Sep 17 00:00:00 2001 From: YikaiFu-cart Date: Thu, 13 Aug 2026 22:12:23 +0800 Subject: [PATCH] =?UTF-8?q?fix:=20=E4=BF=AE=E6=AD=A3ACT=E7=9B=B8=E6=9C=BA?= =?UTF-8?q?=E9=87=87=E6=A0=B7=E8=B4=A8=E9=87=8F=E5=88=A4=E5=AE=9A?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../test/test_act_episode_recorder.py | 48 +++++++++ .../xr_rm_teleop/act_episode_recorder.py | 101 ++++++++++++++---- 2 files changed, 130 insertions(+), 19 deletions(-) diff --git a/xr_rm_teleop/test/test_act_episode_recorder.py b/xr_rm_teleop/test/test_act_episode_recorder.py index cf7d255..bb864e4 100644 --- a/xr_rm_teleop/test/test_act_episode_recorder.py +++ b/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,34 @@ 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 + + requires_h5py = pytest.mark.skipif( h5py is None, reason="h5py is not installed", @@ -506,6 +536,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 +550,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 +592,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 +618,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"), diff --git a/xr_rm_teleop/xr_rm_teleop/act_episode_recorder.py b/xr_rm_teleop/xr_rm_teleop/act_episode_recorder.py index b22b3ad..7ffd417 100644 --- a/xr_rm_teleop/xr_rm_teleop/act_episode_recorder.py +++ b/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,6 +635,7 @@ 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 @@ -623,19 +664,20 @@ 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 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: @@ -652,6 +694,9 @@ 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, ) @@ -1473,14 +1518,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 @@ -1488,7 +1538,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 @@ -1505,8 +1555,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] + ), } ) @@ -1574,6 +1628,13 @@ class ActEpisodeRecorder(Node): 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( @@ -1587,6 +1648,8 @@ 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)