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

[AZ-894] [AZ-896] Add CSV-driven replay adapter + format docs

Browse Source 
Replaces the tlog two-clock replay surface with a single-clock path
driven by the Derkachi-schema CSV. --imu is the new required CLI arg;
--tlog stays as a deprecated alias (warned + ignored when --imu set)
until AZ-895 deletes it.

* csv_ground_truth.py parses the 15-column schema, fails fast at
  startup on every documented schema fault (AC-5).
* CsvReplayFcAdapter slots into ReplayInputBundle.fc_adapter alongside
  the tlog sibling; mirrors Invariant-5 outbound wiring; inbound bus is
  intentionally a no-op since the loop reads CSV directly.
* _run_replay_loop branches on imu_csv_path, stamps
  VioOutput.emitted_at_ns from the CSV-derived frame_end_ns (AC-4),
  closing the AZ-848 two-clock surface for the new path.
* AZ-896 ships the operator-facing format spec at
  _docs/02_document/contracts/replay/csv_replay_format.md plus a
  20-row example CSV (AC-3 regression-locked).

Tests: 11 + 12 new unit tests, plus updates to AZ-401 import-boundary
and AZ-402 CLI suites. Full unit suite 2,327 passed / 86 skipped.

Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
Oleksandr Bezdieniezhnykh
2026-05-26 18:40:29 +03:00
parent 3020779404
commit 6be207cef3
19 changed files with 1833 additions and 93 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/gps_denied_onboard/runtime_root/__init__.py
+131 -71
View File
@@ -789,6 +789,7 @@ def _run_replay_loop(config: Config, runtime: RuntimeRoot) -> int:
fixes (cold-start impossible), or when the estimator raises a
fatal error. ``EXIT_SUCCESS`` (0) on clean completion.
"""
import dataclasses
import time
from gps_denied_onboard._types.geo import LatLonAlt
@@ -802,6 +803,9 @@ def _run_replay_loop(config: Config, runtime: RuntimeRoot) -> int:
EstimatorFatalError,
)
from gps_denied_onboard.logging import get_logger
from gps_denied_onboard.replay_input.csv_ground_truth import (
load_csv_ground_truth,
)
from gps_denied_onboard.replay_input.errors import ReplayInputAdapterError
from gps_denied_onboard.replay_input.tlog_ground_truth import (
load_tlog_ground_truth,
@@ -856,38 +860,66 @@ def _run_replay_loop(config: Config, runtime: RuntimeRoot) -> int:
# explicitly.
calibration = _load_camera_calibration(config)
# Cold-start origin from tlog's first GPS fix. This is the
# ADR-010 / Principle #11 documented fallback when no operator
# Manifest is available. ESKF/GTSAM both require an origin
# before the first add_fc_imu (else EstimatorAlreadyStartedError).
# Cold-start origin from the first GPS fix in the configured input.
# AZ-894: prefer the CSV ground-truth (single canonical clock); fall
# back to the legacy tlog path when ``imu_csv_path`` is unset (the
# AZ-895 deprecation completes the removal).
csv_path_str = config.replay.imu_csv_path
tlog_path_str = config.replay.tlog_path
_log.info(
"replay_loop.loading_gps_for_cold_start: tlog_path=%s",
tlog_path_str,
extra={
"kind": "replay_loop.loading_gps_for_cold_start",
"kv": {"tlog_path": tlog_path_str},
},
)
try:
gt = load_tlog_ground_truth(Path(tlog_path_str))
except ReplayInputAdapterError as exc:
_log.error(
"replay_loop.tlog_load_failed: %r",
exc,
using_csv = bool(csv_path_str)
if using_csv:
_log.info(
"replay_loop.loading_gps_for_cold_start: imu_csv_path=%s",
csv_path_str,
extra={
"kind": "replay_loop.tlog_load_failed",
"kv": {"error": repr(exc)},
"kind": "replay_loop.loading_gps_for_cold_start",
"kv": {"imu_csv_path": csv_path_str},
},
)
return EXIT_GENERIC_FAILURE
try:
csv_gt = load_csv_ground_truth(Path(csv_path_str))
except ReplayInputAdapterError as exc:
_log.error(
"replay_loop.csv_load_failed: %r",
exc,
extra={
"kind": "replay_loop.csv_load_failed",
"kv": {"error": repr(exc)},
},
)
return EXIT_GENERIC_FAILURE
gt = csv_gt
else:
_log.info(
"replay_loop.loading_gps_for_cold_start: tlog_path=%s",
tlog_path_str,
extra={
"kind": "replay_loop.loading_gps_for_cold_start",
"kv": {"tlog_path": tlog_path_str},
},
)
try:
gt = load_tlog_ground_truth(Path(tlog_path_str))
except ReplayInputAdapterError as exc:
_log.error(
"replay_loop.tlog_load_failed: %r",
exc,
extra={
"kind": "replay_loop.tlog_load_failed",
"kv": {"error": repr(exc)},
},
)
return EXIT_GENERIC_FAILURE
if not gt.records:
_log.error(
"replay_loop.cold_start_impossible: tlog has no GPS messages, "
"replay_loop.cold_start_impossible: input has no GPS fixes, "
"cannot seed C5 set_takeoff_origin",
extra={
"kind": "replay_loop.cold_start_impossible",
"kv": {"tlog_path": tlog_path_str},
"kv": {
"input_path": csv_path_str if using_csv else tlog_path_str,
"input_kind": "csv" if using_csv else "tlog",
},
},
)
return EXIT_GENERIC_FAILURE
@@ -924,22 +956,29 @@ def _run_replay_loop(config: Config, runtime: RuntimeRoot) -> int:
},
)
# Open the tlog directly for synchronous IMU read. Bypasses the
# decode-thread race in TlogReplayFcAdapter (see docstring).
try:
from pymavlink import mavutil # type: ignore[import-untyped]
except ImportError as exc:
_log.error(
"replay_loop.pymavlink_unavailable: %r",
exc,
extra={
"kind": "replay_loop.pymavlink_unavailable",
"kv": {"error": repr(exc)},
},
)
return EXIT_GENERIC_FAILURE
tlog_reader = mavutil.mavlink_connection(str(tlog_path_str))
# IMU source. CSV path: walk the pre-loaded list (gt.imu_samples)
# so the loop's IMU draining stays on the single canonical clock.
# Tlog path (legacy): open the tlog directly for synchronous IMU
# read; bypasses the decode-thread race in TlogReplayFcAdapter.
tlog_reader: Any = None
csv_imu_samples: tuple[ImuSample, ...] = ()
csv_imu_idx = 0
if using_csv:
csv_imu_samples = csv_gt.imu_samples
else:
try:
from pymavlink import mavutil # type: ignore[import-untyped]
except ImportError as exc:
_log.error(
"replay_loop.pymavlink_unavailable: %r",
exc,
extra={
"kind": "replay_loop.pymavlink_unavailable",
"kv": {"error": repr(exc)},
},
)
return EXIT_GENERIC_FAILURE
tlog_reader = mavutil.mavlink_connection(str(tlog_path_str))
# IMU sample buffer used to build per-frame ImuWindows. We
# accumulate every RAW_IMU/SCALED_IMU2 sample whose FC-clock
@@ -949,32 +988,41 @@ def _run_replay_loop(config: Config, runtime: RuntimeRoot) -> int:
imu_eof = False
def _drain_imu_until(target_ns: int) -> None:
"""Advance the tlog reader, appending IMU samples up to ``target_ns``.
"""Advance the IMU source, appending samples up to ``target_ns``.
Stops at end-of-stream (``recv_match`` returns ``None``).
Mirrors :meth:`TlogReplayFcAdapter._handle_imu` for sample
construction so the bytes-on-wire and the synchronous-read
paths produce identical IMU samples.
Branches on ``using_csv`` so the closure stays a single
definition. Both branches respect the same ``pending_imu``
buffer + ``imu_anchor_ns`` / ``imu_eof`` state and produce
:class:`ImuSample` instances with identical numeric semantics
— the tlog branch matches :meth:`TlogReplayFcAdapter._handle_imu`
for byte-for-byte compatibility with the legacy path.
"""
nonlocal imu_anchor_ns, imu_eof
nonlocal imu_anchor_ns, imu_eof, csv_imu_idx
while not imu_eof:
if pending_imu and pending_imu[-1].ts_ns >= target_ns:
return
msg = tlog_reader.recv_match(
type=["RAW_IMU", "SCALED_IMU2"],
blocking=False,
)
if msg is None:
imu_eof = True
return
ts_ns = int(getattr(msg, "time_usec", 0)) * 1000
if ts_ns == 0:
continue
sample = ImuSample(
ts_ns=ts_ns,
accel_xyz=(float(msg.xacc), float(msg.yacc), float(msg.zacc)),
gyro_xyz=(float(msg.xgyro), float(msg.ygyro), float(msg.zgyro)),
)
if using_csv:
if csv_imu_idx >= len(csv_imu_samples):
imu_eof = True
return
sample = csv_imu_samples[csv_imu_idx]
csv_imu_idx += 1
else:
msg = tlog_reader.recv_match(
type=["RAW_IMU", "SCALED_IMU2"],
blocking=False,
)
if msg is None:
imu_eof = True
return
ts_ns = int(getattr(msg, "time_usec", 0)) * 1000
if ts_ns == 0:
continue
sample = ImuSample(
ts_ns=ts_ns,
accel_xyz=(float(msg.xacc), float(msg.yacc), float(msg.zacc)),
gyro_xyz=(float(msg.xgyro), float(msg.ygyro), float(msg.zgyro)),
)
if imu_anchor_ns is None:
imu_anchor_ns = sample.ts_ns
pending_imu.append(sample)
@@ -1141,6 +1189,17 @@ def _run_replay_loop(config: Config, runtime: RuntimeRoot) -> int:
return EXIT_GENERIC_FAILURE
if vio_out is not None:
# AZ-894 AC-4: when the CSV adapter drives the loop,
# stamp VioOutput.emitted_at_ns with the CSV-derived
# frame timestamp (single-clock invariant). Without
# this, C1 produces a Jetson-monotonic timestamp that
# disagrees with the CSV-anchored ImuWindow.ts_end_ns
# the estimator consumed the line before — exactly
# the AZ-848 two-clock surface this ticket eliminates.
if using_csv:
vio_out = dataclasses.replace(
vio_out, emitted_at_ns=frame_end_ns
)
try:
state_estimator.add_vio(vio_out)
except EstimatorDegradedError as exc:
@@ -1203,17 +1262,18 @@ def _run_replay_loop(config: Config, runtime: RuntimeRoot) -> int:
if slack_ns > 0:
time.sleep(slack_ns / 1_000_000_000.0)
finally:
try:
tlog_reader.close()
except Exception as exc: # pragma: no cover — defensive.
_log.debug(
"replay_loop.tlog_reader_close_error: %r",
exc,
extra={
"kind": "replay_loop.tlog_reader_close_error",
"kv": {"error": repr(exc)},
},
)
if tlog_reader is not None:
try:
tlog_reader.close()
except Exception as exc: # pragma: no cover — defensive.
_log.debug(
"replay_loop.tlog_reader_close_error: %r",
exc,
extra={
"kind": "replay_loop.tlog_reader_close_error",
"kv": {"error": repr(exc)},
},
)
_log.info(
"replay_loop.complete: frames=%d emitted=%d vio_init_skipped=%d "
src/gps_denied_onboard/runtime_root/_replay_branch.py
+90 -5
View File
@@ -36,6 +36,9 @@ from typing import TYPE_CHECKING, Any, Final
from gps_denied_onboard._types.calibration import CameraCalibration
from gps_denied_onboard._types.fc import FcKind
from gps_denied_onboard.components.c8_fc_adapter.csv_replay_adapter import (
CsvReplayFcAdapter,
)
from gps_denied_onboard.components.c8_fc_adapter.noop_mavlink_transport import (
NoopMavlinkTransport,
)
@@ -44,6 +47,7 @@ from gps_denied_onboard.components.c8_fc_adapter.replay_sink import (
)
from gps_denied_onboard.config import Config
from gps_denied_onboard.fdr_client import make_fdr_client
from gps_denied_onboard.frame_source.video_file import VideoFileFrameSource
from gps_denied_onboard.helpers.wgs_converter import WgsConverter
from gps_denied_onboard.logging import get_logger
from gps_denied_onboard.replay_input import (
@@ -73,6 +77,12 @@ REPLAY_BUILD_FLAGS: Final[tuple[str, ...]] = (
"BUILD_REPLAY_SINK_JSONL",
)
# AZ-894: separate build flag for the CSV adapter so the replay binary
# can opt into the new path without disturbing the BUILD_TLOG_* gate
# (the tlog adapter is still composed by _build_replay_input_bundle's
# legacy branch until AZ-895 removes it).
_CSV_REPLAY_BUILD_FLAG: Final[str] = "BUILD_CSV_REPLAY_ADAPTER"
REPLAY_COMPONENT_KEYS: Final[tuple[str, ...]] = (
"frame_source",
@@ -175,14 +185,21 @@ def _validate_build_flags() -> None:
def _validate_replay_paths(config: Config) -> None:
"""Reject empty / missing replay paths early with a precise message."""
"""Reject empty / missing replay paths early with a precise message.
AZ-894: ``imu_csv_path`` is the canonical replay input. ``tlog_path``
remains valid for the legacy auto-sync path until AZ-895 removes it,
but exactly one of the two must be set so the composition root can
pick a single branch.
"""
if not config.replay.video_path:
raise CompositionError(
"config.replay.video_path is empty; replay mode requires a video path"
)
if not config.replay.tlog_path:
if not config.replay.imu_csv_path and not config.replay.tlog_path:
raise CompositionError(
"config.replay.tlog_path is empty; replay mode requires a tlog path"
"config.replay.imu_csv_path is empty and no tlog_path fallback is set; "
"replay mode requires an IMU+GPS CSV (AZ-894) or a tlog file (legacy)"
)
if not config.replay.output_path:
raise CompositionError(
@@ -197,13 +214,31 @@ def _build_replay_input_bundle(
adapter_factory: Any | None,
mavlink_transport: Any | None = None,
) -> ReplayInputBundle:
"""Build the :class:`ReplayInputAdapter` and call ``open()``."""
"""Build the replay input bundle and open the underlying strategies.
AZ-894: branches on ``config.replay.imu_csv_path`` — when set, builds
the :class:`CsvReplayFcAdapter` + :class:`VideoFileFrameSource` pair
on a single canonical clock derived from the CSV's ``Time`` column;
when unset, falls back to the legacy :class:`ReplayInputAdapter`
tlog path (auto-sync + AC-9 validator). AZ-895 removes the legacy
branch.
"""
pace = _resolve_pace(config.replay.pace)
target_fc_dialect = _resolve_fc_kind(config.replay.target_fc_dialect)
auto_sync = _build_auto_sync_config(config)
camera_calibration = _load_camera_calibration(config)
wgs_converter = WgsConverter()
if config.replay.imu_csv_path:
return _build_csv_bundle(
config,
fdr_client=fdr_client,
pace=pace,
target_fc_dialect=target_fc_dialect,
camera_calibration=camera_calibration,
mavlink_transport=mavlink_transport,
)
auto_sync = _build_auto_sync_config(config)
if adapter_factory is not None:
adapter = adapter_factory(
config=config,
@@ -233,6 +268,56 @@ def _build_replay_input_bundle(
return adapter.open()
def _build_csv_bundle(
config: Config,
*,
fdr_client: "FdrClient",
pace: ReplayPace,
target_fc_dialect: FcKind,
camera_calibration: CameraCalibration,
mavlink_transport: Any | None,
) -> ReplayInputBundle:
"""Compose the AZ-894 CSV bundle (frame source + CSV FC adapter + clock).
No auto-sync / auto-trim is run — the CSV's ``Time`` column is the
single canonical clock by construction, so ``resolved_time_offset_ms``
is fixed at 0 and ``auto_sync_result`` / ``aligned_window`` are
``None``.
"""
from gps_denied_onboard.clock.tlog_derived import TlogDerivedClock
from gps_denied_onboard.clock.wall_clock import WallClock
csv_path = Path(config.replay.imu_csv_path)
if not csv_path.is_file():
raise CompositionError(
f"config.replay.imu_csv_path points at a missing file: {csv_path}"
)
clock = TlogDerivedClock(source=iter([])) if pace is ReplayPace.ASAP else WallClock()
frame_source = VideoFileFrameSource(
path=Path(config.replay.video_path),
camera_calibration_id=camera_calibration.camera_id,
clock=clock,
)
fc_adapter = CsvReplayFcAdapter(
csv_path=csv_path,
target_fc_dialect=target_fc_dialect,
clock=clock,
fdr_client=fdr_client,
pace=pace,
mavlink_transport=mavlink_transport,
)
fc_adapter.open()
return ReplayInputBundle(
frame_source=frame_source,
fc_adapter=fc_adapter,
clock=clock,
resolved_time_offset_ms=0,
auto_sync_result=None,
aligned_window=None,
)
def _resolve_pace(raw: str) -> ReplayPace:
if raw == "asap":
return ReplayPace.ASAP