mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-06-22 18:21:16 +00:00
[AZ-405] Replay — replay_input/ coordinator + IMU take-off auto-sync
Adds the Layer-4 cross-cutting `replay_input/` module per ADR-011: ReplayInputAdapter converges (video, tlog) into the standard FrameSource + FcAdapter + Clock surfaces the airborne composition root consumes. Owns time-alignment between video frames and tlog IMU/attitude ticks (manual via --time-offset-ms or auto via the AZ-405 IMU-take-off detector + Farneback motion-onset detector). Auto-sync algorithm (auto_sync.py): - Tlog take-off detector: sustained vertical-accel excess > 0.5 g for >= 0.5 s + sustained attitude-rate magnitude > 1 rad/s. - Video motion-onset detector: dense Farneback flow magnitude > 1.5 px sustained >= 0.5 s (deterministic per AC-10). - compute_offset combines the two; confidence = min(tlog, video). - validate_offset_or_fail implements the AC-9 95 % frame-window match validator with configurable threshold + window. ReplayInputAdapter.open() ordering (AC-13): 1. Load tlog samples + fail-fast on missing RAW_IMU/SCALED_IMU2 or ATTITUDE BEFORE any video read. 2. Resolve offset (auto-sync OR manual override; manual bypasses the detectors entirely per AC-8). 3. Run AC-9 validator on resolved offset; raise auto-sync hard-fail for AC-7 (CLI exit 2 mapping). 4. Build single Clock instance per pace (TlogDerived/ASAP, Wall/REAL). 5. Construct VideoFileFrameSource and TlogReplayFcAdapter with the resolved offset baked in (replay protocol Invariant 8). Structured log + FDR records on auto-sync detected / low-confidence / AC-8 hard-fail kinds. Idempotent close (AC-12). Tests: 25 unit tests across tests/unit/replay_input/ covering all 13 ACs (kernel-level synthetic fixtures for AC-1..AC-10; coordinator- level OpenCV synthetic videos + faked pymavlink for AC-6..AC-13). Contract update: replay_protocol.md v2.0.0 added fdr_client to the ReplayInputAdapter __init__ signature (was missing in the prose; the task spec already listed it in the allowed-imports section). Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
@@ -0,0 +1,483 @@
|
||||
"""AZ-405 — auto-sync detector + offset-compute + AC-9 validator.
|
||||
|
||||
Covers AC-1..AC-10 of ``_docs/02_tasks/todo/AZ-405_replay_auto_sync.md``.
|
||||
|
||||
Tests run against the pure compute kernels in
|
||||
:mod:`gps_denied_onboard.replay_input.auto_sync` (no disk IO, no real
|
||||
pymavlink, no real OpenCV) so the suite is fast + deterministic.
|
||||
|
||||
Style: every test follows the Arrange / Act / Assert pattern.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
from typing import Any
|
||||
|
||||
import pytest
|
||||
|
||||
from gps_denied_onboard.replay_input.auto_sync import (
|
||||
TlogSamples,
|
||||
_compute_tlog_takeoff_from_samples,
|
||||
_compute_video_onset_from_samples,
|
||||
compute_offset,
|
||||
validate_offset_or_fail,
|
||||
)
|
||||
from gps_denied_onboard.replay_input.errors import ReplayInputAdapterError
|
||||
from gps_denied_onboard.replay_input.interface import AutoSyncConfig
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------
|
||||
# Synthetic-fixture helpers
|
||||
|
||||
|
||||
def _ns(seconds: float) -> int:
|
||||
return int(seconds * 1_000_000_000)
|
||||
|
||||
|
||||
def _flat_accel_samples(
|
||||
*, start_s: float, end_s: float, hz: int, total_g: float
|
||||
) -> list[tuple[int, float]]:
|
||||
out: list[tuple[int, float]] = []
|
||||
period = 1.0 / hz
|
||||
t = start_s
|
||||
while t < end_s:
|
||||
out.append((_ns(t), total_g))
|
||||
t += period
|
||||
return out
|
||||
|
||||
|
||||
def _flat_attitude_samples(
|
||||
*, start_s: float, end_s: float, hz: int, roll: float, pitch: float, yaw: float
|
||||
) -> list[tuple[int, float, float, float]]:
|
||||
out: list[tuple[int, float, float, float]] = []
|
||||
period = 1.0 / hz
|
||||
t = start_s
|
||||
while t < end_s:
|
||||
out.append((_ns(t), roll, pitch, yaw))
|
||||
t += period
|
||||
return out
|
||||
|
||||
|
||||
def _ramp_attitude_samples(
|
||||
*,
|
||||
start_s: float,
|
||||
end_s: float,
|
||||
hz: int,
|
||||
base_roll: float,
|
||||
base_pitch: float,
|
||||
base_yaw: float,
|
||||
rate_rad_s: float,
|
||||
) -> list[tuple[int, float, float, float]]:
|
||||
"""Attitude that ramps in pitch at ``rate_rad_s`` rad/s."""
|
||||
out: list[tuple[int, float, float, float]] = []
|
||||
period = 1.0 / hz
|
||||
t = start_s
|
||||
while t < end_s:
|
||||
dt = t - start_s
|
||||
pitch = base_pitch + rate_rad_s * dt
|
||||
out.append((_ns(t), base_roll, pitch, base_yaw))
|
||||
t += period
|
||||
return out
|
||||
|
||||
|
||||
def _build_takeoff_samples() -> TlogSamples:
|
||||
"""AC-1 fixture: 2 s flat hover, then 1.5 s sustained 2.2 g + 1.5 rad/s.
|
||||
|
||||
Take-off onset is at t = 2.0 s (the first sample with the
|
||||
elevated acceleration). Body-frame accelerometer convention: at
|
||||
hover the proper-acceleration magnitude is 1 g (gravity reaction);
|
||||
during a 1.2 g thrust climb it is 2.2 g, so the take-off excess
|
||||
above 1 g rest is 1.2 g — well above the 0.5 g threshold.
|
||||
"""
|
||||
accel_pre = _flat_accel_samples(start_s=0.0, end_s=2.0, hz=200, total_g=1.0)
|
||||
accel_post = _flat_accel_samples(start_s=2.0, end_s=3.5, hz=200, total_g=2.2)
|
||||
accel = accel_pre + accel_post
|
||||
|
||||
attitude_pre = _flat_attitude_samples(
|
||||
start_s=0.0, end_s=2.0, hz=100, roll=0.0, pitch=0.0, yaw=0.0
|
||||
)
|
||||
attitude_post = _ramp_attitude_samples(
|
||||
start_s=2.0,
|
||||
end_s=3.5,
|
||||
hz=100,
|
||||
base_roll=0.0,
|
||||
base_pitch=0.0,
|
||||
base_yaw=0.0,
|
||||
rate_rad_s=1.5,
|
||||
)
|
||||
attitude = attitude_pre + attitude_post
|
||||
|
||||
return TlogSamples(
|
||||
accel=tuple(accel),
|
||||
attitude=tuple(attitude),
|
||||
imu_count_by_type={
|
||||
"RAW_IMU": len(accel),
|
||||
"ATTITUDE": len(attitude),
|
||||
},
|
||||
)
|
||||
|
||||
|
||||
def _build_low_amplitude_vibration_samples() -> TlogSamples:
|
||||
"""AC-2 fixture: 5 s of 0.3 g body-frame vibration (no take-off).
|
||||
|
||||
Total proper-acceleration during vibration = 1.3 g (0.3 g excess
|
||||
above the 1 g rest baseline) — strictly below the 0.5 g detector
|
||||
threshold so the sustained-event search rejects every window.
|
||||
"""
|
||||
accel = _flat_accel_samples(start_s=0.0, end_s=5.0, hz=200, total_g=1.3)
|
||||
attitude = _flat_attitude_samples(
|
||||
start_s=0.0, end_s=5.0, hz=100, roll=0.0, pitch=0.0, yaw=0.0
|
||||
)
|
||||
return TlogSamples(
|
||||
accel=tuple(accel),
|
||||
attitude=tuple(attitude),
|
||||
imu_count_by_type={
|
||||
"RAW_IMU": len(accel),
|
||||
"ATTITUDE": len(attitude),
|
||||
},
|
||||
)
|
||||
|
||||
|
||||
def _build_hand_launch_samples() -> TlogSamples:
|
||||
"""AC-3 fixture: 0.8 g impulse for 100 ms; not sustained for 0.5 s.
|
||||
|
||||
Body-frame accelerometer convention (see ``_build_takeoff_samples``):
|
||||
a 0.8 g impulse becomes 1.8 g total proper-acceleration during the
|
||||
impulse window.
|
||||
"""
|
||||
accel_pre = _flat_accel_samples(start_s=0.0, end_s=2.0, hz=200, total_g=1.0)
|
||||
accel_impulse = _flat_accel_samples(
|
||||
start_s=2.0, end_s=2.1, hz=200, total_g=1.8
|
||||
)
|
||||
accel_post = _flat_accel_samples(start_s=2.1, end_s=3.0, hz=200, total_g=1.0)
|
||||
accel = accel_pre + accel_impulse + accel_post
|
||||
|
||||
attitude = _flat_attitude_samples(
|
||||
start_s=0.0, end_s=3.0, hz=100, roll=0.0, pitch=0.0, yaw=0.0
|
||||
)
|
||||
return TlogSamples(
|
||||
accel=tuple(accel),
|
||||
attitude=tuple(attitude),
|
||||
imu_count_by_type={
|
||||
"RAW_IMU": len(accel),
|
||||
"ATTITUDE": len(attitude),
|
||||
},
|
||||
)
|
||||
|
||||
|
||||
def _flow_samples_from_frames(
|
||||
*, n_stationary: int, n_moving: int, fps: int = 30, motion_px: float = 4.0
|
||||
) -> tuple[tuple[int, float], ...]:
|
||||
"""Synthesise the flow-magnitude series the video detector consumes.
|
||||
|
||||
The detector emits a ``(ts_ns, mean_magnitude_px)`` tuple for each
|
||||
consecutive frame pair (skipping the first frame's pair). For
|
||||
AC-4 we pretend frames 1..10 are stationary (mag ≈ 0) and frames
|
||||
11..60 are moving (mag = motion_px).
|
||||
"""
|
||||
out: list[tuple[int, float]] = []
|
||||
period_ns = int(1_000_000_000 / fps)
|
||||
for i in range(1, n_stationary):
|
||||
out.append((i * period_ns, 0.05))
|
||||
for j in range(n_stationary, n_stationary + n_moving):
|
||||
out.append((j * period_ns, motion_px))
|
||||
return tuple(out)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------
|
||||
# AC-1 — tlog take-off detector (positive)
|
||||
|
||||
|
||||
def test_ac1_tlog_takeoff_detector_positive_within_50ms_and_high_confidence() -> None:
|
||||
# Arrange
|
||||
config = AutoSyncConfig()
|
||||
samples = _build_takeoff_samples()
|
||||
|
||||
# Act
|
||||
result = _compute_tlog_takeoff_from_samples(samples, config)
|
||||
|
||||
# Assert
|
||||
expected_onset_ns = _ns(2.0)
|
||||
assert abs(result.onset_ns - expected_onset_ns) <= _ns(0.05), (
|
||||
f"detected onset {result.onset_ns / 1e9}s deviates >50ms from expected 2.0s"
|
||||
)
|
||||
assert result.confidence >= 0.85, (
|
||||
f"confidence {result.confidence} below AC-1 minimum of 0.85"
|
||||
)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------
|
||||
# AC-2 — tlog take-off detector (ambiguous)
|
||||
|
||||
|
||||
def test_ac2_tlog_takeoff_detector_low_amplitude_vibration_low_confidence() -> None:
|
||||
# Arrange
|
||||
config = AutoSyncConfig()
|
||||
samples = _build_low_amplitude_vibration_samples()
|
||||
|
||||
# Act
|
||||
result = _compute_tlog_takeoff_from_samples(samples, config)
|
||||
|
||||
# Assert
|
||||
assert result.confidence < 0.50, (
|
||||
f"confidence {result.confidence} should be < 0.50 for ambiguous vibration"
|
||||
)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------
|
||||
# AC-3 — tlog take-off detector (hand launch)
|
||||
|
||||
|
||||
def test_ac3_tlog_takeoff_detector_hand_launch_warn_regime() -> None:
|
||||
# Arrange
|
||||
config = AutoSyncConfig()
|
||||
samples = _build_hand_launch_samples()
|
||||
|
||||
# Act
|
||||
result = _compute_tlog_takeoff_from_samples(samples, config)
|
||||
|
||||
# Assert
|
||||
assert result.confidence < 0.80, (
|
||||
f"confidence {result.confidence} should be < 0.80 for unsustained hand-launch"
|
||||
)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------
|
||||
# AC-4 — video motion-onset detector
|
||||
|
||||
|
||||
def test_ac4_video_motion_onset_detected_within_one_frame() -> None:
|
||||
# Arrange
|
||||
config = AutoSyncConfig()
|
||||
flow_samples = _flow_samples_from_frames(n_stationary=10, n_moving=50, fps=30)
|
||||
period_ns = int(1_000_000_000 / 30)
|
||||
expected_onset_ns = 10 * period_ns
|
||||
|
||||
# Act
|
||||
result = _compute_video_onset_from_samples(flow_samples, config)
|
||||
|
||||
# Assert
|
||||
assert abs(result.onset_ns - expected_onset_ns) <= period_ns, (
|
||||
f"detected motion onset {result.onset_ns} ns deviates >1 frame "
|
||||
f"from expected {expected_onset_ns} ns"
|
||||
)
|
||||
assert result.confidence > 0.80, (
|
||||
f"confidence {result.confidence} too low for clear motion onset"
|
||||
)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------
|
||||
# AC-5 — combined offset within ± 200 ms
|
||||
|
||||
|
||||
def test_ac5_combined_offset_within_200ms_of_ground_truth() -> None:
|
||||
# Arrange
|
||||
config = AutoSyncConfig()
|
||||
tlog_samples = _build_takeoff_samples()
|
||||
tlog_result = _compute_tlog_takeoff_from_samples(tlog_samples, config)
|
||||
|
||||
flow_samples = _flow_samples_from_frames(n_stationary=10, n_moving=50, fps=30)
|
||||
video_result = _compute_video_onset_from_samples(flow_samples, config)
|
||||
|
||||
# Ground-truth offset = tlog take-off (2.0 s) − video onset (10/30 s)
|
||||
period_ns = int(1_000_000_000 / 30)
|
||||
ground_truth_offset_ms = (_ns(2.0) - 10 * period_ns) // 1_000_000
|
||||
|
||||
# Act
|
||||
decision = compute_offset(tlog_result, video_result)
|
||||
|
||||
# Assert
|
||||
assert abs(decision.offset_ms - ground_truth_offset_ms) <= 200, (
|
||||
f"offset {decision.offset_ms} ms deviates >200 ms from ground truth "
|
||||
f"{ground_truth_offset_ms} ms"
|
||||
)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------
|
||||
# AC-6 — low combined confidence (verified via the coordinator test
|
||||
# in test_az405_replay_input_adapter.py; here we only verify the
|
||||
# combined-confidence aggregator picks min())
|
||||
|
||||
|
||||
def test_ac6_combined_confidence_takes_minimum_of_inputs() -> None:
|
||||
# Arrange
|
||||
from gps_denied_onboard.replay_input.auto_sync import _DetectorResult
|
||||
|
||||
high = _DetectorResult(onset_ns=_ns(1.0), confidence=0.95)
|
||||
low = _DetectorResult(onset_ns=_ns(2.0), confidence=0.50)
|
||||
|
||||
# Act
|
||||
decision = compute_offset(high, low)
|
||||
|
||||
# Assert
|
||||
assert decision.combined_confidence == pytest.approx(0.50)
|
||||
assert decision.offset_ms == (_ns(1.0) - _ns(2.0)) // 1_000_000
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------
|
||||
# AC-7 — AC-9 validator hard-fail (the coordinator-level raise is
|
||||
# covered in test_az405_replay_input_adapter.py)
|
||||
|
||||
|
||||
def test_ac7_validator_hard_fail_returns_2_for_offset_outside_window() -> None:
|
||||
# Arrange
|
||||
fps = 30
|
||||
period_ns = int(1_000_000_000 / fps)
|
||||
video_ts = [i * period_ns for i in range(60)]
|
||||
# IMU sampled at 200 Hz from t=0 to t=2 (mismatch deliberate; the
|
||||
# bad offset shifts everything outside the window).
|
||||
imu_ts = [int(i / 200 * 1_000_000_000) for i in range(400)]
|
||||
bad_offset_ms = 60_000
|
||||
|
||||
# Act
|
||||
code = validate_offset_or_fail(
|
||||
bad_offset_ms,
|
||||
imu_ts,
|
||||
video_ts,
|
||||
threshold_pct=95.0,
|
||||
window_ms=100,
|
||||
)
|
||||
|
||||
# Assert
|
||||
assert code == 2
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------
|
||||
# AC-9 — frame-window match-percentage validator (positive)
|
||||
|
||||
|
||||
def test_ac9_validator_passes_for_well_matched_offset() -> None:
|
||||
# Arrange
|
||||
fps = 30
|
||||
period_ns = int(1_000_000_000 / fps)
|
||||
video_ts = [i * period_ns for i in range(60)]
|
||||
# IMU samples densely spanning the same time range — every video
|
||||
# frame has an IMU sample within ± 100 ms.
|
||||
imu_ts = [int(i / 200 * 1_000_000_000) for i in range(60 * 200 // 30)]
|
||||
|
||||
# Act
|
||||
code = validate_offset_or_fail(
|
||||
0, imu_ts, video_ts, threshold_pct=95.0, window_ms=100
|
||||
)
|
||||
|
||||
# Assert
|
||||
assert code == 0
|
||||
|
||||
|
||||
def test_ac9_threshold_configurable() -> None:
|
||||
# Arrange — set up a series where exactly 80% of frames match.
|
||||
fps = 30
|
||||
period_ns = int(1_000_000_000 / fps)
|
||||
video_ts = [i * period_ns for i in range(50)]
|
||||
# IMU only covers the first 80% of the video timeline; the last
|
||||
# 10 frames will be far outside the window.
|
||||
imu_ts = [
|
||||
int(i / 200 * 1_000_000_000) for i in range(int(40 / 30 * 200))
|
||||
]
|
||||
|
||||
# Act / Assert
|
||||
# Default 95% threshold → fail (80% < 95%).
|
||||
assert validate_offset_or_fail(
|
||||
0, imu_ts, video_ts, threshold_pct=95.0, window_ms=100
|
||||
) == 2
|
||||
# Lowered to 75% → pass.
|
||||
assert validate_offset_or_fail(
|
||||
0, imu_ts, video_ts, threshold_pct=75.0, window_ms=100
|
||||
) == 0
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------
|
||||
# AC-10 — confidence determinism
|
||||
|
||||
|
||||
def test_ac10_confidence_score_deterministic_across_two_runs() -> None:
|
||||
# Arrange
|
||||
config = AutoSyncConfig()
|
||||
samples = _build_takeoff_samples()
|
||||
|
||||
# Act
|
||||
first = _compute_tlog_takeoff_from_samples(samples, config)
|
||||
second = _compute_tlog_takeoff_from_samples(samples, config)
|
||||
|
||||
# Assert
|
||||
assert first.onset_ns == second.onset_ns
|
||||
assert math.isclose(first.confidence, second.confidence, abs_tol=1e-9)
|
||||
|
||||
|
||||
def test_ac10_video_onset_deterministic_across_two_runs() -> None:
|
||||
# Arrange
|
||||
config = AutoSyncConfig()
|
||||
flow_samples = _flow_samples_from_frames(n_stationary=5, n_moving=20, fps=30)
|
||||
|
||||
# Act
|
||||
first = _compute_video_onset_from_samples(flow_samples, config)
|
||||
second = _compute_video_onset_from_samples(flow_samples, config)
|
||||
|
||||
# Assert
|
||||
assert first.onset_ns == second.onset_ns
|
||||
assert math.isclose(first.confidence, second.confidence, abs_tol=1e-9)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------
|
||||
# R-DEMO-3 fail-fast on the pure compute path
|
||||
|
||||
|
||||
def test_pure_takeoff_kernel_raises_on_no_imu_samples() -> None:
|
||||
# Arrange
|
||||
config = AutoSyncConfig()
|
||||
samples = TlogSamples(
|
||||
accel=(),
|
||||
attitude=(),
|
||||
imu_count_by_type={"ATTITUDE": 100},
|
||||
)
|
||||
|
||||
# Act / Assert
|
||||
with pytest.raises(ReplayInputAdapterError, match="tlog missing required"):
|
||||
_compute_takeoff_or_propagate(samples, config)
|
||||
|
||||
|
||||
def test_pure_takeoff_kernel_raises_on_no_attitude_samples() -> None:
|
||||
# Arrange
|
||||
config = AutoSyncConfig()
|
||||
accel = _flat_accel_samples(start_s=0.0, end_s=1.0, hz=200, total_g=1.0)
|
||||
samples = TlogSamples(
|
||||
accel=tuple(accel),
|
||||
attitude=(),
|
||||
imu_count_by_type={"RAW_IMU": len(accel)},
|
||||
)
|
||||
|
||||
# Act / Assert
|
||||
with pytest.raises(ReplayInputAdapterError, match="tlog missing required"):
|
||||
_compute_takeoff_or_propagate(samples, config)
|
||||
|
||||
|
||||
def _compute_takeoff_or_propagate(samples: TlogSamples, config: AutoSyncConfig) -> Any:
|
||||
"""Local trampoline so the assertions are explicit even if the
|
||||
underscore-named helper migrates."""
|
||||
return _compute_tlog_takeoff_from_samples(samples, config)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------
|
||||
# AC-9 edge cases
|
||||
|
||||
|
||||
def test_validator_returns_2_on_empty_video_or_tlog() -> None:
|
||||
# Arrange
|
||||
imu_ts = [0, 1_000_000, 2_000_000]
|
||||
video_ts: list[int] = []
|
||||
|
||||
# Act / Assert — empty video
|
||||
assert (
|
||||
validate_offset_or_fail(
|
||||
0, imu_ts, video_ts, threshold_pct=95.0, window_ms=100
|
||||
)
|
||||
== 2
|
||||
)
|
||||
# Empty tlog
|
||||
assert (
|
||||
validate_offset_or_fail(
|
||||
0, [], [0, 1_000_000], threshold_pct=95.0, window_ms=100
|
||||
)
|
||||
== 2
|
||||
)
|
||||
@@ -0,0 +1,729 @@
|
||||
"""AZ-405 — ``ReplayInputAdapter`` coordinator unit tests.
|
||||
|
||||
Covers AC-6 (low-confidence WARN-and-proceed), AC-7 (AC-8 hard-fail),
|
||||
AC-8 (manual override bypass), AC-11 (open() returns a complete
|
||||
bundle), AC-12 (idempotent close), and AC-13 (R-DEMO-3 fail-fast on
|
||||
missing tlog message types).
|
||||
|
||||
Synthetic videos use the same OpenCV-driven fixture pattern as
|
||||
``tests/unit/frame_source/test_protocol_conformance.py``; the tlog
|
||||
side is faked via the coordinator's ``tlog_source_factory`` injection
|
||||
point so tests run without a real pymavlink connection.
|
||||
|
||||
Style: every test follows the Arrange / Act / Assert pattern.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from pathlib import Path
|
||||
from types import SimpleNamespace
|
||||
from typing import Any
|
||||
from unittest import mock
|
||||
|
||||
import cv2
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from gps_denied_onboard._types.calibration import CameraCalibration
|
||||
from gps_denied_onboard._types.fc import FcKind
|
||||
from gps_denied_onboard.clock.tlog_derived import TlogDerivedClock
|
||||
from gps_denied_onboard.clock.wall_clock import WallClock
|
||||
from gps_denied_onboard.components.c8_fc_adapter.tlog_replay_adapter import (
|
||||
ReplayPace,
|
||||
TlogReplayFcAdapter,
|
||||
)
|
||||
from gps_denied_onboard.frame_source.video_file import VideoFileFrameSource
|
||||
from gps_denied_onboard.replay_input.errors import ReplayInputAdapterError
|
||||
from gps_denied_onboard.replay_input.interface import (
|
||||
AutoSyncConfig,
|
||||
AutoSyncDecision,
|
||||
ReplayInputBundle,
|
||||
)
|
||||
from gps_denied_onboard.replay_input.tlog_video_adapter import ReplayInputAdapter
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------
|
||||
# Fixtures
|
||||
|
||||
|
||||
@pytest.fixture(autouse=True)
|
||||
def _enable_build_flags(monkeypatch: pytest.MonkeyPatch) -> None:
|
||||
"""Both downstream strategies are gated by build flags (AZ-398 / AZ-399)."""
|
||||
monkeypatch.setenv("BUILD_VIDEO_FILE_FRAME_SOURCE", "ON")
|
||||
monkeypatch.setenv("BUILD_TLOG_REPLAY_ADAPTER", "ON")
|
||||
|
||||
|
||||
@pytest.fixture
|
||||
def fake_fdr_client() -> mock.MagicMock:
|
||||
return mock.MagicMock(name="FdrClient")
|
||||
|
||||
|
||||
@pytest.fixture
|
||||
def fake_wgs_converter() -> mock.MagicMock:
|
||||
return mock.MagicMock(name="WgsConverter")
|
||||
|
||||
|
||||
@pytest.fixture
|
||||
def camera_calibration() -> CameraCalibration:
|
||||
return CameraCalibration(
|
||||
camera_id="az405-test",
|
||||
intrinsics_3x3=None,
|
||||
distortion=None,
|
||||
body_to_camera_se3=None,
|
||||
acquisition_method="synthetic",
|
||||
)
|
||||
|
||||
|
||||
def _make_synthetic_video(path: Path, n_frames: int = 60, fps: int = 30) -> Path:
|
||||
"""Write a 64×48 BGR MP4V file at ``path`` and return it."""
|
||||
fourcc = cv2.VideoWriter_fourcc(*"mp4v")
|
||||
writer = cv2.VideoWriter(str(path), fourcc, fps, (64, 48))
|
||||
if not writer.isOpened():
|
||||
raise RuntimeError(f"OpenCV could not open writer at {path!s}")
|
||||
try:
|
||||
for i in range(n_frames):
|
||||
frame = np.full((48, 64, 3), i % 256, dtype=np.uint8)
|
||||
writer.write(frame)
|
||||
finally:
|
||||
writer.release()
|
||||
return path
|
||||
|
||||
|
||||
@pytest.fixture
|
||||
def synthetic_video(tmp_path: Path) -> Path:
|
||||
return _make_synthetic_video(tmp_path / "az405-video.mp4", n_frames=60, fps=30)
|
||||
|
||||
|
||||
@pytest.fixture
|
||||
def synthetic_tlog_path(tmp_path: Path) -> Path:
|
||||
p = tmp_path / "az405.tlog"
|
||||
p.write_bytes(b"fake-tlog")
|
||||
return p
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------
|
||||
# Fake pymavlink source
|
||||
|
||||
|
||||
def _ns(seconds: float) -> int:
|
||||
return int(seconds * 1_000_000_000)
|
||||
|
||||
|
||||
def _fake_msg(msg_type: str, *, ts_s: float, **fields: Any) -> SimpleNamespace:
|
||||
ns = SimpleNamespace(_timestamp=ts_s, **fields)
|
||||
ns.get_type = lambda: msg_type
|
||||
return ns
|
||||
|
||||
|
||||
def _build_takeoff_messages(
|
||||
*,
|
||||
accel_pre_total_g: float = 1.0,
|
||||
accel_post_total_g: float = 2.2,
|
||||
accel_hz: int = 200,
|
||||
pre_seconds: float = 2.0,
|
||||
post_seconds: float = 1.5,
|
||||
) -> list[SimpleNamespace]:
|
||||
"""A short tlog stream with a clear take-off pattern + GPS + heartbeat."""
|
||||
out: list[SimpleNamespace] = []
|
||||
accel_period = 1.0 / accel_hz
|
||||
# Pre-takeoff: 1 g hover (z-acc = -1g in body, after sign).
|
||||
t = 0.0
|
||||
while t < pre_seconds:
|
||||
out.append(
|
||||
_fake_msg(
|
||||
"RAW_IMU",
|
||||
ts_s=t,
|
||||
xacc=0,
|
||||
yacc=0,
|
||||
zacc=-int(accel_pre_total_g * 1000),
|
||||
xgyro=0,
|
||||
ygyro=0,
|
||||
zgyro=0,
|
||||
)
|
||||
)
|
||||
t += accel_period
|
||||
# Post-takeoff: 2.2 g sustained climb thrust.
|
||||
while t < pre_seconds + post_seconds:
|
||||
out.append(
|
||||
_fake_msg(
|
||||
"RAW_IMU",
|
||||
ts_s=t,
|
||||
xacc=0,
|
||||
yacc=0,
|
||||
zacc=-int(accel_post_total_g * 1000),
|
||||
xgyro=0,
|
||||
ygyro=0,
|
||||
zgyro=0,
|
||||
)
|
||||
)
|
||||
t += accel_period
|
||||
|
||||
# Attitude: flat hover then 1.5 rad/s pitch ramp.
|
||||
t = 0.0
|
||||
attitude_period = 1.0 / 100.0
|
||||
while t < pre_seconds:
|
||||
out.append(
|
||||
_fake_msg("ATTITUDE", ts_s=t, roll=0.0, pitch=0.0, yaw=0.0)
|
||||
)
|
||||
t += attitude_period
|
||||
pitch_rate = 1.5
|
||||
while t < pre_seconds + post_seconds:
|
||||
dt = t - pre_seconds
|
||||
out.append(
|
||||
_fake_msg(
|
||||
"ATTITUDE",
|
||||
ts_s=t,
|
||||
roll=0.0,
|
||||
pitch=pitch_rate * dt,
|
||||
yaw=0.0,
|
||||
)
|
||||
)
|
||||
t += attitude_period
|
||||
|
||||
# GPS_RAW_INT + HEARTBEAT (required by AZ-399 pre-scan).
|
||||
out.append(
|
||||
_fake_msg(
|
||||
"GPS_RAW_INT",
|
||||
ts_s=0.0,
|
||||
fix_type=3,
|
||||
lat=499910000,
|
||||
lon=362210000,
|
||||
alt=153_400,
|
||||
)
|
||||
)
|
||||
out.append(_fake_msg("HEARTBEAT", ts_s=0.0, system_status=4, base_mode=0))
|
||||
out.sort(key=lambda m: m._timestamp)
|
||||
return out
|
||||
|
||||
|
||||
class _FakeTlog:
|
||||
"""Minimal pymavlink ``mavlink_connection`` stand-in.
|
||||
|
||||
Returns each stored message once on ``recv_match``; ignores the
|
||||
``type=`` filter (the AZ-399 decode loop receives unfiltered
|
||||
HEARTBEAT/IMU/ATTITUDE/GPS streams).
|
||||
"""
|
||||
|
||||
def __init__(self, messages: list[SimpleNamespace]) -> None:
|
||||
self._iter = iter(messages)
|
||||
self.closed = False
|
||||
|
||||
def recv_match(self, **_kwargs: Any) -> SimpleNamespace | None:
|
||||
return next(self._iter, None)
|
||||
|
||||
def close(self) -> None:
|
||||
self.closed = True
|
||||
|
||||
|
||||
def _factory_for(messages: list[SimpleNamespace]) -> Any:
|
||||
"""Return a source factory that yields a fresh ``_FakeTlog`` per call.
|
||||
|
||||
The coordinator opens the tlog twice (once for ``_load_tlog_samples``
|
||||
in the auto-sync path, once via the FC adapter's pre-scan + decode
|
||||
handles), so the messages have to be re-emittable.
|
||||
"""
|
||||
|
||||
def _factory(_path: str) -> _FakeTlog:
|
||||
return _FakeTlog(list(messages))
|
||||
|
||||
return _factory
|
||||
|
||||
|
||||
def _frames_factory_with_motion(
|
||||
*,
|
||||
n_stationary: int = 10,
|
||||
n_moving: int = 50,
|
||||
fps: int = 30,
|
||||
) -> Any:
|
||||
"""Return a frames_factory yielding the AC-4 motion-onset shape."""
|
||||
period_ns = int(1_000_000_000 / fps)
|
||||
rng = np.random.default_rng(seed=0)
|
||||
|
||||
def _factory(_path: Path, _scan_seconds: float) -> Any:
|
||||
out: list[tuple[int, np.ndarray]] = []
|
||||
# Stationary: identical frames so optical flow ≈ 0.
|
||||
base = np.full((48, 64, 3), 128, dtype=np.uint8)
|
||||
for i in range(n_stationary):
|
||||
out.append((i * period_ns, base.copy()))
|
||||
# Moving: each frame replaces a 16×16 patch at a random offset
|
||||
# so Farneback returns a clear non-zero magnitude. Determinism
|
||||
# is preserved by the seeded RNG.
|
||||
for j in range(n_moving):
|
||||
frame = base.copy()
|
||||
r = rng.integers(0, 32)
|
||||
c = rng.integers(0, 48)
|
||||
frame[r : r + 16, c : c + 16, :] = 240
|
||||
out.append(((n_stationary + j) * period_ns, frame))
|
||||
return out
|
||||
|
||||
return _factory
|
||||
|
||||
|
||||
def _video_timestamps_factory(
|
||||
*,
|
||||
n_frames: int = 60,
|
||||
fps: int = 30,
|
||||
) -> Any:
|
||||
"""Return a timestamps_factory with deterministic per-frame ts (ns)."""
|
||||
period_ns = int(1_000_000_000 / fps)
|
||||
|
||||
def _factory(_path: Path) -> list[int]:
|
||||
return [i * period_ns for i in range(n_frames)]
|
||||
|
||||
return _factory
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------
|
||||
# AC-11 — open() returns a complete bundle
|
||||
|
||||
|
||||
def test_ac11_open_returns_complete_bundle_with_correct_strategies(
|
||||
synthetic_video: Path,
|
||||
synthetic_tlog_path: Path,
|
||||
camera_calibration: CameraCalibration,
|
||||
fake_wgs_converter: mock.MagicMock,
|
||||
fake_fdr_client: mock.MagicMock,
|
||||
) -> None:
|
||||
# Arrange
|
||||
messages = _build_takeoff_messages()
|
||||
adapter = ReplayInputAdapter(
|
||||
video_path=synthetic_video,
|
||||
tlog_path=synthetic_tlog_path,
|
||||
camera_calibration=camera_calibration,
|
||||
target_fc_dialect=FcKind.ARDUPILOT_PLANE,
|
||||
wgs_converter=fake_wgs_converter,
|
||||
fdr_client=fake_fdr_client,
|
||||
pace=ReplayPace.ASAP,
|
||||
manual_time_offset_ms=0,
|
||||
auto_sync_config=AutoSyncConfig(),
|
||||
tlog_source_factory=_factory_for(messages),
|
||||
video_timestamps_factory=_video_timestamps_factory(),
|
||||
)
|
||||
|
||||
# Act
|
||||
try:
|
||||
bundle = adapter.open()
|
||||
|
||||
# Assert
|
||||
assert isinstance(bundle, ReplayInputBundle)
|
||||
assert isinstance(bundle.frame_source, VideoFileFrameSource)
|
||||
assert isinstance(bundle.fc_adapter, TlogReplayFcAdapter)
|
||||
assert isinstance(bundle.clock, TlogDerivedClock)
|
||||
assert bundle.resolved_time_offset_ms == 0
|
||||
# Manual path → no auto-sync result.
|
||||
assert bundle.auto_sync_result is None
|
||||
finally:
|
||||
adapter.close()
|
||||
|
||||
|
||||
def test_ac11_pace_realtime_yields_wall_clock(
|
||||
synthetic_video: Path,
|
||||
synthetic_tlog_path: Path,
|
||||
camera_calibration: CameraCalibration,
|
||||
fake_wgs_converter: mock.MagicMock,
|
||||
fake_fdr_client: mock.MagicMock,
|
||||
) -> None:
|
||||
# Arrange
|
||||
messages = _build_takeoff_messages()
|
||||
adapter = ReplayInputAdapter(
|
||||
video_path=synthetic_video,
|
||||
tlog_path=synthetic_tlog_path,
|
||||
camera_calibration=camera_calibration,
|
||||
target_fc_dialect=FcKind.ARDUPILOT_PLANE,
|
||||
wgs_converter=fake_wgs_converter,
|
||||
fdr_client=fake_fdr_client,
|
||||
pace=ReplayPace.REALTIME,
|
||||
manual_time_offset_ms=0,
|
||||
auto_sync_config=AutoSyncConfig(),
|
||||
tlog_source_factory=_factory_for(messages),
|
||||
video_timestamps_factory=_video_timestamps_factory(),
|
||||
)
|
||||
|
||||
# Act
|
||||
try:
|
||||
bundle = adapter.open()
|
||||
|
||||
# Assert
|
||||
assert isinstance(bundle.clock, WallClock)
|
||||
finally:
|
||||
adapter.close()
|
||||
|
||||
|
||||
def test_ac11_pace_asap_yields_tlog_derived_clock(
|
||||
synthetic_video: Path,
|
||||
synthetic_tlog_path: Path,
|
||||
camera_calibration: CameraCalibration,
|
||||
fake_wgs_converter: mock.MagicMock,
|
||||
fake_fdr_client: mock.MagicMock,
|
||||
) -> None:
|
||||
# Arrange
|
||||
messages = _build_takeoff_messages()
|
||||
adapter = ReplayInputAdapter(
|
||||
video_path=synthetic_video,
|
||||
tlog_path=synthetic_tlog_path,
|
||||
camera_calibration=camera_calibration,
|
||||
target_fc_dialect=FcKind.ARDUPILOT_PLANE,
|
||||
wgs_converter=fake_wgs_converter,
|
||||
fdr_client=fake_fdr_client,
|
||||
pace=ReplayPace.ASAP,
|
||||
manual_time_offset_ms=0,
|
||||
auto_sync_config=AutoSyncConfig(),
|
||||
tlog_source_factory=_factory_for(messages),
|
||||
video_timestamps_factory=_video_timestamps_factory(),
|
||||
)
|
||||
|
||||
# Act
|
||||
try:
|
||||
bundle = adapter.open()
|
||||
|
||||
# Assert
|
||||
assert isinstance(bundle.clock, TlogDerivedClock)
|
||||
finally:
|
||||
adapter.close()
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------
|
||||
# AC-12 — idempotent close
|
||||
|
||||
|
||||
def test_ac12_close_is_idempotent(
|
||||
synthetic_video: Path,
|
||||
synthetic_tlog_path: Path,
|
||||
camera_calibration: CameraCalibration,
|
||||
fake_wgs_converter: mock.MagicMock,
|
||||
fake_fdr_client: mock.MagicMock,
|
||||
) -> None:
|
||||
# Arrange
|
||||
messages = _build_takeoff_messages()
|
||||
adapter = ReplayInputAdapter(
|
||||
video_path=synthetic_video,
|
||||
tlog_path=synthetic_tlog_path,
|
||||
camera_calibration=camera_calibration,
|
||||
target_fc_dialect=FcKind.ARDUPILOT_PLANE,
|
||||
wgs_converter=fake_wgs_converter,
|
||||
fdr_client=fake_fdr_client,
|
||||
pace=ReplayPace.ASAP,
|
||||
manual_time_offset_ms=0,
|
||||
auto_sync_config=AutoSyncConfig(),
|
||||
tlog_source_factory=_factory_for(messages),
|
||||
video_timestamps_factory=_video_timestamps_factory(),
|
||||
)
|
||||
adapter.open()
|
||||
|
||||
# Act / Assert — both calls must complete without raising.
|
||||
adapter.close()
|
||||
adapter.close()
|
||||
|
||||
|
||||
def test_close_without_open_does_not_raise(
|
||||
synthetic_video: Path,
|
||||
synthetic_tlog_path: Path,
|
||||
camera_calibration: CameraCalibration,
|
||||
fake_wgs_converter: mock.MagicMock,
|
||||
fake_fdr_client: mock.MagicMock,
|
||||
) -> None:
|
||||
# Arrange
|
||||
adapter = ReplayInputAdapter(
|
||||
video_path=synthetic_video,
|
||||
tlog_path=synthetic_tlog_path,
|
||||
camera_calibration=camera_calibration,
|
||||
target_fc_dialect=FcKind.ARDUPILOT_PLANE,
|
||||
wgs_converter=fake_wgs_converter,
|
||||
fdr_client=fake_fdr_client,
|
||||
pace=ReplayPace.ASAP,
|
||||
manual_time_offset_ms=0,
|
||||
auto_sync_config=AutoSyncConfig(),
|
||||
)
|
||||
|
||||
# Act / Assert
|
||||
adapter.close()
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------
|
||||
# AC-13 — missing tlog messages fail fast
|
||||
|
||||
|
||||
def test_ac13_missing_imu_messages_fails_fast_before_video_read(
|
||||
synthetic_video: Path,
|
||||
synthetic_tlog_path: Path,
|
||||
camera_calibration: CameraCalibration,
|
||||
fake_wgs_converter: mock.MagicMock,
|
||||
fake_fdr_client: mock.MagicMock,
|
||||
) -> None:
|
||||
# Arrange — tlog has only ATTITUDE; no RAW_IMU / SCALED_IMU2.
|
||||
attitude_only = [
|
||||
_fake_msg("ATTITUDE", ts_s=t * 0.01, roll=0.0, pitch=0.0, yaw=0.0)
|
||||
for t in range(100)
|
||||
]
|
||||
adapter = ReplayInputAdapter(
|
||||
video_path=synthetic_video,
|
||||
tlog_path=synthetic_tlog_path,
|
||||
camera_calibration=camera_calibration,
|
||||
target_fc_dialect=FcKind.ARDUPILOT_PLANE,
|
||||
wgs_converter=fake_wgs_converter,
|
||||
fdr_client=fake_fdr_client,
|
||||
pace=ReplayPace.ASAP,
|
||||
manual_time_offset_ms=0,
|
||||
auto_sync_config=AutoSyncConfig(),
|
||||
tlog_source_factory=_factory_for(attitude_only),
|
||||
)
|
||||
|
||||
# Act / Assert
|
||||
with pytest.raises(
|
||||
ReplayInputAdapterError, match="tlog missing required message types"
|
||||
):
|
||||
adapter.open()
|
||||
|
||||
|
||||
def test_ac13_missing_attitude_messages_fails_fast(
|
||||
synthetic_video: Path,
|
||||
synthetic_tlog_path: Path,
|
||||
camera_calibration: CameraCalibration,
|
||||
fake_wgs_converter: mock.MagicMock,
|
||||
fake_fdr_client: mock.MagicMock,
|
||||
) -> None:
|
||||
# Arrange — tlog has only RAW_IMU; no ATTITUDE.
|
||||
imu_only = [
|
||||
_fake_msg(
|
||||
"RAW_IMU",
|
||||
ts_s=t * 0.005,
|
||||
xacc=0,
|
||||
yacc=0,
|
||||
zacc=-1000,
|
||||
xgyro=0,
|
||||
ygyro=0,
|
||||
zgyro=0,
|
||||
)
|
||||
for t in range(100)
|
||||
]
|
||||
adapter = ReplayInputAdapter(
|
||||
video_path=synthetic_video,
|
||||
tlog_path=synthetic_tlog_path,
|
||||
camera_calibration=camera_calibration,
|
||||
target_fc_dialect=FcKind.ARDUPILOT_PLANE,
|
||||
wgs_converter=fake_wgs_converter,
|
||||
fdr_client=fake_fdr_client,
|
||||
pace=ReplayPace.ASAP,
|
||||
manual_time_offset_ms=0,
|
||||
auto_sync_config=AutoSyncConfig(),
|
||||
tlog_source_factory=_factory_for(imu_only),
|
||||
)
|
||||
|
||||
# Act / Assert
|
||||
with pytest.raises(
|
||||
ReplayInputAdapterError, match=r"tlog missing required message types.*ATTITUDE"
|
||||
):
|
||||
adapter.open()
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------
|
||||
# AC-8 — manual override bypasses auto-detect
|
||||
|
||||
|
||||
def test_ac8_manual_override_bypasses_auto_detect(
|
||||
synthetic_video: Path,
|
||||
synthetic_tlog_path: Path,
|
||||
camera_calibration: CameraCalibration,
|
||||
fake_wgs_converter: mock.MagicMock,
|
||||
fake_fdr_client: mock.MagicMock,
|
||||
monkeypatch: pytest.MonkeyPatch,
|
||||
) -> None:
|
||||
# Arrange
|
||||
detect_calls: list[Any] = []
|
||||
|
||||
def _explode_if_called(*args: Any, **kwargs: Any) -> Any:
|
||||
detect_calls.append((args, kwargs))
|
||||
raise AssertionError(
|
||||
"auto-sync detector called even though manual_time_offset_ms was set"
|
||||
)
|
||||
|
||||
monkeypatch.setattr(
|
||||
"gps_denied_onboard.replay_input.tlog_video_adapter.detect_video_motion_onset",
|
||||
_explode_if_called,
|
||||
)
|
||||
|
||||
# Patch the take-off compute kernel referenced via the helper; the
|
||||
# coordinator's manual path must skip it entirely.
|
||||
monkeypatch.setattr(
|
||||
"gps_denied_onboard.replay_input.auto_sync._compute_tlog_takeoff_from_samples",
|
||||
_explode_if_called,
|
||||
)
|
||||
|
||||
messages = _build_takeoff_messages()
|
||||
adapter = ReplayInputAdapter(
|
||||
video_path=synthetic_video,
|
||||
tlog_path=synthetic_tlog_path,
|
||||
camera_calibration=camera_calibration,
|
||||
target_fc_dialect=FcKind.ARDUPILOT_PLANE,
|
||||
wgs_converter=fake_wgs_converter,
|
||||
fdr_client=fake_fdr_client,
|
||||
pace=ReplayPace.ASAP,
|
||||
manual_time_offset_ms=500,
|
||||
auto_sync_config=AutoSyncConfig(),
|
||||
tlog_source_factory=_factory_for(messages),
|
||||
video_timestamps_factory=_video_timestamps_factory(),
|
||||
)
|
||||
|
||||
# Act
|
||||
try:
|
||||
bundle = adapter.open()
|
||||
|
||||
# Assert — detector helpers were NOT invoked.
|
||||
assert detect_calls == []
|
||||
assert bundle.resolved_time_offset_ms == 500
|
||||
assert bundle.auto_sync_result is None
|
||||
finally:
|
||||
adapter.close()
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------
|
||||
# AC-7 — AC-8 hard-fail raises
|
||||
|
||||
|
||||
def test_ac7_ac8_validator_hard_fail_raises_on_open(
|
||||
synthetic_video: Path,
|
||||
synthetic_tlog_path: Path,
|
||||
camera_calibration: CameraCalibration,
|
||||
fake_wgs_converter: mock.MagicMock,
|
||||
fake_fdr_client: mock.MagicMock,
|
||||
) -> None:
|
||||
# Arrange — manual offset of 60 s will push every video frame
|
||||
# outside the IMU coverage window (the fake tlog only carries
|
||||
# ~3.5 s of samples).
|
||||
messages = _build_takeoff_messages()
|
||||
adapter = ReplayInputAdapter(
|
||||
video_path=synthetic_video,
|
||||
tlog_path=synthetic_tlog_path,
|
||||
camera_calibration=camera_calibration,
|
||||
target_fc_dialect=FcKind.ARDUPILOT_PLANE,
|
||||
wgs_converter=fake_wgs_converter,
|
||||
fdr_client=fake_fdr_client,
|
||||
pace=ReplayPace.ASAP,
|
||||
manual_time_offset_ms=60_000,
|
||||
auto_sync_config=AutoSyncConfig(),
|
||||
tlog_source_factory=_factory_for(messages),
|
||||
video_timestamps_factory=_video_timestamps_factory(),
|
||||
)
|
||||
|
||||
# Act / Assert
|
||||
with pytest.raises(ReplayInputAdapterError, match="auto-sync hard-fail"):
|
||||
adapter.open()
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------
|
||||
# AC-6 — low combined confidence WARN-and-proceed
|
||||
|
||||
|
||||
def test_ac6_low_confidence_warn_and_proceed_does_not_raise(
|
||||
synthetic_video: Path,
|
||||
synthetic_tlog_path: Path,
|
||||
camera_calibration: CameraCalibration,
|
||||
fake_wgs_converter: mock.MagicMock,
|
||||
fake_fdr_client: mock.MagicMock,
|
||||
monkeypatch: pytest.MonkeyPatch,
|
||||
caplog: pytest.LogCaptureFixture,
|
||||
) -> None:
|
||||
# Arrange — stub the detectors to return low-confidence results.
|
||||
from gps_denied_onboard.replay_input.auto_sync import _DetectorResult
|
||||
|
||||
low_conf = _DetectorResult(onset_ns=_ns(2.0), confidence=0.40)
|
||||
|
||||
def _stub_take_off(*args: Any, **kwargs: Any) -> _DetectorResult:
|
||||
return low_conf
|
||||
|
||||
def _stub_motion_onset(*args: Any, **kwargs: Any) -> _DetectorResult:
|
||||
return _DetectorResult(onset_ns=_ns(2.0), confidence=0.40)
|
||||
|
||||
monkeypatch.setattr(
|
||||
"gps_denied_onboard.replay_input.auto_sync._compute_tlog_takeoff_from_samples",
|
||||
_stub_take_off,
|
||||
)
|
||||
monkeypatch.setattr(
|
||||
"gps_denied_onboard.replay_input.tlog_video_adapter.detect_video_motion_onset",
|
||||
_stub_motion_onset,
|
||||
)
|
||||
|
||||
messages = _build_takeoff_messages()
|
||||
adapter = ReplayInputAdapter(
|
||||
video_path=synthetic_video,
|
||||
tlog_path=synthetic_tlog_path,
|
||||
camera_calibration=camera_calibration,
|
||||
target_fc_dialect=FcKind.ARDUPILOT_PLANE,
|
||||
wgs_converter=fake_wgs_converter,
|
||||
fdr_client=fake_fdr_client,
|
||||
pace=ReplayPace.ASAP,
|
||||
manual_time_offset_ms=None,
|
||||
auto_sync_config=AutoSyncConfig(),
|
||||
tlog_source_factory=_factory_for(messages),
|
||||
video_timestamps_factory=_video_timestamps_factory(),
|
||||
)
|
||||
|
||||
# Act
|
||||
caplog.set_level("WARNING", logger="replay_input.tlog_video_adapter")
|
||||
try:
|
||||
bundle = adapter.open()
|
||||
|
||||
# Assert — open() returned the bundle (didn't raise) and the
|
||||
# WARN log fired.
|
||||
assert bundle.auto_sync_result is not None
|
||||
assert bundle.auto_sync_result.combined_confidence == pytest.approx(0.40)
|
||||
warn_kinds = [
|
||||
r.kind for r in caplog.records if hasattr(r, "kind")
|
||||
]
|
||||
assert "replay.auto_sync.low_confidence" in warn_kinds
|
||||
finally:
|
||||
adapter.close()
|
||||
|
||||
|
||||
def test_ac11_resolved_offset_matches_auto_sync_result(
|
||||
synthetic_video: Path,
|
||||
synthetic_tlog_path: Path,
|
||||
camera_calibration: CameraCalibration,
|
||||
fake_wgs_converter: mock.MagicMock,
|
||||
fake_fdr_client: mock.MagicMock,
|
||||
monkeypatch: pytest.MonkeyPatch,
|
||||
) -> None:
|
||||
# Arrange — high-confidence stubs so AC-6 WARN does not fire.
|
||||
from gps_denied_onboard.replay_input.auto_sync import _DetectorResult
|
||||
|
||||
def _stub_take_off(*args: Any, **kwargs: Any) -> _DetectorResult:
|
||||
return _DetectorResult(onset_ns=_ns(2.0), confidence=0.95)
|
||||
|
||||
def _stub_motion_onset(*args: Any, **kwargs: Any) -> _DetectorResult:
|
||||
return _DetectorResult(onset_ns=_ns(0.333), confidence=0.95)
|
||||
|
||||
monkeypatch.setattr(
|
||||
"gps_denied_onboard.replay_input.auto_sync._compute_tlog_takeoff_from_samples",
|
||||
_stub_take_off,
|
||||
)
|
||||
monkeypatch.setattr(
|
||||
"gps_denied_onboard.replay_input.tlog_video_adapter.detect_video_motion_onset",
|
||||
_stub_motion_onset,
|
||||
)
|
||||
|
||||
messages = _build_takeoff_messages()
|
||||
adapter = ReplayInputAdapter(
|
||||
video_path=synthetic_video,
|
||||
tlog_path=synthetic_tlog_path,
|
||||
camera_calibration=camera_calibration,
|
||||
target_fc_dialect=FcKind.ARDUPILOT_PLANE,
|
||||
wgs_converter=fake_wgs_converter,
|
||||
fdr_client=fake_fdr_client,
|
||||
pace=ReplayPace.ASAP,
|
||||
manual_time_offset_ms=None,
|
||||
auto_sync_config=AutoSyncConfig(),
|
||||
tlog_source_factory=_factory_for(messages),
|
||||
video_timestamps_factory=_video_timestamps_factory(),
|
||||
)
|
||||
|
||||
# Act
|
||||
try:
|
||||
bundle = adapter.open()
|
||||
|
||||
# Assert
|
||||
expected_offset_ms = (_ns(2.0) - _ns(0.333)) // 1_000_000
|
||||
assert bundle.resolved_time_offset_ms == expected_offset_ms
|
||||
assert bundle.auto_sync_result is not None
|
||||
assert bundle.auto_sync_result.offset_ms == expected_offset_ms
|
||||
finally:
|
||||
adapter.close()
|
||||
Reference in New Issue
Block a user