azaion/gps-denied-onboard
1
0
Fork 0
mirror of https://github.com/azaion/gps-denied-onboard.git synced 2026-06-22 22: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/cli/replay.py
+43 -6
View File
@@ -98,7 +98,32 @@ def _build_argparser() -> argparse.ArgumentParser:
),
)
parser.add_argument("--video", required=True, type=Path, metavar="PATH")
parser.add_argument("--tlog", required=True, type=Path, metavar="PATH")
parser.add_argument(
"--imu",
dest="imu",
required=True,
type=Path,
metavar="PATH",
help=(
"Paired Derkachi-schema CSV (IMU + GPS ground truth on a "
"single canonical Time clock). Required for new replay runs "
"(AZ-894). Schema spec: "
"_docs/02_document/contracts/replay/csv_replay_format.md."
),
)
parser.add_argument(
"--tlog",
required=False,
default=None,
type=Path,
metavar="PATH",
help=(
"DEPRECATED (AZ-894 → AZ-895): legacy pymavlink .tlog file. "
"Accepted for transitional CLIs but the replay pipeline now "
"drives off --imu; --tlog is ignored when --imu is present. "
"Remove from new invocations."
),
)
parser.add_argument("--output", required=True, type=Path, metavar="PATH")
parser.add_argument(
"--camera-calibration",
@@ -188,13 +213,15 @@ def _build_argparser() -> argparse.ArgumentParser:
def _validate_paths(args: argparse.Namespace) -> None:
"""Fail fast if any required-file argument is missing or unreadable."""
paths: tuple[tuple[str, Path], ...] = (
paths: list[tuple[str, Path]] = [
("video", args.video),
("tlog", args.tlog),
("imu", args.imu),
("camera-calibration", args.camera_calibration),
("config", args.config_path),
("mavlink-signing-key", args.mavlink_signing_key),
)
]
if args.tlog is not None:
paths.append(("tlog", args.tlog))
for label, path in paths:
if not path.exists():
raise ReplayCliError(f"--{label} path does not exist: {path}")
@@ -250,7 +277,8 @@ def _build_replay_config(
"""
new_replay = ReplayConfig(
video_path=str(args.video),
tlog_path=str(args.tlog),
tlog_path=str(args.tlog) if args.tlog is not None else "",
imu_csv_path=str(args.imu),
output_path=str(args.output),
pace=args.pace,
time_offset_ms=args.time_offset_ms,
@@ -301,7 +329,8 @@ def _print_startup_banner(args: argparse.Namespace) -> None:
Logging is bootstrapped inside the airborne main; this banner gives
the operator a single line confirming what the CLI parsed before any
further output.
further output. AZ-894: also surfaces the --tlog deprecation warning
inline so operators see it even when stderr is the only sink.
"""
sanitised = vars(args).copy()
sanitised["mavlink_signing_key"] = "<redacted>"
@@ -310,6 +339,14 @@ def _print_startup_banner(args: argparse.Namespace) -> None:
file=sys.stderr,
flush=True,
)
if args.tlog is not None:
print(
"gps-denied-replay: WARNING --tlog is deprecated (AZ-894 / AZ-895). "
"The replay pipeline drives off --imu; --tlog is accepted but ignored. "
"Remove it from your invocation.",
file=sys.stderr,
flush=True,
)
# ----------------------------------------------------------------------
src/gps_denied_onboard/components/c8_fc_adapter/csv_replay_adapter.py
+312
View File
@@ -0,0 +1,312 @@
"""``CsvReplayFcAdapter`` (AZ-894 / E-DEMO-REPLAY).
Replay-only :class:`FcAdapter` sibling to :class:`TlogReplayFcAdapter`
that backs the CSV-driven replay input (AZ-894). The CSV variant exists
to remove the AZ-848 / AZ-883 two-clock surface from the replay test/demo
path — the canonical replay loop reads IMU + GPS straight from
:func:`replay_input.csv_ground_truth.load_csv_ground_truth`, so this
adapter's inbound :class:`SubscriptionBus` is intentionally never fed.
The adapter exists for two reasons:
1. **Protocol parity (replay_protocol Invariant 1).** The composition
root populates ``components["fc_adapter"]`` and downstream code (e.g.
:func:`_run_replay_loop`) requires a non-``None`` value implementing
the :class:`FcAdapter` Protocol; substituting this thin sibling keeps
the loop's preconditions identical to the tlog path.
2. **Outbound byte equality (Invariant 5).** Encoders write through the
:class:`MavlinkTransport` seam in both modes; this adapter routes
``emit_external_position`` / ``emit_status_text`` through the injected
transport so the AC-9 ``bytes_written`` invariant holds without
touching ``tlog_replay_adapter.py``.
Inbound surface is reduced to a no-op subscription bus by design — the
replay loop reads from the CSV directly and never subscribes (mirroring
the documented bypass that the tlog adapter already relies on; see
``runtime_root._run_replay_loop`` docstring "IMU samples are read
SYNCHRONOUSLY…").
"""
from __future__ import annotations
import os
from pathlib import Path
from typing import TYPE_CHECKING, Any, Final
from gps_denied_onboard._types.fc import (
FcKind,
FlightState,
FlightStateSignal,
PortConfig,
Severity,
Subscription,
TelemetryCallback,
)
from gps_denied_onboard._types.geo import LatLonAlt
from gps_denied_onboard.components.c8_fc_adapter._outbound_mavlink_payloads import (
encode_gps_input,
encode_named_value_float,
encode_statustext,
send_via_transport,
)
from gps_denied_onboard.components.c8_fc_adapter._outbound_provenance import (
source_label_to_float,
)
from gps_denied_onboard.components.c8_fc_adapter._subscription import SubscriptionBus
from gps_denied_onboard.components.c8_fc_adapter.errors import (
FcAdapterConfigError,
FcEmitError,
FcOpenError,
SourceSetSwitchNotSupportedError,
)
from gps_denied_onboard.components.c8_fc_adapter.interface import MavlinkTransport
from gps_denied_onboard.components.c8_fc_adapter.tlog_replay_adapter import ReplayPace
from gps_denied_onboard.logging import get_logger
if TYPE_CHECKING:
from gps_denied_onboard._types.emitted import EmittedExternalPosition
from gps_denied_onboard._types.state import EstimatorOutput
from gps_denied_onboard.clock import Clock
from gps_denied_onboard.fdr_client.client import FdrClient
__all__ = ["CsvReplayFcAdapter"]
_BUILD_FLAG: Final[str] = "BUILD_CSV_REPLAY_ADAPTER"
_LOG_KIND_OPENED: Final[str] = "c8.csv_replay.opened"
def _build_flag_on() -> bool:
"""Return ``True`` when ``BUILD_CSV_REPLAY_ADAPTER`` is a truthy token."""
raw = os.environ.get(_BUILD_FLAG, "")
return raw.strip().lower() in {"on", "1", "true", "yes"}
class CsvReplayFcAdapter:
"""Thin :class:`FcAdapter` backing the CSV-driven replay input.
The constructor signature mirrors :class:`TlogReplayFcAdapter` on the
fields that the composition root threads through, so swapping the
two adapters at construction time is a single-line change inside
:mod:`runtime_root._replay_branch`. Inbound subscription is a no-op
by design (see module docstring).
"""
__slots__ = (
"_csv_path",
"_target_fc_dialect",
"_clock",
"_fdr_client",
"_pace",
"_log",
"_bus",
"_opened",
"_closed",
"_mavlink_transport",
"_outbound_mav",
"_sequence_number",
"_clock_us_provider",
"_clock_ms_boot_provider",
)
def __init__(
self,
*,
csv_path: Path,
target_fc_dialect: FcKind,
clock: "Clock",
fdr_client: "FdrClient",
pace: ReplayPace = ReplayPace.ASAP,
mavlink_transport: "MavlinkTransport | None" = None,
outbound_mav: Any | None = None,
) -> None:
if not _build_flag_on():
raise FcAdapterConfigError(
f"{_BUILD_FLAG} is OFF in this binary; CsvReplayFcAdapter "
"is unavailable. Rebuild with the flag set to ON in the "
"replay binary's Dockerfile."
)
if not isinstance(csv_path, Path):
raise FcAdapterConfigError(
f"csv_path must be a pathlib.Path; got {type(csv_path).__name__}"
)
if target_fc_dialect not in (FcKind.ARDUPILOT_PLANE, FcKind.INAV):
raise FcAdapterConfigError(
f"target_fc_dialect must be ARDUPILOT_PLANE or INAV; "
f"got {target_fc_dialect!r}"
)
if not isinstance(pace, ReplayPace):
raise FcAdapterConfigError(
f"pace must be a ReplayPace enum; got {type(pace).__name__}"
)
self._csv_path = csv_path
self._target_fc_dialect = target_fc_dialect
self._clock = clock
self._fdr_client = fdr_client
self._pace = pace
self._log = get_logger("c8_fc_adapter.csv_replay")
self._bus = SubscriptionBus()
self._opened = False
self._closed = False
self._mavlink_transport: MavlinkTransport | None = mavlink_transport
self._outbound_mav: Any = outbound_mav
self._sequence_number: int = 0
self._clock_us_provider = lambda: int(self._clock.monotonic_ns() // 1000)
self._clock_ms_boot_provider = lambda: int(
self._clock.monotonic_ns() // 1_000_000
) % 0xFFFFFFFF
# ------------------------------------------------------------------
# FcAdapter Protocol implementation
def open(
self,
port: PortConfig | None = None,
signing_key: bytes | None = None,
) -> None:
"""Validate the CSV exists; lazy-build the outbound MAVLink instance.
``port`` and ``signing_key`` are accepted for Protocol parity but
unused (replay has no FC link to open). The actual CSV parsing
happens inside :func:`load_csv_ground_truth` from the runtime
loop; this method only fails fast on a missing file so the
composition root surfaces the same shape of error as the tlog
path.
"""
if self._opened:
raise FcOpenError("CsvReplayFcAdapter already opened")
if not self._csv_path.is_file():
raise FcOpenError(f"CSV file not found: {self._csv_path}")
if self._mavlink_transport is not None and self._outbound_mav is None:
from pymavlink.dialects.v20 import ardupilotmega as _mavlink
self._outbound_mav = _mavlink.MAVLink(
file=None, srcSystem=1, srcComponent=1
)
self._opened = True
self._log.info(
f"{_LOG_KIND_OPENED}: csv_path={self._csv_path} "
f"dialect={self._target_fc_dialect.value} "
f"pace={self._pace.value}",
extra={
"kind": _LOG_KIND_OPENED,
"kv": {
"csv_path": str(self._csv_path),
"target_fc_dialect": self._target_fc_dialect.value,
"pace": self._pace.value,
},
},
)
def close(self) -> None:
"""Release any held outbound resources; idempotent."""
if not self._opened or self._closed:
return
self._closed = True
def subscribe_telemetry(self, callback: TelemetryCallback) -> Subscription:
# Bus is intentionally never fed in the CSV variant — the replay
# loop reads IMU + GPS directly from the parsed CsvGroundTruth.
# We still hand back a real Subscription so Protocol consumers
# (and any future inbound-mirroring code) get a no-op handle
# instead of a contract violation.
return self._bus.subscribe(callback)
def emit_external_position(
self, output: "EstimatorOutput"
) -> "EmittedExternalPosition":
from gps_denied_onboard._types.emitted import EmittedExternalPosition
if self._mavlink_transport is None or self._outbound_mav is None:
raise FcEmitError("replay adapter does not emit to FC")
if output.smoothed:
raise FcEmitError(
"smoothed output cannot be emitted to FC (Invariant 6)"
)
wgs = output.position_wgs84
if not isinstance(wgs, LatLonAlt):
raise FcEmitError(
f"EstimatorOutput.position_wgs84 must be a LatLonAlt; "
f"got {type(wgs).__name__}"
)
emitted_at = self._clock.monotonic_ns()
self._sequence_number += 1
seq = self._sequence_number
try:
gps_msg = encode_gps_input(
self._outbound_mav,
time_usec=int(self._clock_us_provider()),
gps_id=0,
ignore_flags=0,
time_week_ms=0,
time_week=0,
fix_type=3,
lat=int(wgs.lat_deg * 1e7),
lon=int(wgs.lon_deg * 1e7),
alt=float(wgs.alt_m),
hdop=0.0,
vdop=0.0,
vn=0.0,
ve=0.0,
vd=0.0,
speed_accuracy=0.0,
horiz_accuracy=0.0,
vert_accuracy=0.0,
satellites_visible=10,
yaw=0,
)
send_via_transport(self._outbound_mav, gps_msg, self._mavlink_transport)
label_msg = encode_named_value_float(
self._outbound_mav,
time_boot_ms=int(self._clock_ms_boot_provider()),
name=b"src_lbl",
value=source_label_to_float(output.source_label),
)
send_via_transport(
self._outbound_mav, label_msg, self._mavlink_transport
)
except Exception as exc:
raise FcEmitError(
f"replay outbound wire emit failed: {exc!r}"
) from exc
return EmittedExternalPosition(
fc_kind=FcKind.ARDUPILOT_PLANE,
horiz_accuracy_m=0.0,
source_label=output.source_label,
emitted_at=emitted_at,
sequence_number=seq,
)
def emit_status_text(self, msg: str, severity: Severity) -> None:
if self._mavlink_transport is None or self._outbound_mav is None:
raise FcEmitError("replay adapter does not emit to FC")
try:
text = msg.encode("utf-8")[:50]
txt_msg = encode_statustext(
self._outbound_mav,
severity=int(severity.value),
text=text,
)
send_via_transport(self._outbound_mav, txt_msg, self._mavlink_transport)
except Exception as exc:
raise FcEmitError(
f"replay outbound statustext failed: {exc!r}"
) from exc
def request_source_set_switch(self) -> None:
raise SourceSetSwitchNotSupportedError(
"CsvReplayFcAdapter cannot issue MAV_CMD_SET_EKF_SOURCE_SET; "
"replay reads telemetry from a recorded CSV"
)
def current_flight_state(self) -> FlightStateSignal:
# The CSV does not carry MAVLink HEARTBEAT, so we cannot derive a
# latched flight-state. Returning INIT mirrors what the tlog adapter
# returns before its first decoded heartbeat; the replay loop never
# consumes this value (it drives the loop from the CSV directly).
return FlightStateSignal(
state=FlightState.INIT,
last_valid_gps_hint_wgs84=None,
last_valid_gps_age_ms=None,
captured_at=self._clock.monotonic_ns(),
)
src/gps_denied_onboard/config/schema.py
+13 -1
View File
@@ -347,7 +347,18 @@ class ReplayConfig:
``.h264``). Empty string is the default sentinel; a
non-empty value is required when ``mode == "replay"``.
tlog_path: Filesystem path to the matching pymavlink ``.tlog``.
Empty string is the default sentinel.
Empty string is the default sentinel. Deprecated as of AZ-894 —
the canonical replay input is now the CSV pair routed through
:attr:`imu_csv_path`. ``tlog_path`` remains valid only for
legacy callers that have not migrated to the CSV input; AZ-895
removes it entirely.
imu_csv_path: Filesystem path to the paired Derkachi-schema CSV
(IMU + GPS-ground-truth on a single canonical ``Time`` clock).
Empty string is the default sentinel; a non-empty value is
required for new replay runs (AZ-894). Setting this in
combination with ``tlog_path`` is permitted during the
AZ-894 → AZ-895 migration window: ``imu_csv_path`` wins; the
tlog path is then unused.
output_path: Filesystem path the :class:`JsonlReplaySink` will
write to. Default points at ``/tmp/replay.jsonl`` for
developer ergonomics; production wiring overrides via the
@@ -390,6 +401,7 @@ class ReplayConfig:
video_path: str = ""
tlog_path: str = ""
imu_csv_path: str = ""
output_path: str = "/tmp/replay.jsonl"
pace: str = "asap"
time_offset_ms: int | None = None
src/gps_denied_onboard/replay_input/__init__.py
+8
View File
@@ -20,6 +20,11 @@ path.
"""
from gps_denied_onboard._types.route import RouteSpec
from gps_denied_onboard.replay_input.csv_ground_truth import (
CsvGpsFix,
CsvGroundTruth,
load_csv_ground_truth,
)
from gps_denied_onboard.replay_input.errors import ReplayInputAdapterError
from gps_denied_onboard.replay_input.interface import (
AlignedWindow,
@@ -42,6 +47,8 @@ __all__ = [
"AlignedWindow",
"AutoSyncConfig",
"AutoSyncDecision",
"CsvGpsFix",
"CsvGroundTruth",
"ReplayInputAdapter",
"ReplayInputAdapterError",
"ReplayInputBundle",
@@ -50,5 +57,6 @@ __all__ = [
"TlogGpsFix",
"TlogGroundTruth",
"extract_route_from_tlog",
"load_csv_ground_truth",
"load_tlog_ground_truth",
]
src/gps_denied_onboard/replay_input/csv_ground_truth.py
+255
View File
@@ -0,0 +1,255 @@
"""CSV-driven IMU + GPS ground-truth extractor (AZ-894 / E-DEMO-REPLAY).
Streams paired IMU samples and GPS-truth fixes from a Derkachi-schema CSV
into a typed :class:`CsvGroundTruth` DTO. Sibling to :mod:`tlog_ground_truth`
— the CSV variant lets the replay pipeline run on a single canonical clock
(the CSV's ``Time`` column), eliminating the AZ-848 / AZ-883 two-clock
mismatch that cycle-3 surfaced.
Schema (19 cols, header-required):
* ``timestamp(ms)`` — FC-boot-relative ms, kept for traceability only.
* ``Time`` — flight-relative seconds, the canonical clock.
* ``SCALED_IMU2.{x,y,z}{acc,gyro,mag}`` — 10 Hz IMU stream (raw mg / mrad/s
/ mGauss per ArduPilot convention; preserved unchanged to match the
byte-for-byte semantics the tlog adapter uses for ``ImuSample.accel_xyz``
and ``gyro_xyz``).
* ``GLOBAL_POSITION_INT.{lat,lon,alt,relative_alt,vx,vy,vz,hdg}`` — 10 Hz
GPS truth. ``lat``/``lon`` already in degrees (Derkachi dump format),
``alt`` in mm, ``vx``/``vy``/``vz`` in cm/s, ``hdg`` in cdeg.
The full operator-facing schema spec lives in
``_docs/02_document/contracts/replay/csv_replay_format.md`` (AZ-896).
"""
from __future__ import annotations
import csv
import logging
import math
from dataclasses import dataclass
from pathlib import Path
from typing import Final
from gps_denied_onboard._types.nav import ImuSample
from gps_denied_onboard.replay_input.errors import ReplayInputAdapterError
__all__ = [
"CSV_SOURCE_LABEL",
"REQUIRED_COLUMNS",
"CsvGpsFix",
"CsvGroundTruth",
"load_csv_ground_truth",
]
_LOGGER = logging.getLogger("gps_denied_onboard.replay_input.csv_ground_truth")
CSV_SOURCE_LABEL: Final[str] = "GLOBAL_POSITION_INT_CSV"
REQUIRED_COLUMNS: Final[tuple[str, ...]] = (
"timestamp(ms)",
"Time",
"SCALED_IMU2.xacc",
"SCALED_IMU2.yacc",
"SCALED_IMU2.zacc",
"SCALED_IMU2.xgyro",
"SCALED_IMU2.ygyro",
"SCALED_IMU2.zgyro",
"GLOBAL_POSITION_INT.lat",
"GLOBAL_POSITION_INT.lon",
"GLOBAL_POSITION_INT.alt",
"GLOBAL_POSITION_INT.vx",
"GLOBAL_POSITION_INT.vy",
"GLOBAL_POSITION_INT.vz",
"GLOBAL_POSITION_INT.hdg",
)
# Per the Derkachi dump format the lat/lon columns are already in degrees
# (pre-converted from MAVLink's int×1e7). alt stays in mm; vx/vy/vz in
# cm/s; hdg in cdeg.
_MM_PER_M: Final[float] = 1000.0
_CM_PER_M_S: Final[float] = 100.0
_CDEG_PER_DEG: Final[float] = 100.0
_S_TO_NS: Final[float] = 1.0e9
@dataclass(frozen=True, slots=True)
class CsvGpsFix:
"""One time-aligned GPS-truth row extracted from the CSV.
Field semantics match :class:`tlog_ground_truth.TlogGpsFix` so the
replay loop's downstream consumers see an identical shape regardless
of input format.
"""
ts_ns: int
lat_deg: float
lon_deg: float
alt_m: float
hdg_deg: float
vx_m_s: float
vy_m_s: float
vz_m_s: float
@dataclass(frozen=True, slots=True)
class CsvGroundTruth:
"""Paired IMU + GPS series extracted from a Derkachi-schema CSV.
Attributes:
records: Time-ordered GPS fixes.
imu_samples: Time-ordered IMU samples; one per CSV row (10 Hz).
Values preserve the raw mg / mrad/s units the tlog adapter
also passes through so C1 / C5 see identical numeric shapes
regardless of input source.
source: Discriminator label echoed into the cold-start FDR
event (``"GLOBAL_POSITION_INT_CSV"`` for the Derkachi CSV).
"""
records: tuple[CsvGpsFix, ...]
imu_samples: tuple[ImuSample, ...]
source: str
def load_csv_ground_truth(csv_path: Path) -> CsvGroundTruth:
"""Parse the Derkachi-schema CSV into a :class:`CsvGroundTruth`.
Performs all schema validation at entry so a malformed CSV raises a
single :class:`ReplayInputAdapterError` (AC-5 fail-fast) rather than
surfacing the same problem as a downstream NaN deep inside the loop.
Args:
csv_path: Path to the CSV file. Existence is checked at entry.
Returns:
A :class:`CsvGroundTruth` with row-aligned GPS records and IMU
samples. The IMU and GPS sequences share the same length (one
entry per CSV row).
Raises:
ReplayInputAdapterError: When the file is missing, the header
is missing a required column, the ``Time`` column contains
NaN / non-monotonic values, or any required numeric column
contains an unparseable value.
"""
if not csv_path.is_file():
raise ReplayInputAdapterError(f"CSV file not found: {csv_path}")
with csv_path.open("r", encoding="utf-8", newline="") as handle:
reader = csv.DictReader(handle)
fieldnames = reader.fieldnames
if fieldnames is None:
raise ReplayInputAdapterError(
f"CSV {csv_path} is empty (no header row)"
)
missing = [col for col in REQUIRED_COLUMNS if col not in fieldnames]
if missing:
raise ReplayInputAdapterError(
f"CSV {csv_path} missing required columns: {missing}"
)
gps_records: list[CsvGpsFix] = []
imu_samples: list[ImuSample] = []
last_ts_ns: int | None = None
for row_idx, row in enumerate(reader, start=2):
ts_ns = _parse_time_ns(row, csv_path, row_idx)
if last_ts_ns is not None and ts_ns <= last_ts_ns:
raise ReplayInputAdapterError(
f"CSV {csv_path} row {row_idx}: non-monotonic Time "
f"({ts_ns / _S_TO_NS:.6f} s <= previous "
f"{last_ts_ns / _S_TO_NS:.6f} s)"
)
last_ts_ns = ts_ns
imu_samples.append(_parse_imu(row, csv_path, row_idx, ts_ns))
gps_records.append(_parse_gps(row, csv_path, row_idx, ts_ns))
if not gps_records:
raise ReplayInputAdapterError(
f"CSV {csv_path} has a header but no data rows"
)
_LOGGER.info(
"csv_ground_truth.loaded: csv_path=%s rows=%d span_s=%.3f",
csv_path,
len(gps_records),
(gps_records[-1].ts_ns - gps_records[0].ts_ns) / _S_TO_NS,
)
return CsvGroundTruth(
records=tuple(gps_records),
imu_samples=tuple(imu_samples),
source=CSV_SOURCE_LABEL,
)
def _parse_time_ns(row: dict[str, str], path: Path, row_idx: int) -> int:
raw = row.get("Time", "")
try:
seconds = float(raw)
except ValueError as exc:
raise ReplayInputAdapterError(
f"CSV {path} row {row_idx}: Time={raw!r} is not a number"
) from exc
if math.isnan(seconds) or math.isinf(seconds):
raise ReplayInputAdapterError(
f"CSV {path} row {row_idx}: Time={raw!r} is NaN/Inf"
)
return int(seconds * _S_TO_NS)
def _parse_imu(
row: dict[str, str],
path: Path,
row_idx: int,
ts_ns: int,
) -> ImuSample:
accel = (
_parse_float(row, "SCALED_IMU2.xacc", path, row_idx),
_parse_float(row, "SCALED_IMU2.yacc", path, row_idx),
_parse_float(row, "SCALED_IMU2.zacc", path, row_idx),
)
gyro = (
_parse_float(row, "SCALED_IMU2.xgyro", path, row_idx),
_parse_float(row, "SCALED_IMU2.ygyro", path, row_idx),
_parse_float(row, "SCALED_IMU2.zgyro", path, row_idx),
)
return ImuSample(ts_ns=ts_ns, accel_xyz=accel, gyro_xyz=gyro)
def _parse_gps(
row: dict[str, str],
path: Path,
row_idx: int,
ts_ns: int,
) -> CsvGpsFix:
return CsvGpsFix(
ts_ns=ts_ns,
lat_deg=_parse_float(row, "GLOBAL_POSITION_INT.lat", path, row_idx),
lon_deg=_parse_float(row, "GLOBAL_POSITION_INT.lon", path, row_idx),
alt_m=_parse_float(row, "GLOBAL_POSITION_INT.alt", path, row_idx) / _MM_PER_M,
hdg_deg=_parse_float(row, "GLOBAL_POSITION_INT.hdg", path, row_idx) / _CDEG_PER_DEG,
vx_m_s=_parse_float(row, "GLOBAL_POSITION_INT.vx", path, row_idx) / _CM_PER_M_S,
vy_m_s=_parse_float(row, "GLOBAL_POSITION_INT.vy", path, row_idx) / _CM_PER_M_S,
vz_m_s=_parse_float(row, "GLOBAL_POSITION_INT.vz", path, row_idx) / _CM_PER_M_S,
)
def _parse_float(
row: dict[str, str],
column: str,
path: Path,
row_idx: int,
) -> float:
raw = row.get(column, "")
try:
value = float(raw)
except ValueError as exc:
raise ReplayInputAdapterError(
f"CSV {path} row {row_idx}: {column}={raw!r} is not a number"
) from exc
if math.isnan(value) or math.isinf(value):
raise ReplayInputAdapterError(
f"CSV {path} row {row_idx}: {column}={raw!r} is NaN/Inf"
)
return value
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