feat: 对齐ACT双相机帧

xr_rm_teleop/test/test_act_episode_recorder.py

@@ -1,8 +1,16 @@
+import numpy as np
+import pytest
+
 from xr_rm_teleop.act_episode_recorder import (
     NO_ACTION,
     ButtonTracker,
+    CameraBuffer,
+    CameraFrame,
+    QualityError,
     RecordingSession,
     RecordingState,
+    select_camera_pair,
+    select_frame,
 )
 
 
@@ -19,7 +27,7 @@ def _recording_session(*, origin_seq=10, max_samples=1800):
     return session
 
 
-def test_recording_starts_on_first_sent_grip_action_and_samples_every_third_cycle():
+def test_recording_starts_on_first_sent_grip_action_and_downsamples():
     session = RecordingSession(max_samples=1800)
     session.arm()
 
@@ -184,3 +192,84 @@ def test_right_a_reports_recording_rejection_event():
     )
 
     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)