azaion/gps-denied-onboard
1
0
Fork 0
mirror of https://github.com/azaion/gps-denied-onboard.git synced 2026-06-22 22:11:13 +00:00
Code Issues Packages Projects Releases Wiki Activity

[AZ-405] Replay — replay_input/ coordinator + IMU take-off auto-sync

Browse Source 
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:
Oleksandr Bezdieniezhnykh
2026-05-14 09:50:51 +03:00
parent f9b4241d3a
commit 8149083cac
14 changed files with 2979 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/gps_denied_onboard/replay_input/__init__.py
+36
View File
@@ -0,0 +1,36 @@
"""``replay_input/`` cross-cutting coordinator (AZ-405 / E-DEMO-REPLAY).
Layer-4 module per ``_docs/02_document/module-layout.md``. Converges
``(video, tlog)`` inputs into the standard :class:`FrameSource`,
:class:`FcAdapter`, and :class:`Clock` surfaces consumed by the
airborne composition root. Owns the time-alignment concern between
video frames and tlog IMU/attitude ticks (manual via
``--time-offset-ms`` or automatic via the AZ-405 IMU-take-off
detector).
New under ADR-011 (replay-as-configuration) — replaces the v1.0.0
design where replay had its own composition root.
Public surface re-exports the coordinator class, the bundle DTO, the
auto-sync decision DTO, the auto-sync config DTO, and the coordinator
error class. The detector functions in :mod:`auto_sync` are NOT
re-exported here so the public API stays focused on the composition
root's wiring needs; tests import the detectors via their full module
path.
"""
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
__all__ = [
"AutoSyncConfig",
"AutoSyncDecision",
"ReplayInputAdapter",
"ReplayInputAdapterError",
"ReplayInputBundle",
]
src/gps_denied_onboard/replay_input/auto_sync.py
+646
View File
@@ -0,0 +1,646 @@
"""Auto-sync detectors + offset compute + AC-9 validator (AZ-405).
Three concerns:
1. **Tlog take-off detector** — walks the head of the tlog, looks for
a sustained vertical-acceleration excess + sustained attitude-rate
excess, returns ``(takeoff_ns, confidence)``.
2. **Video motion-onset detector** — runs OpenCV pyramidal optical
flow over the leading seconds of the video, returns
``(motion_onset_ns, confidence)``.
3. **AC-9 frame-window match validator** — given a candidate offset
and the tlog/video timestamp series, returns 0 if ≥ 95 % of
video frames have an IMU sample within ± 100 ms after the offset
is applied; 2 otherwise.
The detector functions are split into a thin path-reading wrapper
(``detect_tlog_takeoff`` / ``detect_video_motion_onset``) and a pure
sample-driven core (``_compute_tlog_takeoff_from_samples`` /
``_compute_video_onset_from_samples``). Tests exercise the pure cores
directly with synthetic fixtures; production calls the wrappers,
which read the tlog via ``pymavlink`` and the video via ``cv2``.
Both wrappers accept an optional ``source_factory`` (tlog) /
``frames_factory`` (video) injection point so unit tests can swap in
fakes without touching the filesystem (mirrors AZ-399's pattern).
"""
from __future__ import annotations
import bisect
import math
import os
from collections.abc import Callable, Iterable
from dataclasses import dataclass
from pathlib import Path
from typing import TYPE_CHECKING, Any
from gps_denied_onboard._types.fc import FcKind
from gps_denied_onboard.replay_input.errors import ReplayInputAdapterError
from gps_denied_onboard.replay_input.interface import AutoSyncConfig, AutoSyncDecision
if TYPE_CHECKING:
import numpy as np
__all__ = [
"TlogSamples",
"compute_offset",
"detect_tlog_takeoff",
"detect_video_motion_onset",
"validate_offset_or_fail",
]
# Conversion: MAVLink RAW_IMU / SCALED_IMU2 publish accelerometer
# components in mG (milli-G); 1 g ≡ 9.80665 m/s² by ISO 80000-3.
_MG_PER_G: float = 1000.0
# Per the AZ-405 spec, the vertical-accel signal of interest is the
# magnitude excess above gravity (i.e., body acceleration regardless
# of frame orientation). At rest |a| ≈ 1 g; during upward thrust |a|
# > 1 g; during free-fall |a| ≈ 0 g. The take-off pattern is a
# sustained excess with positive sign (upward thrust), so we use
# ``|total_g - 1.0|`` as the criterion.
_REST_TOTAL_G: float = 1.0
# ---------------------------------------------------------------------
# DTOs (internal — public API surfaces results via AutoSyncDecision)
@dataclass(frozen=True, slots=True)
class _DetectorResult:
"""Outcome of a single detector pass.
``onset_ns`` is the best-guess event start (ns); ``confidence``
is in [0, 1] and reflects how sustained the signal was relative
to the configured threshold + sustained-time requirement.
"""
onset_ns: int
confidence: float
@dataclass(frozen=True, slots=True)
class TlogSamples:
"""Pre-loaded tlog samples extracted by the take-off detector.
Used as the input shape for :func:`_compute_tlog_takeoff_from_samples`
so unit tests can build a deterministic fixture without parsing a
real ``.tlog`` file.
Attributes:
accel: Sequence of ``(ts_ns, total_accel_g)`` pairs sourced
from ``RAW_IMU`` / ``SCALED_IMU2`` messages.
attitude: Sequence of ``(ts_ns, roll_rad, pitch_rad, yaw_rad)``
tuples sourced from ``ATTITUDE`` messages.
imu_count_by_type: Map of message-type-name → count, used for
the ``"tlog missing required message types: [...]"``
error path (R-DEMO-3).
"""
accel: tuple[tuple[int, float], ...]
attitude: tuple[tuple[int, float, float, float], ...]
imu_count_by_type: dict[str, int]
# ---------------------------------------------------------------------
# Public entrypoints
def detect_tlog_takeoff(
tlog_path: Path,
target_fc_dialect: FcKind,
config: AutoSyncConfig,
*,
source_factory: Callable[[str], Any] | None = None,
) -> _DetectorResult:
"""Walk the tlog head, detect the take-off pattern, return result.
Args:
tlog_path: Path to the tlog file. Existence is checked at
entry.
target_fc_dialect: ``ARDUPILOT_PLANE`` or ``INAV``. Both speak
``ardupilotmega`` MAVLink on the GCS telemetry channel
(the iNav-side native MSP traffic is irrelevant here);
this parameter is accepted for parity with the rest of
the replay surface and is also used in the missing-
messages error to name the dialect explicitly.
config: Operator-tunable thresholds (see
:class:`AutoSyncConfig`).
source_factory: Test-only injection — when provided, replaces
the pymavlink open call with the factory's return value.
The factory must yield an object with ``recv_match`` /
``close`` semantics matching pymavlink's
``mavutil.mavlink_connection``.
Raises:
ReplayInputAdapterError: When the tlog is missing
``RAW_IMU`` / ``SCALED_IMU2`` (no IMU samples) or
``ATTITUDE`` (no attitude samples). This is the R-DEMO-3
fail-fast path — it surfaces BEFORE any video read in the
coordinator's ``open()`` flow.
"""
if target_fc_dialect not in (FcKind.ARDUPILOT_PLANE, FcKind.INAV):
raise ReplayInputAdapterError(
f"target_fc_dialect must be ARDUPILOT_PLANE or INAV; got {target_fc_dialect!r}"
)
if not tlog_path.is_file():
raise ReplayInputAdapterError(f"tlog file not found: {tlog_path}")
samples = _load_tlog_samples(
tlog_path,
config.prescan_max_messages,
source_factory=source_factory,
)
return _compute_tlog_takeoff_from_samples(samples, config)
def detect_video_motion_onset(
video_path: Path,
config: AutoSyncConfig,
*,
frames_factory: Callable[[Path, float], Iterable[tuple[int, "np.ndarray"]]]
| None = None,
) -> _DetectorResult:
"""Scan the leading video segment, detect motion onset, return result.
Args:
video_path: Path to an MP4 / MKV / AVI file.
config: Operator-tunable thresholds (see
:class:`AutoSyncConfig`).
frames_factory: Test-only injection — when provided, returns
a synthetic iterable of ``(monotonic_ns, frame_bgr)``
tuples. Must yield at least 2 frames for the pairwise
optical-flow magnitudes to compute.
Raises:
ReplayInputAdapterError: When the video file is missing or
unreadable, or fewer than 2 frames are decoded.
"""
if not video_path.is_file():
raise ReplayInputAdapterError(f"video file not found: {video_path}")
if frames_factory is None:
frames = list(_read_video_frames(video_path, config.video_motion_scan_seconds))
else:
frames = list(frames_factory(video_path, config.video_motion_scan_seconds))
if len(frames) < 2:
raise ReplayInputAdapterError(
f"video file unreadable or too short: {video_path} "
f"(decoded {len(frames)} frame(s); need ≥ 2)"
)
flow_samples = _compute_flow_magnitudes(frames)
return _compute_video_onset_from_samples(flow_samples, config)
def compute_offset(
tlog_result: _DetectorResult,
video_result: _DetectorResult,
) -> AutoSyncDecision:
"""Combine tlog + video detector outputs into an :class:`AutoSyncDecision`.
Offset semantics (positive = video starts before take-off recorded
in tlog): ``offset_ns = tlog_takeoff_ns - video_motion_onset_ns``.
Combined confidence = ``min(tlog_confidence, video_confidence)`` —
the weakest signal dominates so downstream WARN-and-proceed (AC-6)
fires whenever either side is unreliable.
"""
offset_ns = tlog_result.onset_ns - video_result.onset_ns
combined = min(tlog_result.confidence, video_result.confidence)
return AutoSyncDecision(
offset_ms=offset_ns // 1_000_000,
tlog_takeoff_ns=tlog_result.onset_ns,
video_motion_onset_ns=video_result.onset_ns,
tlog_confidence=tlog_result.confidence,
video_confidence=video_result.confidence,
combined_confidence=combined,
)
def validate_offset_or_fail(
offset_ms: int,
tlog_imu_timestamps_ns: Iterable[int],
video_frame_timestamps_ns: Iterable[int],
threshold_pct: float,
*,
window_ms: int = 100,
) -> int:
"""AC-9 frame-window match validator.
Returns ``0`` when ≥ ``threshold_pct`` % of video frames have an
IMU sample within ± ``window_ms`` after the offset is applied;
returns ``2`` otherwise (CLI exit code for AC-8 hard-fail).
The check is symmetric in offset sign — the offset is added to
each video timestamp and the nearest tlog IMU timestamp is then
looked up by binary search.
"""
video_list = list(video_frame_timestamps_ns)
if not video_list:
# Degenerate input — no frames to match. The replay binary
# rejects empty videos earlier, so reaching this branch
# would be a bug; return 2 so the operator sees the hard-fail
# rather than a false PASS.
return 2
tlog_sorted = sorted(tlog_imu_timestamps_ns)
if not tlog_sorted:
return 2
offset_ns = int(offset_ms) * 1_000_000
window_ns = int(window_ms) * 1_000_000
matched = 0
for vts in video_list:
target_ns = vts + offset_ns
idx = bisect.bisect_left(tlog_sorted, target_ns)
# The nearest IMU sample is whichever of the immediate
# neighbours of `target_ns` is closer. Either may be out of
# range at the ends of the array.
nearest: int | None = None
for j in (idx - 1, idx):
if 0 <= j < len(tlog_sorted):
cand = tlog_sorted[j]
if nearest is None or abs(cand - target_ns) < abs(nearest - target_ns):
nearest = cand
if nearest is not None and abs(nearest - target_ns) <= window_ns:
matched += 1
match_pct = (matched / len(video_list)) * 100.0
return 0 if match_pct >= threshold_pct else 2
# ---------------------------------------------------------------------
# Pure compute kernels (testable without disk IO)
def _compute_tlog_takeoff_from_samples(
samples: TlogSamples,
config: AutoSyncConfig,
) -> _DetectorResult:
"""Pure detector: turn pre-loaded tlog samples into a result.
Algorithm: find the first sustained-window where (a) accel
magnitude excess above 1 g exceeds the threshold for at least
``sustained_seconds``, and (b) attitude-rate magnitude exceeds
its threshold sustained over the same duration. Combined
confidence = ``min(accel_ratio, attitude_ratio)`` — both
signals must agree for a high-confidence take-off.
Raises:
ReplayInputAdapterError: When the tlog had no IMU samples or
no ATTITUDE samples (R-DEMO-3 fail-fast).
"""
if not samples.accel:
missing = ["RAW_IMU", "SCALED_IMU2"]
raise ReplayInputAdapterError(
f"tlog missing required message types: {missing}"
)
if not samples.attitude:
raise ReplayInputAdapterError(
"tlog missing required message types: ['ATTITUDE']"
)
sustained_ns = int(config.sustained_seconds * 1_000_000_000)
# Pair-wise attitude rates (rad/s magnitude vector) — emitted at
# the timestamp of the LATER sample so the rate aligns with when
# it is observable downstream.
attitude_rates: list[tuple[int, float]] = []
for i in range(1, len(samples.attitude)):
ts_prev, roll_prev, pitch_prev, yaw_prev = samples.attitude[i - 1]
ts_curr, roll_curr, pitch_curr, yaw_curr = samples.attitude[i]
dt_s = (ts_curr - ts_prev) / 1_000_000_000.0
if dt_s <= 0.0:
continue
dr = roll_curr - roll_prev
dp = pitch_curr - pitch_prev
dy = _wrap_pi(yaw_curr - yaw_prev)
rate_mag = math.sqrt((dr / dt_s) ** 2 + (dp / dt_s) ** 2 + (dy / dt_s) ** 2)
attitude_rates.append((ts_curr, rate_mag))
accel_excess = tuple(
(ts, abs(total_g - _REST_TOTAL_G)) for ts, total_g in samples.accel
)
accel_event = _find_sustained_event(
accel_excess,
threshold=config.takeoff_accel_threshold_g,
sustained_ns=sustained_ns,
)
attitude_event = _find_sustained_event(
tuple(attitude_rates),
threshold=config.takeoff_attitude_rate_threshold_rad_s,
sustained_ns=sustained_ns,
)
if accel_event is None and attitude_event is None:
# Neither signal crossed; best we can do is flag "no clear
# take-off" so the coordinator can WARN and continue with the
# tlog start as a fallback origin.
first_ns = samples.accel[0][0]
return _DetectorResult(onset_ns=first_ns, confidence=0.0)
if accel_event is not None and attitude_event is not None:
# Both signals fired — they should both point at the same
# event. We adopt the EARLIER of the two onsets so the offset
# is referenced against the moment thrust began (the attitude
# body-rate spike usually trails the thrust by a few hundred
# ms during a vertical climb).
onset_ns = min(accel_event[0], attitude_event[0])
# Confidence is the weakest of the two signals, scaled by
# how cleanly they agree. We keep it simple: min().
confidence = min(accel_event[1], attitude_event[1])
elif accel_event is not None:
# Only the accel signal — discount confidence so the
# combined offset eventually trips the WARN-and-proceed
# threshold (combined_confidence < 0.80 → AC-6).
onset_ns, raw_conf = accel_event
confidence = raw_conf * 0.6
else:
# Only attitude rate — same rationale as above. The
# mypy-narrowing else covers attitude_event is not None.
assert attitude_event is not None
onset_ns, raw_conf = attitude_event
confidence = raw_conf * 0.6
return _DetectorResult(onset_ns=onset_ns, confidence=confidence)
def _compute_video_onset_from_samples(
flow_samples: tuple[tuple[int, float], ...],
config: AutoSyncConfig,
) -> _DetectorResult:
"""Pure detector: turn pre-computed optical-flow magnitudes into a result.
Algorithm: find the first sustained window where the flow
magnitude exceeds the configured threshold for at least
``sustained_seconds``. Confidence = sustained ratio.
"""
if not flow_samples:
return _DetectorResult(onset_ns=0, confidence=0.0)
sustained_ns = int(config.sustained_seconds * 1_000_000_000)
event = _find_sustained_event(
flow_samples,
threshold=config.video_motion_threshold,
sustained_ns=sustained_ns,
)
if event is None:
return _DetectorResult(onset_ns=flow_samples[0][0], confidence=0.0)
onset_ns, confidence = event
return _DetectorResult(onset_ns=onset_ns, confidence=confidence)
def _find_sustained_event(
samples: tuple[tuple[int, float], ...] | list[tuple[int, float]],
*,
threshold: float,
sustained_ns: int,
) -> tuple[int, float] | None:
"""Sliding-window scan: return ``(start_ns, ratio)`` of the
earliest window where the fraction of samples above
``threshold`` is maximised, provided that fraction is ≥ 0.5
(signal-vs-noise floor) and the window covers at least 80 % of
``sustained_ns`` (guards against truncated windows at the tail).
Returns ``None`` when no qualifying window exists.
"""
seq = list(samples)
n = len(seq)
if n < 2:
return None
best_start_ns: int | None = None
best_ratio = 0.0
min_window_ns = int(sustained_ns * 0.8)
for i in range(n):
start_ns = seq[i][0]
end_ns = start_ns + sustained_ns
# Walk j forward while still inside the window.
j = i
above = 0
while j < n and seq[j][0] <= end_ns:
if seq[j][1] > threshold:
above += 1
j += 1
window_size = j - i
if window_size < 2:
continue
window_dur_ns = seq[j - 1][0] - start_ns
if window_dur_ns < min_window_ns:
continue
ratio = above / window_size
if ratio > best_ratio:
best_ratio = ratio
best_start_ns = start_ns
if best_start_ns is None or best_ratio < 0.5:
return None
return (best_start_ns, best_ratio)
def _wrap_pi(angle_rad: float) -> float:
"""Wrap an angle delta into ``(-π, π]`` to handle yaw wrap-around."""
twopi = 2.0 * math.pi
a = angle_rad % twopi
if a > math.pi:
a -= twopi
return a
# ---------------------------------------------------------------------
# Disk-reading wrappers (production paths)
_REQUIRED_TLOG_TYPES: tuple[str, ...] = (
"RAW_IMU",
"SCALED_IMU2",
"ATTITUDE",
)
def _load_tlog_samples(
tlog_path: Path,
max_messages: int,
*,
source_factory: Callable[[str], Any] | None,
) -> TlogSamples:
"""Stream the tlog head, capture IMU + ATTITUDE samples.
Mirrors the AZ-399 source-factory test pattern: production builds
use ``pymavlink`` lazily; tests pass an in-memory fake.
"""
source = _open_tlog(tlog_path, source_factory=source_factory)
accel: list[tuple[int, float]] = []
attitude: list[tuple[int, float, float, float]] = []
counts: dict[str, int] = {}
try:
for _ in range(max_messages):
try:
msg = source.recv_match(
type=list(_REQUIRED_TLOG_TYPES),
blocking=False,
)
except Exception as exc: # pragma: no cover — defensive.
raise ReplayInputAdapterError(
f"tlog scan failed on {tlog_path}: {exc!r}"
) from exc
if msg is None:
break
msg_type = _safe_msg_type(msg)
if not msg_type:
continue
counts[msg_type] = counts.get(msg_type, 0) + 1
ts_ns = _msg_timestamp_ns(msg)
if msg_type in ("RAW_IMU", "SCALED_IMU2"):
xa = float(getattr(msg, "xacc", 0.0)) / _MG_PER_G
ya = float(getattr(msg, "yacc", 0.0)) / _MG_PER_G
za = float(getattr(msg, "zacc", 0.0)) / _MG_PER_G
total_g = math.sqrt(xa * xa + ya * ya + za * za)
accel.append((ts_ns, total_g))
elif msg_type == "ATTITUDE":
roll = float(getattr(msg, "roll", 0.0))
pitch = float(getattr(msg, "pitch", 0.0))
yaw = float(getattr(msg, "yaw", 0.0))
attitude.append((ts_ns, roll, pitch, yaw))
finally:
if hasattr(source, "close"):
try:
source.close()
except Exception: # pragma: no cover — defensive.
pass
return TlogSamples(
accel=tuple(accel),
attitude=tuple(attitude),
imu_count_by_type=counts,
)
def _open_tlog(
tlog_path: Path,
*,
source_factory: Callable[[str], Any] | None,
) -> Any:
if source_factory is not None:
return source_factory(str(tlog_path))
try:
from pymavlink import mavutil # type: ignore[import-not-found]
except ImportError as exc:
raise ReplayInputAdapterError(
"pymavlink is required for replay auto-sync but is not "
"importable in this binary"
) from exc
return mavutil.mavlink_connection(
str(tlog_path),
dialect="ardupilotmega",
mavlink_version="2.0",
)
def _safe_msg_type(msg: Any) -> str:
try:
if hasattr(msg, "get_type"):
return str(msg.get_type())
except Exception:
return ""
return type(msg).__name__
def _msg_timestamp_ns(msg: Any) -> int:
raw = getattr(msg, "_timestamp", None)
if raw is None:
raise ReplayInputAdapterError(
"tlog message missing _timestamp attribute; pymavlink "
"mavlogfile should populate it on every recv_match() return"
)
return int(float(raw) * 1_000_000_000)
def _read_video_frames(
video_path: Path,
scan_seconds: float,
) -> Iterable[tuple[int, "np.ndarray"]]:
"""Decode the leading ``scan_seconds`` of the video.
Yields ``(monotonic_ns, frame_bgr)`` tuples where ``monotonic_ns``
is the file's per-frame ``CAP_PROP_POS_MSEC × 1e6`` so the
returned timestamps align with what
:class:`VideoFileFrameSource` will report later. The Python
``time.monotonic_ns()`` is NOT used — the auto-sync result has to
be deterministic across runs (AC-10) and tied to the video
timeline.
"""
try:
import cv2 as _cv2 # type: ignore[import-not-found]
except ImportError as exc:
raise ReplayInputAdapterError(
"opencv-python is required for replay auto-sync but is "
"not importable in this binary"
) from exc
capture = _cv2.VideoCapture(str(video_path))
if not capture.isOpened():
capture.release()
raise ReplayInputAdapterError(
f"video file unreadable / unsupported codec: {video_path}"
)
try:
max_pos_ms = scan_seconds * 1000.0
while True:
ok, frame = capture.read()
if not ok or frame is None:
break
pos_ms = float(capture.get(_cv2.CAP_PROP_POS_MSEC))
if pos_ms > max_pos_ms:
break
ts_ns = int(pos_ms * 1_000_000)
yield ts_ns, frame
finally:
capture.release()
def _compute_flow_magnitudes(
frames: list[tuple[int, "np.ndarray"]],
) -> tuple[tuple[int, float], ...]:
"""Pairwise mean optical-flow magnitude between consecutive frames.
Uses Farneback dense flow (``cv2.calcOpticalFlowFarneback``)
rather than pyramidal LK because Farneback returns a flow field
over the whole image with no per-frame feature-tracking state, so
the result is deterministic given the same input frames (AC-10).
Returns ``((ts_ns_of_second_frame, mean_magnitude_px), ...)``.
"""
try:
import cv2 as _cv2 # type: ignore[import-not-found]
import numpy as _np # type: ignore[import-not-found]
except ImportError as exc: # pragma: no cover — guarded at call sites.
raise ReplayInputAdapterError(
"opencv-python + numpy are required for replay auto-sync"
) from exc
if len(frames) < 2:
return ()
# Convert all frames to grayscale once up-front so the per-pair
# cost is dominated by the optical-flow computation itself.
gray_frames = []
for ts_ns, frame in frames:
gray = _cv2.cvtColor(frame, _cv2.COLOR_BGR2GRAY)
gray_frames.append((ts_ns, gray))
out: list[tuple[int, float]] = []
for i in range(1, len(gray_frames)):
prev_ts, prev = gray_frames[i - 1]
curr_ts, curr = gray_frames[i]
flow = _cv2.calcOpticalFlowFarneback(
prev,
curr,
None,
pyr_scale=0.5,
levels=3,
winsize=15,
iterations=3,
poly_n=5,
poly_sigma=1.2,
flags=0,
)
# ``flow`` shape: (H, W, 2) — dx + dy per pixel.
magnitudes = _np.sqrt(flow[..., 0] ** 2 + flow[..., 1] ** 2)
mean_mag = float(magnitudes.mean())
out.append((curr_ts, mean_mag))
return tuple(out)
# Re-export the BUILD-flag check for symmetry with other replay modules.
def _build_flag_on(name: str) -> bool:
raw = os.environ.get(name, "")
return raw.strip().lower() in {"on", "1", "true", "yes"}
src/gps_denied_onboard/replay_input/errors.py
+38
View File
@@ -0,0 +1,38 @@
"""``replay_input/`` error taxonomy (AZ-405 / E-DEMO-REPLAY).
The coordinator surfaces a single error class so the shared main can
map every coordinator-scope failure to CLI exit code 2 (per epic
AZ-265 AC-8 and the v2.0.0 replay protocol). The class is a subclass
of :class:`RuntimeError` to keep stdlib-style ``except RuntimeError``
catch sites (composition root) covering it without explicit imports.
Translation rule: ``ReplayInputAdapter.open()`` re-raises strategy-side
exceptions — :class:`FcOpenError`, :class:`FrameSourceConfigError`,
:class:`FrameSourceError` — as :class:`ReplayInputAdapterError` after
re-shaping the message into the contract-mandated form (e.g. ``"tlog
missing required message types: [...]"``). The original is chained as
``__cause__`` so debug logs retain the underlying detail.
"""
from __future__ import annotations
__all__ = ["ReplayInputAdapterError"]
class ReplayInputAdapterError(RuntimeError):
"""Base class for every :class:`ReplayInputAdapter` failure.
Concrete failure modes (per epic AZ-265 + replay protocol v2.0.0):
- ``"tlog missing required message types: [...]"`` — R-DEMO-3
fail-fast at startup; raised from inside ``open()`` BEFORE the
video is read so a malformed tlog does not hang on
:class:`cv2.VideoCapture` initialisation.
- ``"auto-sync hard-fail: ..."`` — AC-8 frame-window match
violation; the resolved offset (auto OR manual) failed the
≥ 95 % match threshold.
- ``"video file unreadable / unsupported codec / ..."`` — surfaced
from :class:`FrameSourceConfigError` raised by
:class:`VideoFileFrameSource` at coordinator scope so the CLI's
exit-code mapping stays single-source.
"""
src/gps_denied_onboard/replay_input/interface.py
+145
View File
@@ -0,0 +1,145 @@
"""``replay_input/`` DTOs (AZ-405 / E-DEMO-REPLAY).
Frozen + slotted dataclasses per ADR-002 / module-layout.md so the
composition root and the coordinator can pass these by value without
fear of mutation downstream.
The DTOs come in two flavours:
- :class:`AutoSyncConfig` — operator-tunable thresholds for the
auto-sync algorithm. The composition root builds an instance from
``config.replay.auto_sync`` (owned by AZ-269 / AZ-270) and passes
it to :class:`ReplayInputAdapter`. Defaults match the contract
in :mod:`auto_sync` and the AC-1 / AC-2 / AC-3 thresholds.
- :class:`AutoSyncDecision` — the outcome of one auto-sync run. The
composition root attaches this to the FDR record so an operator can
audit how the offset was resolved.
- :class:`ReplayInputBundle` — the trio of strategies the composition
root consumes after :meth:`ReplayInputAdapter.open` returns. The
bundle also carries the resolved offset so the FDR write at the
start of the replay run can record provenance.
"""
from __future__ import annotations
from dataclasses import dataclass
from typing import TYPE_CHECKING
if TYPE_CHECKING:
from gps_denied_onboard._types.fc import FcKind # noqa: F401 # for docstrings.
from gps_denied_onboard.clock import Clock
from gps_denied_onboard.components.c8_fc_adapter.tlog_replay_adapter import (
TlogReplayFcAdapter,
)
from gps_denied_onboard.frame_source.video_file import VideoFileFrameSource
__all__ = [
"AutoSyncConfig",
"AutoSyncDecision",
"ReplayInputBundle",
]
@dataclass(frozen=True, slots=True)
class AutoSyncConfig:
"""Operator-tunable thresholds for the AZ-405 auto-sync algorithm.
Defaults match the contract in
``_docs/02_document/contracts/replay/replay_protocol.md`` v2.0.0
and the AC-1 / AC-2 / AC-3 thresholds in the AZ-405 spec.
Attributes:
takeoff_accel_threshold_g: Sustained vertical-acceleration
magnitude (in g) above which a tlog sample is considered
part of a take-off pattern. Default 0.5 (AC-1).
takeoff_attitude_rate_threshold_rad_s: Sustained attitude-rate
magnitude (rad/s) above which an ``ATTITUDE`` pair is
considered part of a take-off pattern. Default 1.0.
sustained_seconds: Minimum duration both signals must persist
above their thresholds for a candidate to be accepted.
Default 0.5.
prescan_max_messages: Upper bound on tlog messages walked by
the take-off detector. ~30 s of telemetry at 200 Hz =
6000 messages, matching the AZ-399 pre-scan budget.
video_motion_threshold: Mean optical-flow magnitude (pixels)
above which a video frame pair is considered ``moving``.
Default 1.5 (calibrated for 720p footage).
video_motion_scan_seconds: Length of the leading video segment
inspected for the motion onset. Default 10.0 (AC-4 covers
an onset at frame 11 of a 60-frame fixture).
match_threshold_pct: AC-9 frame-window match-percentage
threshold (default 95.0). Configurable per
``config.replay.auto_sync_match_threshold_pct``.
match_window_ms: AC-9 per-frame matching tolerance in
milliseconds (default 100).
low_confidence_threshold: Combined-confidence cut-off below
which :meth:`ReplayInputAdapter.open` logs WARN and uses
the best-guess offset (AC-6). Default 0.80.
"""
takeoff_accel_threshold_g: float = 0.5
takeoff_attitude_rate_threshold_rad_s: float = 1.0
sustained_seconds: float = 0.5
prescan_max_messages: int = 6000
video_motion_threshold: float = 1.5
video_motion_scan_seconds: float = 10.0
match_threshold_pct: float = 95.0
match_window_ms: int = 100
low_confidence_threshold: float = 0.80
@dataclass(frozen=True, slots=True)
class AutoSyncDecision:
"""Outcome of one auto-sync run (AZ-405).
Attributes:
offset_ms: Resolved offset to be applied to tlog timestamps.
``offset_ms = tlog_takeoff_ns - video_motion_onset_ns``
converted to milliseconds.
tlog_takeoff_ns: Detected tlog take-off timestamp.
video_motion_onset_ns: Detected video motion-onset timestamp.
tlog_confidence: Take-off detector confidence in [0, 1].
video_confidence: Motion-onset detector confidence in [0, 1].
combined_confidence: Aggregated confidence in [0, 1]. Below
:attr:`AutoSyncConfig.low_confidence_threshold` the
coordinator logs WARN and proceeds (AC-6).
"""
offset_ms: int
tlog_takeoff_ns: int
video_motion_onset_ns: int
tlog_confidence: float
video_confidence: float
combined_confidence: float
@dataclass(frozen=True, slots=True)
class ReplayInputBundle:
"""Trio of strategies returned by :meth:`ReplayInputAdapter.open`.
The composition root wires the bundle into the same C1–C7 + C13
pipeline as live (replay protocol Invariant 1 — the components
see only the standard :class:`FrameSource` / :class:`FcAdapter` /
:class:`Clock` interfaces past this point).
Attributes:
frame_source: :class:`VideoFileFrameSource` instance ready
for ``next_frame()`` calls.
fc_adapter: :class:`TlogReplayFcAdapter` instance with its
decode thread already started by :meth:`open`.
clock: :class:`TlogDerivedClock` (pace=ASAP) or
:class:`WallClock` (pace=REALTIME).
resolved_time_offset_ms: Offset applied to tlog timestamps.
Equals either the ``manual_time_offset_ms`` constructor
argument or :attr:`AutoSyncDecision.offset_ms`.
auto_sync_result: Auto-sync outcome; ``None`` when the
constructor received an explicit
``manual_time_offset_ms``.
"""
frame_source: "VideoFileFrameSource"
fc_adapter: "TlogReplayFcAdapter"
clock: "Clock"
resolved_time_offset_ms: int
auto_sync_result: AutoSyncDecision | None
src/gps_denied_onboard/replay_input/tlog_video_adapter.py
+528
View File
@@ -0,0 +1,528 @@
"""``ReplayInputAdapter`` (AZ-405 / E-DEMO-REPLAY).
Layer-4 cross-cutting coordinator that converges ``(video, tlog)``
inputs into the standard :class:`FrameSource`, :class:`FcAdapter`,
and :class:`Clock` surfaces consumed by the airborne composition
root. Owns the time-alignment concern: either the operator's manual
``--time-offset-ms`` override or the AZ-405 IMU-take-off auto-detect.
``open()`` performs strict ordering so AC-13 holds:
1. **Tlog message-type pre-validation** runs FIRST so a tlog missing
``RAW_IMU`` / ``ATTITUDE`` raises before the video is ever read.
2. If the constructor received ``manual_time_offset_ms is None``,
the auto-sync detectors run; otherwise the manual offset is
adopted directly (AC-8 verifies the bypass).
3. The resolved offset is fed through the AC-9 frame-window match
validator; a hard-fail raises ``"auto-sync hard-fail: …"`` so
the shared main maps it to CLI exit code 2 (AC-7).
4. The :class:`Clock` strategy is constructed (``TlogDerivedClock``
for ``pace=ASAP``, ``WallClock`` for ``pace=REALTIME``) — the
single instance the bundle ships to the composition root
(Invariant 2; AC-5).
5. :class:`VideoFileFrameSource` and :class:`TlogReplayFcAdapter`
are constructed against the offset + clock + dialect; the FC
adapter's own ``open()`` triggers its independent pre-scan (a
second sanity check; the operator gets the original error path
if step 1 was bypassed via a test fake).
6. The bundle is returned with ``auto_sync_result`` populated for
the auto path and ``None`` for the manual path.
The coordinator is idempotent on ``close()`` — repeated calls are
no-ops once the underlying strategies have been released (AC-12).
"""
from __future__ import annotations
import logging
from pathlib import Path
from typing import TYPE_CHECKING, Any
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.errors import (
FcAdapterConfigError,
FcAdapterError,
FcOpenError,
)
from gps_denied_onboard.components.c8_fc_adapter.tlog_replay_adapter import (
ReplayPace,
TlogReplayFcAdapter,
)
from gps_denied_onboard.fdr_client.records import FdrRecord
from gps_denied_onboard.frame_source.errors import (
FrameSourceConfigError,
FrameSourceError,
)
from gps_denied_onboard.frame_source.video_file import VideoFileFrameSource
from gps_denied_onboard.helpers.iso_timestamps import iso_ts_now
from gps_denied_onboard.replay_input.auto_sync import (
_load_tlog_samples,
compute_offset,
detect_video_motion_onset,
validate_offset_or_fail,
)
from gps_denied_onboard.replay_input.errors import ReplayInputAdapterError
from gps_denied_onboard.replay_input.interface import (
AutoSyncConfig,
AutoSyncDecision,
ReplayInputBundle,
)
if TYPE_CHECKING:
from gps_denied_onboard._types.calibration import CameraCalibration
from gps_denied_onboard.clock import Clock
from gps_denied_onboard.fdr_client.client import FdrClient
from gps_denied_onboard.helpers.wgs_converter import WgsConverter
__all__ = ["ReplayInputAdapter"]
_FDR_PRODUCER_ID = "replay_input.tlog_video_adapter"
_LOG_KIND_AUTO_SYNC_DETECTED = "replay.auto_sync.detected"
_LOG_KIND_AUTO_SYNC_LOW_CONF = "replay.auto_sync.low_confidence"
_LOG_KIND_AUTO_SYNC_AC8_FAIL = "replay.auto_sync.ac8_validation_failed"
_LOG_KIND_OPEN_MANUAL = "replay.input.opened_manual_offset"
class ReplayInputAdapter:
"""Coordinator that converges ``(video, tlog)`` into the airborne strategies.
Constructor parameters:
- ``video_path`` / ``tlog_path`` — filesystem inputs.
- ``camera_calibration`` — :class:`CameraCalibration` used to
derive the calibration ID propagated into every emitted
:class:`NavCameraFrame`.
- ``target_fc_dialect`` — ``ARDUPILOT_PLANE`` or ``INAV``;
passed through to :class:`TlogReplayFcAdapter`.
- ``wgs_converter`` — shared geodesy helper, constructor-injected
into :class:`TlogReplayFcAdapter`.
- ``fdr_client`` — FDR sink for the TlogReplayFcAdapter and for
the coordinator's own structured-event mirror.
- ``pace`` — :class:`ReplayPace` (``ASAP`` or ``REALTIME``).
- ``manual_time_offset_ms`` — ``None`` triggers auto-sync; an
integer bypasses auto-sync entirely (AC-8).
- ``auto_sync_config`` — :class:`AutoSyncConfig` thresholds.
Behaviour:
- :meth:`open` resolves the offset, validates AC-9, and returns a
:class:`ReplayInputBundle` with the wired strategies. Raises
:class:`ReplayInputAdapterError` on every coordinator-scope
failure so the shared main can map cleanly to CLI exit code 2.
- :meth:`close` releases the FC adapter and the frame source;
idempotent (AC-12).
"""
__slots__ = (
"_video_path",
"_tlog_path",
"_camera_calibration",
"_target_fc_dialect",
"_wgs_converter",
"_fdr_client",
"_pace",
"_manual_time_offset_ms",
"_auto_sync_config",
"_tlog_source_factory",
"_video_frames_factory",
"_video_timestamps_factory",
"_log",
"_opened",
"_closed",
"_bundle",
)
def __init__(
self,
*,
video_path: Path,
tlog_path: Path,
camera_calibration: "CameraCalibration",
target_fc_dialect: FcKind,
wgs_converter: "WgsConverter",
fdr_client: "FdrClient",
pace: ReplayPace,
manual_time_offset_ms: int | None,
auto_sync_config: AutoSyncConfig,
tlog_source_factory: Any | None = None,
video_frames_factory: Any | None = None,
video_timestamps_factory: Any | None = None,
) -> None:
if not isinstance(video_path, Path):
raise ReplayInputAdapterError(
f"video_path must be a pathlib.Path; got {type(video_path).__name__}"
)
if not isinstance(tlog_path, Path):
raise ReplayInputAdapterError(
f"tlog_path must be a pathlib.Path; got {type(tlog_path).__name__}"
)
if target_fc_dialect not in (FcKind.ARDUPILOT_PLANE, FcKind.INAV):
raise ReplayInputAdapterError(
f"target_fc_dialect must be ARDUPILOT_PLANE or INAV; "
f"got {target_fc_dialect!r}"
)
if not isinstance(pace, ReplayPace):
raise ReplayInputAdapterError(
f"pace must be a ReplayPace enum; got {type(pace).__name__}"
)
self._video_path = video_path
self._tlog_path = tlog_path
self._camera_calibration = camera_calibration
self._target_fc_dialect = target_fc_dialect
self._wgs_converter = wgs_converter
self._fdr_client = fdr_client
self._pace = pace
self._manual_time_offset_ms = manual_time_offset_ms
self._auto_sync_config = auto_sync_config
self._tlog_source_factory = tlog_source_factory
self._video_frames_factory = video_frames_factory
self._video_timestamps_factory = video_timestamps_factory
self._log = logging.getLogger("replay_input.tlog_video_adapter")
self._opened = False
self._closed = False
self._bundle: ReplayInputBundle | None = None
def open(self) -> ReplayInputBundle:
"""Resolve the offset, build the strategies, return the bundle.
Idempotent only in the failure-then-retry sense — calling
``open()`` twice without an intervening ``close()`` raises
:class:`ReplayInputAdapterError`.
"""
if self._opened:
raise ReplayInputAdapterError("ReplayInputAdapter already opened")
# Step 1 — tlog presence + required-message check (R-DEMO-3,
# AC-13). Runs BEFORE any video read so a malformed tlog
# surfaces without paying the cv2.VideoCapture cost.
tlog_imu_timestamps_ns, tlog_samples_for_auto = self._load_and_validate_tlog()
# Step 2 — resolve the offset (auto-sync or manual override).
decision: AutoSyncDecision | None
if self._manual_time_offset_ms is None:
decision = self._run_auto_sync(tlog_samples_for_auto)
resolved_offset_ms = decision.offset_ms
else:
decision = None
resolved_offset_ms = int(self._manual_time_offset_ms)
self._log.info(
f"{_LOG_KIND_OPEN_MANUAL}: resolved_offset_ms={resolved_offset_ms}",
extra={
"kind": _LOG_KIND_OPEN_MANUAL,
"kv": {"resolved_offset_ms": resolved_offset_ms},
},
)
# Step 3 — load video frame timestamps and run AC-9 validator.
video_frame_timestamps_ns = self._load_video_timestamps()
result_code = validate_offset_or_fail(
resolved_offset_ms,
tlog_imu_timestamps_ns,
video_frame_timestamps_ns,
threshold_pct=self._auto_sync_config.match_threshold_pct,
window_ms=self._auto_sync_config.match_window_ms,
)
if result_code != 0:
self._raise_ac8_fail(
resolved_offset_ms,
len(tlog_imu_timestamps_ns),
len(video_frame_timestamps_ns),
)
# Step 4 — clock strategy (single instance per Invariant 2).
clock = self._build_clock()
# Step 5 — concrete strategies. The frame source is built
# first because its constructor verifies the build flag and
# opens the cv2 capture handle — a failure here is a clean
# config error (no resources held). The FC adapter is built
# second; its open() launches the decode thread.
try:
frame_source = VideoFileFrameSource(
path=self._video_path,
camera_calibration_id=self._camera_calibration.camera_id,
clock=clock,
)
except FrameSourceConfigError as exc:
raise ReplayInputAdapterError(
f"video file unreadable / unsupported codec: {self._video_path} "
f"({exc})"
) from exc
except FrameSourceError as exc:
raise ReplayInputAdapterError(
f"video file decode error: {self._video_path} ({exc})"
) from exc
try:
fc_adapter = TlogReplayFcAdapter(
tlog_path=self._tlog_path,
target_fc_dialect=self._target_fc_dialect,
clock=clock,
wgs_converter=self._wgs_converter,
fdr_client=self._fdr_client,
time_offset_ms=resolved_offset_ms,
pace=self._pace,
source_factory=self._tlog_source_factory,
)
fc_adapter.open()
except (FcOpenError, FcAdapterConfigError, FcAdapterError) as exc:
# Release the already-built frame source so we do not
# leak the cv2 handle when the FC adapter fails after
# the video was opened.
try:
frame_source.close()
except Exception: # pragma: no cover — defensive.
self._log.debug(
"ReplayInputAdapter: frame_source.close() during FC adapter rollback failed",
exc_info=True,
)
# Translate the FC error into the coordinator's single
# public failure shape so the CLI exit-code mapping
# remains single-source. Pre-scan failures naturally
# surface the "tlog missing required messages: …" prefix
# the contract mandates.
raise ReplayInputAdapterError(str(exc)) from exc
# Step 6 — assemble + record the bundle.
bundle = ReplayInputBundle(
frame_source=frame_source,
fc_adapter=fc_adapter,
clock=clock,
resolved_time_offset_ms=resolved_offset_ms,
auto_sync_result=decision,
)
self._bundle = bundle
self._opened = True
return bundle
def close(self) -> None:
"""Release the FC adapter + frame source; idempotent (AC-12)."""
if self._closed:
self._log.debug(
"ReplayInputAdapter.close called twice; no-op"
)
return
self._closed = True
bundle = self._bundle
self._bundle = None
if bundle is None:
return
try:
bundle.fc_adapter.close()
except Exception: # pragma: no cover — defensive.
self._log.debug(
"ReplayInputAdapter: fc_adapter.close() raised", exc_info=True
)
try:
bundle.frame_source.close()
except Exception: # pragma: no cover — defensive.
self._log.debug(
"ReplayInputAdapter: frame_source.close() raised", exc_info=True
)
# ------------------------------------------------------------------
# Internal helpers
def _load_and_validate_tlog(
self,
) -> tuple[list[int], Any]:
"""Load tlog IMU + ATTITUDE samples; raise on missing types.
Returns the IMU-only timestamp list (used by the AC-9
validator) plus the full :class:`TlogSamples` so the auto-
sync path can reuse the same scan for take-off detection.
Raises :class:`ReplayInputAdapterError` for the R-DEMO-3
missing-types path; this is the AC-13 fail-fast surface.
"""
if not self._tlog_path.is_file():
raise ReplayInputAdapterError(
f"tlog file not found: {self._tlog_path}"
)
samples = _load_tlog_samples(
self._tlog_path,
self._auto_sync_config.prescan_max_messages,
source_factory=self._tlog_source_factory,
)
if not samples.accel:
raise ReplayInputAdapterError(
"tlog missing required message types: ['RAW_IMU', 'SCALED_IMU2']"
)
if not samples.attitude:
raise ReplayInputAdapterError(
"tlog missing required message types: ['ATTITUDE']"
)
return [ts for ts, _ in samples.accel], samples
def _run_auto_sync(self, tlog_samples: Any) -> AutoSyncDecision:
"""Auto path — compute the take-off / motion-onset / offset.
Re-uses the already-loaded ``tlog_samples`` for the take-off
detector so the tlog is walked exactly once per ``open()``
regardless of which path runs.
"""
from gps_denied_onboard.replay_input.auto_sync import (
_compute_tlog_takeoff_from_samples,
)
tlog_result = _compute_tlog_takeoff_from_samples(
tlog_samples, self._auto_sync_config
)
video_result = detect_video_motion_onset(
self._video_path,
self._auto_sync_config,
frames_factory=self._video_frames_factory,
)
decision = compute_offset(tlog_result, video_result)
if decision.combined_confidence < self._auto_sync_config.low_confidence_threshold:
self._log_decision(
kind=_LOG_KIND_AUTO_SYNC_LOW_CONF,
level="WARN",
decision=decision,
extra_kv={"proceeding_with_best_guess": True},
)
else:
self._log_decision(
kind=_LOG_KIND_AUTO_SYNC_DETECTED,
level="INFO",
decision=decision,
extra_kv={},
)
return decision
def _load_video_timestamps(self) -> list[int]:
"""Decode the leading video segment, return per-frame timestamps.
Used by the AC-9 frame-window match validator and as a
fallback when the auto-sync video scan was bypassed (manual
path). Stops at ``video_motion_scan_seconds`` so wildly long
clips do not hold up startup.
"""
if self._video_timestamps_factory is not None:
return list(self._video_timestamps_factory(self._video_path))
try:
import cv2 as _cv2 # type: ignore[import-not-found]
except ImportError as exc:
raise ReplayInputAdapterError(
"opencv-python is required for replay auto-sync but is "
"not importable in this binary"
) from exc
capture = _cv2.VideoCapture(str(self._video_path))
if not capture.isOpened():
capture.release()
raise ReplayInputAdapterError(
f"video file unreadable / unsupported codec: {self._video_path}"
)
out: list[int] = []
max_pos_ms = self._auto_sync_config.video_motion_scan_seconds * 1000.0
try:
while True:
ok = capture.grab()
if not ok:
break
pos_ms = float(capture.get(_cv2.CAP_PROP_POS_MSEC))
if pos_ms > max_pos_ms:
break
out.append(int(pos_ms * 1_000_000))
finally:
capture.release()
return out
def _build_clock(self) -> "Clock":
"""Pick the :class:`Clock` strategy per pace; single instance.
The ``TlogDerivedClock`` is constructed against an empty
iterable here: the composition root (AZ-401) is responsible
for hooking the clock's source up to the live tlog cursor
once the FC adapter's decode thread starts streaming. The
empty-source default keeps unit tests self-contained.
"""
if self._pace is ReplayPace.ASAP:
return TlogDerivedClock(source=iter([]))
return WallClock()
def _log_decision(
self,
*,
kind: str,
level: str,
decision: AutoSyncDecision,
extra_kv: dict[str, Any],
) -> None:
kv: dict[str, Any] = {
"tlog_takeoff_ns": decision.tlog_takeoff_ns,
"video_motion_onset_ns": decision.video_motion_onset_ns,
"offset_ms": decision.offset_ms,
"tlog_confidence": decision.tlog_confidence,
"video_confidence": decision.video_confidence,
"combined_confidence": decision.combined_confidence,
}
kv.update(extra_kv)
msg = f"{kind}: offset_ms={decision.offset_ms} confidence={decision.combined_confidence:.3f}"
if level == "WARN":
self._log.warning(msg, extra={"kind": kind, "kv": kv})
else:
self._log.info(msg, extra={"kind": kind, "kv": kv})
self._emit_fdr_event(level=level, log_kind=kind, msg=msg, kv=kv)
def _raise_ac8_fail(
self,
offset_ms: int,
imu_count: int,
frame_count: int,
) -> None:
kv = {
"offset_ms": offset_ms,
"frame_window_match_pct_threshold": self._auto_sync_config.match_threshold_pct,
"imu_sample_count": imu_count,
"video_frame_count": frame_count,
}
msg = (
f"auto-sync hard-fail: frame-window match below "
f"{self._auto_sync_config.match_threshold_pct}% with "
f"offset_ms={offset_ms}"
)
self._log.error(
f"{_LOG_KIND_AUTO_SYNC_AC8_FAIL}: {msg}",
extra={"kind": _LOG_KIND_AUTO_SYNC_AC8_FAIL, "kv": kv},
)
self._emit_fdr_event(
level="ERROR", log_kind=_LOG_KIND_AUTO_SYNC_AC8_FAIL, msg=msg, kv=kv
)
raise ReplayInputAdapterError(msg)
def _emit_fdr_event(
self,
*,
level: str,
log_kind: str,
msg: str,
kv: dict[str, Any],
) -> None:
record = FdrRecord(
schema_version=1,
ts=iso_ts_now(),
producer_id=_FDR_PRODUCER_ID,
kind="log",
payload={
"level": level,
"component": "replay_input",
"kind": log_kind,
"msg": msg,
"kv": kv,
},
)
try:
self._fdr_client.enqueue(record)
except Exception as exc:
self._log.debug(
f"replay_input.fdr_enqueue_failed: {exc!r}",
extra={
"kind": "replay_input.fdr_enqueue_failed",
"kv": {"error": repr(exc), "downstream_kind": log_kind},
},
)