mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-06-22 18:31:13 +00:00
[AZ-894] [AZ-896] Add CSV-driven replay adapter + format docs
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
@@ -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(),
|
||||
)
|
||||
Reference in New Issue
Block a user