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-600] Batch 80: refactor sitl_replay_builder to strategy pattern

Browse Source 
Replace per-scenario fixture builders with a parameterized strategy
framework so future Derkachi-based scenarios compose existing pieces
instead of duplicating ~200 lines of orchestration per scenario.

New e2e/fixtures/sitl_replay_builder/builder.py:
- VideoSource ABC + StillImagesSource, Mp4PassthroughSource
- TlogSource ABC + SyntheticStationaryTlog, ImuCsvTlog
- FdrProjection ABC + RawFdrPassthrough, OutboundMessagesProjection
- FixtureBuilderConfig + build_fixtures(cfg) orchestrator
- Consolidated MAVLink pack_raw_imu / pack_attitude helpers
- Consolidated run_gps_denied_replay + write_observer_fixture

build_p01_fixtures.py: 423 -> 107 lines (75% reduction).
build_p02_fixtures.py: 292 -> 98 lines (66% reduction).
_common.py: deleted (folded into builder.py).

Tests reorganized:
- test_sitl_replay_builder_builder.py (new, 33 strategy-level tests)
- test_sitl_replay_builder.py (slimmed, 6 FT-P-01 integration)
- test_sitl_replay_builder_p02.py (slimmed, 7 FT-P-02 integration)

README documents the strategy framework + a worked example for
adding FT-P-04 in ~30 lines (no new strategy code required).

Regression gate: 700 passing (was 686; +14 from finer-grained
coverage of new strategy classes and the build_fixtures orchestrator).

Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
Oleksandr Bezdieniezhnykh
2026-05-17 14:19:08 +03:00
parent 4e0717e543
commit 7fb3cb3f34
13 changed files with 2050 additions and 1272 deletions
Download Patch File Download Diff File Expand all files Collapse all files
e2e/fixtures/sitl_replay_builder/README.md
+104 -94
View File
@@ -1,53 +1,93 @@
# SITL Replay Fixture Builder (AZ-598, AZ-599)
# SITL Replay Fixture Builder (AZ-598, AZ-599, AZ-600)
Per-scenario fixture builders for the offline FDR-replay path used
by the b75 `sitl_observer` module + FT-* blackbox scenarios. Each
builder takes recorded flight inputs (still images / video / IMU
CSV / tlog) and produces the artifacts a specific scenario needs.
Parameterized fixture-builder framework for the offline FDR-replay path
used by the b75 `sitl_observer` module + FT-* blackbox scenarios. A new
scenario typically only writes a ~60-line config factory + CLI on top of
the framework — no new strategy code required.
| Scenario | Builder | Inputs | Outputs |
|----------|---------|--------|---------|
| FT-P-01 (still-image accuracy) | `build_p01_fixtures.py` | 60 `AD0000NN.jpg` + coordinates CSV | `outbound_messages_<fc>_<host>.json` + `observer_<fc>_<host>.json` + `stills.mp4` + `stationary.tlog` + `fdr.jsonl` |
| FT-P-01 (still-image accuracy) | `build_p01_fixtures.py` | 60 `AD0000NN.jpg` | `outbound_messages_<fc>_<host>.json` + `observer_<fc>_<host>.json` + `stills.mp4` + `stationary.tlog` + `fdr.jsonl` |
| FT-P-02 (Derkachi drift) | `build_p02_fixtures.py` | `flight_derkachi.mp4` + `data_imu.csv` | `derkachi.tlog` + `fdr/fdr.jsonl` (FDR archive) + `observer_<fc>_<host>.json` |
Other scenarios (FT-P-03 / 04 / 05 / 07 / 08 / 10 / 11, FT-N-01 / 02 / 03 / 04)
need their own capture flows and will land as follow-up tickets.
Other scenarios (FT-P-03 / 04 / 05 / 07 / 08 / 10 / 11, FT-N-01..04) will
land as follow-ups; each will reuse the strategies below.
## Shared helpers (`_common.py`)
## Framework (`builder.py`)
Both builders shell out to the production `gps-denied-replay` CLI and
write the same minimal `observer_<fc_kind>_<host>.json`. These two
operations live in `_common.py`:
Three strategy ABCs decompose the per-scenario variance:
* `run_gps_denied_replay(video, tlog, fdr_out, *, time_offset_ms=0, ...)`
* `write_observer_fixture(output_path)`
| Strategy | Concrete impls | Used by |
|----------|----------------|---------|
| `VideoSource` — materialize the MP4 the replay CLI consumes | `StillImagesSource(image_paths, fps)`, `Mp4PassthroughSource(mp4_path)` | b78 / b79 |
| `TlogSource` — materialize the tlog the replay CLI consumes | `SyntheticStationaryTlog(duration_s, hz)`, `ImuCsvTlog(csv_path, schema=DEFAULT_DERKACHI_IMU_SCHEMA)` | b78 / b79 |
| `FdrProjection` — translate the FDR JSONL into scenario fixture shape | `RawFdrPassthrough(verify_estimates=True)`, `OutboundMessagesProjection(image_ids, fdr_kind="outbound_position_estimate")` | b79 / b78 |
Future per-scenario builders should import from `_common.py` rather
than re-implementing.
The `build_fixtures(cfg: FixtureBuilderConfig)` orchestrator composes the
three strategies plus the shared `run_gps_denied_replay` subprocess driver
and `write_observer_fixture` helper.
## FT-P-01 (`build_p01_fixtures.py`)
Shared helpers (in `builder.py`):
### Strategy
* `run_gps_denied_replay(video, tlog, fdr_out, *, time_offset_ms=0, ...)` — shells out to the production CLI.
* `write_observer_fixture(output_path)` — writes the minimal `observer_*.json` `sitl_observer.get_observer` requires.
* `pack_raw_imu(time_usec, *, xacc=0, yacc=0, zacc=0, xgyro=0, ygyro=0, zgyro=0)` — parameterized RAW_IMU packer. Stationary callers pass `zacc=STATIONARY_Z_ACCEL_MG` (gravity).
* `pack_attitude(time_boot_ms, *, roll=0.0, pitch=0.0, yaw=0.0)` — parameterized ATTITUDE packer.
* `parse_fdr_for_outbound_estimates(fdr_path, *, fdr_kind, lat_key, lon_key)` — read FDR JSONL into per-image dicts.
* `verify_fdr_has_estimates(fdr_path)` — assert ≥1 `record_type=="estimate"` record.
* `hdg_centideg_to_rad(hdg_cdeg)` — utility for ATTITUDE yaw synthesis.
Rather than spinning up a SITL container, this builder reuses the
production `gps-denied-replay` CLI + `ReplayInputAdapter`:
## Adding a new scenario (worked example: FT-P-04)
1. Encode the 60 `AD0000NN.jpg` still images into a 1 fps MP4.
2. Generate a synthetic stationary tlog (zero-motion `RAW_IMU` +
`ATTITUDE` pairs at 200 Hz) — bypasses the AZ-405 take-off
pre-validator without needing real flight data.
3. Run `gps-denied-replay --video stills.mp4 --tlog stationary.tlog
--time-offset-ms 0 --fdr-out fdr.jsonl` (auto-sync bypassed
because the synthetic tlog has no take-off signal).
4. Read `fdr.jsonl`, filter to `kind == outbound_position_estimate`,
project each into the `outbound_messages_*` schema.
5. Write the two fixture JSON files into `--output-dir`.
FT-P-04 (Derkachi frame-to-frame registration) reuses the same Derkachi MP4
+ IMU CSV as FT-P-02 but consumes the FDR archive differently. With the
framework in place, the new builder is purely a config factory:
This avoids needing new SUT-side frame-ingestion code (HTTP endpoint,
file-watch source, etc.) which would otherwise be required to push
individual stills to a running SUT container.
```python
# e2e/fixtures/sitl_replay_builder/build_p04_fixtures.py (sketch)
from dataclasses import dataclass
from pathlib import Path
from e2e.fixtures.sitl_replay_builder.builder import (
DEFAULT_CLI_BIN,
FixtureBuilderConfig,
ImuCsvTlog,
Mp4PassthroughSource,
RawFdrPassthrough,
build_fixtures,
)
### Usage
@dataclass(frozen=True)
class P04BuilderConfig:
derkachi_dir: Path
output_dir: Path
fc_kind: str = "ardupilot"
host: str = "sitl-host"
def build_p04_fixtures(cfg, **deps):
mp4 = cfg.derkachi_dir / "flight_derkachi.mp4"
csv_path = cfg.derkachi_dir / "data_imu.csv"
builder_cfg = FixtureBuilderConfig(
video_source=Mp4PassthroughSource(mp4_path=mp4),
tlog_source=ImuCsvTlog(csv_path=csv_path),
fdr_projection=RawFdrPassthrough(verify_estimates=True),
output_dir=cfg.output_dir,
fc_kind=cfg.fc_kind, host=cfg.host,
tlog_filename="derkachi.tlog", fdr_subdir="fdr",
)
return build_fixtures(builder_cfg, **deps)
```
Total new code: ~30 lines + argparse CLI. No new strategy class is needed
because every Derkachi-based scenario consumes the same `Mp4PassthroughSource +
ImuCsvTlog + RawFdrPassthrough` triple. A scenario that emits a *new* fixture
shape (e.g. FT-P-13's "anchor-search-region" record extraction) writes a new
`FdrProjection` subclass alongside.
## Per-scenario usage
### FT-P-01
```bash
python -m e2e.fixtures.sitl_replay_builder.build_p01_fixtures \
@@ -57,51 +97,14 @@ python -m e2e.fixtures.sitl_replay_builder.build_p01_fixtures \
--host sitl-host
```
The output directory will contain:
* `stills.mp4` — the 60 images encoded at 1 fps.
* `stationary.tlog` — synthetic 120-s zero-motion tlog at 200 Hz.
* `fdr.jsonl` — the FDR JSONL stream from the replay run.
* `outbound_messages_ardupilot_sitl-host.json` — the consumed fixture.
* `observer_ardupilot_sitl-host.json` — the consumed fixture.
To activate the fixtures in a scenario run:
Activation:
```bash
E2E_SITL_REPLAY_DIR=e2e/fixtures/sitl_replay/p01 \
pytest e2e/tests/positive/test_ft_p_01_still_image_accuracy.py
```
### Limitations
* The synthetic tlog encodes zero motion — auto-sync MUST be bypassed
via `--time-offset-ms 0` (the builder does this automatically).
* The FDR record `kind` is assumed to be `outbound_position_estimate`
— the `--fdr-kind` CLI flag overrides if the actual schema differs.
* Per-image timeout handling: if the SUT emits fewer outbound estimates
than pushed frames, trailing image_ids are written as `null` entries
(encoded as TimeoutError on scenario replay).
* iNav adapter is NOT supported by this batch — only ArduPilot. iNav
will land as a follow-up once the AP path is validated end-to-end.
## FT-P-02 (`build_p02_fixtures.py`)
### Strategy
Same overall shape as FT-P-01 (drive `gps-denied-replay` against a
video + tlog pair), with two differences:
1. Video is already MP4 — skip the OpenCV still-image encoding step.
2. IMU is recorded telemetry (`data_imu.csv`, 10 Hz `SCALED_IMU2` +
`GLOBAL_POSITION_INT`). A CSV → tlog conversion packs each row as
a `RAW_IMU` + `ATTITUDE` MAVLink pair, with yaw synthesised from
`GLOBAL_POSITION_INT.hdg` (centidegrees → radians) and roll/pitch
= 0 (acceptable for the fixed-wing cruise data this represents).
Output is the SUT's natural FDR archive directory; the FT-P-02
scenario reads it via `runner.helpers.fdr_reader.iter_records`.
### Usage
### FT-P-02
```bash
python -m e2e.fixtures.sitl_replay_builder.build_p02_fixtures \
@@ -111,28 +114,35 @@ python -m e2e.fixtures.sitl_replay_builder.build_p02_fixtures \
--host sitl-host
```
Output:
## Limitations
* `derkachi.tlog` — generated from `data_imu.csv`.
* `fdr/fdr.jsonl` — the FDR archive from the replay run.
* `observer_ardupilot_sitl-host.json` — minimal observer config.
### Limitations
* The synthesised ATTITUDE has roll/pitch = 0 — acceptable for
fixed-wing cruise but unrealistic for aggressive manoeuvres.
* RAW_IMU is packed from `SCALED_IMU2` columns as pass-through (no
true scaled → raw unit conversion). If the SUT's tlog parser
strictly demands true raw counts the builder will need a units
conversion pass — surfaced as a follow-up after live-run.
* Auto-sync is bypassed via `--time-offset-ms 0` because the
Derkachi CSV is already aligned with the video.
* The synthesised ATTITUDE has roll/pitch = 0 — acceptable for fixed-wing
cruise but unrealistic for aggressive manoeuvres. Override the packer call
inside a custom `TlogSource` when needed.
* RAW_IMU is packed from `SCALED_IMU2` columns as pass-through (no true
scaled → raw unit conversion). If the SUT's tlog parser strictly demands
true raw counts the builder will need a units conversion pass — surfaced
as a follow-up after the first live run.
* Auto-sync (`time_offset_ms != 0`) is bypassed by every scenario currently;
operators running this against truly independent tlog+video pairs should
override `FixtureBuilderConfig.time_offset_ms`.
* iNav adapter is NOT supported by the existing builders — ArduPilot only.
* The FDR record `kind`/`record_type` schemas are assumed to match the
production contract; overrides live on each projection class.
## Testing
Unit tests under `e2e/_unit_tests/fixtures/test_sitl_replay_builder*.py`
mock all external dependencies (OpenCV, pymavlink, subprocess) so the
test suite runs without a real `gps-denied-replay` install. The actual
end-to-end run requires the SUT to be installed (`pip install -e .` at
repo root) and is documented as a manual step until CI infrastructure
catches up.
Unit tests under `e2e/_unit_tests/fixtures/`:
* `test_sitl_replay_builder_builder.py` — strategy-level tests
(`VideoSource`, `TlogSource`, `FdrProjection` impls + shared helpers +
`build_fixtures` orchestrator).
* `test_sitl_replay_builder.py` — FT-P-01 scenario integration.
* `test_sitl_replay_builder_p02.py` — FT-P-02 scenario integration.
All external dependencies (OpenCV, pymavlink, subprocess) are mocked via
the underscore-prefixed `_runner` / `_video_writer_factory` / `_imread` /
`_mavlink_writer_factory` injection points so the suite runs without a
real `gps-denied-replay` install. The actual end-to-end run requires the
SUT to be installed (`pip install -e .` at repo root) and is documented as
a manual step until CI infrastructure catches up.
e2e/fixtures/sitl_replay_builder/_common.py
-85
View File
@@ -1,85 +0,0 @@
"""Shared helpers for the per-scenario fixture builders (AZ-599).
Both `build_p01_fixtures.py` (still-image FT-P-01) and
`build_p02_fixtures.py` (Derkachi FT-P-02) shell out to the production
`gps-denied-replay` CLI and write the same minimal `observer_*.json`
config; the helpers below live here so there's one canonical
implementation.
Future per-scenario builders (FT-P-04 / FT-P-05 / FT-P-10 / …) should
also import from this module.
"""
from __future__ import annotations
import json
import logging
import subprocess
from pathlib import Path
from typing import Callable, Sequence
_LOG = logging.getLogger(__name__)
DEFAULT_CLI_BIN = "gps-denied-replay"
def run_gps_denied_replay(
video: Path,
tlog: Path,
fdr_out: Path,
*,
cli_bin: str = DEFAULT_CLI_BIN,
time_offset_ms: int = 0,
extra_args: Sequence[str] = (),
_runner: Callable[[Sequence[str]], subprocess.CompletedProcess] | None = None,
) -> subprocess.CompletedProcess:
"""Run ``gps-denied-replay`` as a subprocess.
The `time_offset_ms` defaults to 0 because both b78 (synthetic
stationary tlog) and b79 (Derkachi real-motion tlog) intentionally
bypass auto-sync — b78 because there's no take-off signal, b79
because the IMU CSV is already aligned with the video. Operators
running this against truly independent tlog+video pairs SHOULD
omit ``time_offset_ms`` and let the production auto-sync run.
Raises ``subprocess.CalledProcessError`` on non-zero exit code.
The default subprocess runner can be swapped via ``_runner`` for
unit tests.
"""
fdr_out.parent.mkdir(parents=True, exist_ok=True)
cmd: list[str] = [
cli_bin,
"--video", str(video),
"--tlog", str(tlog),
"--time-offset-ms", str(time_offset_ms),
"--fdr-out", str(fdr_out),
*extra_args,
]
_LOG.info("running: %s", " ".join(cmd))
runner = _runner or (lambda c: subprocess.run(c, check=True, capture_output=True, text=True))
return runner(cmd)
def write_observer_fixture(output_path: Path) -> None:
"""Write minimal `observer_<fc_kind>_<host>.json` so `get_observer` succeeds.
Scenarios that only consume `wait_for_outbound` or `iter_records`
still trigger `sitl_observer.get_observer(...)` for construction.
Populate with safe defaults; scenarios that care about
`read_gps_state` carry their own observer fixtures.
"""
payload = {
"gps_state": {
"primary_source": "MAV",
"last_position_lat_deg": 0.0,
"last_position_lon_deg": 0.0,
"last_position_alt_m": 0.0,
"fix_quality": 3,
"horizontal_accuracy_m": 1.0,
"last_update_age_ms": 0,
},
"parameters": {},
}
output_path.parent.mkdir(parents=True, exist_ok=True)
output_path.write_text(json.dumps(payload, indent=2))
e2e/fixtures/sitl_replay_builder/build_p01_fixtures.py
+40 -348
View File
@@ -1,56 +1,44 @@
"""FT-P-01 fixture builder (AZ-598).
"""FT-P-01 fixture builder (AZ-598; refactored to strategy pattern in AZ-600).
Produces:
Composes the parameterized fixture-builder framework
(``e2e.fixtures.sitl_replay_builder.builder``) into the FT-P-01 scenario:
* ``outbound_messages_<fc_kind>_<host>.json`` — per-image SUT outbound GPS
estimates, in image-order. ``null`` entries encode per-image timeouts.
* ``observer_<fc_kind>_<host>.json`` — minimal observer config so
``sitl_observer.get_observer`` succeeds when the fixtures are activated.
* Video source: 60 ``AD000NN.jpg`` still images encoded at ``fps``.
* Tlog source: synthetic stationary RAW_IMU + ATTITUDE pairs.
* FDR projection: parse ``outbound_position_estimate`` records and write
``outbound_messages_<fc_kind>_<host>.json`` (the FT-P-01 fixture shape).
Strategy: drive the production ``gps-denied-replay`` CLI against a 1 fps
MP4 encoded from the FT-P-01 still-image set and a synthetic stationary
tlog, then read the resulting FDR JSONL for per-frame outbound estimates.
Compared with the rejected "live SITL docker capture" path this:
* Adds no new SUT-side frame-ingestion code (reuses
``ReplayInputAdapter`` + ``VideoFileFrameSource``).
* Bypasses the SITL container entirely (FT-P-01 tests upstream
geo-estimate accuracy; the FC is just a delivery channel).
* Runs as a single subprocess instead of a multi-container compose.
The helpers below are intentionally dependency-injectable so the unit
tests can mock OpenCV / pymavlink / subprocess / filesystem without
touching real hardware or libraries.
This module is intentionally thin — strategy implementations + the
orchestrator live in ``builder.py``. Adding a new scenario typically only
requires writing a similar ~60-line config factory + CLI module.
"""
from __future__ import annotations
import argparse
import json
import logging
import subprocess
import sys
from dataclasses import dataclass
from pathlib import Path
from typing import Callable, Iterable, Sequence
from typing import Callable, Sequence
from e2e.fixtures.sitl_replay_builder._common import (
from e2e.fixtures.sitl_replay_builder.builder import (
DEFAULT_CLI_BIN,
run_gps_denied_replay,
write_observer_fixture,
DEFAULT_FPS,
DEFAULT_TLOG_DURATION_S,
DEFAULT_TLOG_HZ,
FixtureBuilderConfig,
OutboundMessagesProjection,
StillImagesSource,
SyntheticStationaryTlog,
build_fixtures,
)
_LOG = logging.getLogger(__name__)
DEFAULT_FPS = 1.0
DEFAULT_TLOG_DURATION_S = 120
DEFAULT_TLOG_HZ = 200
DEFAULT_FDR_KIND = "outbound_position_estimate"
@dataclass(frozen=True)
class BuilderConfig:
"""Per-invocation builder configuration."""
"""Per-invocation FT-P-01 builder configuration."""
input_dir: Path
output_dir: Path
@@ -59,261 +47,11 @@ class BuilderConfig:
fps: float = DEFAULT_FPS
tlog_duration_s: int = DEFAULT_TLOG_DURATION_S
tlog_hz: int = DEFAULT_TLOG_HZ
fdr_kind: str = DEFAULT_FDR_KIND
cli_bin: str = DEFAULT_CLI_BIN
# Step 1 — encode the still images into a 1 fps MP4
def encode_stills_to_mp4(
image_paths: Sequence[Path],
output_mp4: Path,
*,
fps: float = DEFAULT_FPS,
_video_writer_factory: Callable | None = None,
_imread: Callable | None = None,
) -> int:
"""Encode `image_paths` (in order) as an MP4 at `fps`. Returns frame count.
Raises ``FileNotFoundError`` when no image paths are supplied or when
any input image cannot be read.
The OpenCV dependencies are injected via the underscore-prefixed
parameters so unit tests can run without OpenCV being available.
"""
if not image_paths:
raise FileNotFoundError(
"encode_stills_to_mp4: image_paths is empty; nothing to encode"
)
if _video_writer_factory is None or _imread is None:
import cv2
_imread = _imread or (lambda path: cv2.imread(str(path), cv2.IMREAD_COLOR))
if _video_writer_factory is None:
_fourcc = cv2.VideoWriter_fourcc(*"mp4v")
def _video_writer_factory(out: Path, width: int, height: int):
return cv2.VideoWriter(str(out), _fourcc, fps, (width, height))
first_frame = _imread(image_paths[0])
if first_frame is None:
raise FileNotFoundError(
f"encode_stills_to_mp4: failed to read {image_paths[0]}"
)
height, width = first_frame.shape[:2]
output_mp4.parent.mkdir(parents=True, exist_ok=True)
writer = _video_writer_factory(output_mp4, width, height)
try:
writer.write(first_frame)
for path in image_paths[1:]:
frame = _imread(path)
if frame is None:
raise FileNotFoundError(
f"encode_stills_to_mp4: failed to read {path}"
)
writer.write(frame)
finally:
writer.release()
return len(image_paths)
# Step 2 — generate a synthetic stationary tlog
def generate_stationary_tlog(
output_tlog: Path,
*,
duration_s: int = DEFAULT_TLOG_DURATION_S,
hz: int = DEFAULT_TLOG_HZ,
_mavlink_writer_factory: Callable | None = None,
) -> int:
"""Write a tlog with `duration_s * hz` stationary RAW_IMU + ATTITUDE pairs.
The output is the minimum tlog content ``ReplayInputAdapter`` requires:
monotonic-timestamp RAW_IMU + ATTITUDE messages so the AZ-405 tlog
pre-validator (`AC-13`) doesn't reject the input.
The samples encode zero accel/gyro/attitude — auto-sync will refuse to
find a take-off, so callers MUST drive ``gps-denied-replay`` with an
explicit ``--time-offset-ms 0`` to bypass auto-sync.
Returns the number of message PAIRS written.
"""
if duration_s <= 0:
raise ValueError(f"duration_s must be positive; got {duration_s}")
if hz <= 0:
raise ValueError(f"hz must be positive; got {hz}")
if _mavlink_writer_factory is None:
from pymavlink import mavutil
def _mavlink_writer_factory(out: Path):
return mavutil.mavlogfile(str(out), write=True)
output_tlog.parent.mkdir(parents=True, exist_ok=True)
pairs = 0
writer = _mavlink_writer_factory(output_tlog)
try:
period_us = int(1_000_000 / hz)
total_pairs = duration_s * hz
for i in range(total_pairs):
time_us = i * period_us
writer.write(_pack_raw_imu_zero(time_us))
writer.write(_pack_attitude_zero(time_us // 1000))
pairs += 1
finally:
close = getattr(writer, "close", None)
if callable(close):
close()
return pairs
def _pack_raw_imu_zero(time_usec: int) -> bytes:
"""Pack a zero-motion RAW_IMU MAVLink frame (msg id 27).
Constructed with pymavlink's MAVLink2 packer so the produced bytes are
a wire-compatible MAVLink frame including header + CRC. Stationary
semantics: all accel/gyro/mag fields are zero except the Z accel which
carries one g (gravity, ~9.81 m/s² × 1000 in mg).
"""
from pymavlink.dialects.v20 import ardupilotmega as mavlink
packer = mavlink.MAVLink(file=None, srcSystem=1, srcComponent=1)
msg = mavlink.MAVLink_raw_imu_message(
time_usec=time_usec,
xacc=0,
yacc=0,
zacc=-9810,
xgyro=0,
ygyro=0,
zgyro=0,
xmag=0,
ymag=0,
zmag=0,
id=0,
temperature=0,
)
return msg.pack(packer)
def _pack_attitude_zero(time_boot_ms: int) -> bytes:
"""Pack a zero-motion ATTITUDE MAVLink frame (msg id 30)."""
from pymavlink.dialects.v20 import ardupilotmega as mavlink
packer = mavlink.MAVLink(file=None, srcSystem=1, srcComponent=1)
msg = mavlink.MAVLink_attitude_message(
time_boot_ms=time_boot_ms,
roll=0.0,
pitch=0.0,
yaw=0.0,
rollspeed=0.0,
pitchspeed=0.0,
yawspeed=0.0,
)
return msg.pack(packer)
# Step 3 — drive `gps-denied-replay` against the generated video+tlog
# (`run_gps_denied_replay` is re-exported from `_common.py` so b78 + b79 share one impl.)
# Step 4 — extract per-frame outbound estimates from the FDR JSONL
def parse_fdr_for_outbound_estimates(
fdr_path: Path,
*,
fdr_kind: str = DEFAULT_FDR_KIND,
lat_key: str = "lat_deg",
lon_key: str = "lon_deg",
) -> list[dict]:
"""Walk `fdr_path` (JSONL) and return outbound-estimate payloads in order.
A record contributes one entry when its ``kind`` matches `fdr_kind` AND
its payload carries both `lat_key` and `lon_key`. Other records are
silently skipped (the FDR carries many record types per the
`_docs/02_document/contracts/fdr/` schema). Malformed JSON lines raise
``ValueError`` with the line number.
"""
if not fdr_path.is_file():
raise FileNotFoundError(f"FDR JSONL not found: {fdr_path}")
out: list[dict] = []
with fdr_path.open("r", encoding="utf-8") as fp:
for line_no, line in enumerate(fp, start=1):
line = line.strip()
if not line:
continue
try:
record = json.loads(line)
except json.JSONDecodeError as exc:
raise ValueError(
f"malformed FDR JSON at {fdr_path}:{line_no}: {exc.msg}"
) from exc
if record.get("kind") != fdr_kind:
continue
payload = record.get("payload", {})
if not isinstance(payload, dict):
continue
if lat_key not in payload or lon_key not in payload:
continue
out.append(
{
"lat_deg": float(payload[lat_key]),
"lon_deg": float(payload[lon_key]),
}
)
return out
# Step 5 — write the two fixture files in the b75/b78 schema
def write_outbound_messages_fixture(
output_path: Path,
image_ids: Sequence[str],
estimates: Sequence[dict | None],
) -> None:
"""Write `outbound_messages_<fc_kind>_<host>.json`.
`image_ids` and `estimates` must have the same length. `None` entries
in `estimates` are persisted as JSON `null` (timeout markers); other
entries must carry `lat_deg`/`lon_deg`.
"""
if len(image_ids) != len(estimates):
raise ValueError(
f"length mismatch: {len(image_ids)} image_ids vs "
f"{len(estimates)} estimates"
)
messages: list[dict | None] = []
for image_id, estimate in zip(image_ids, estimates):
if estimate is None:
messages.append(None)
continue
messages.append(
{
"image_id": image_id,
"lat_deg": float(estimate["lat_deg"]),
"lon_deg": float(estimate["lon_deg"]),
}
)
output_path.parent.mkdir(parents=True, exist_ok=True)
output_path.write_text(json.dumps({"messages": messages}, indent=2))
# `write_observer_fixture` is re-exported from `_common.py` (used by both b78 + b79).
# Orchestration
def _resolve_p01_image_paths(input_dir: Path) -> list[Path]:
"""Return the AD0000NN.jpg images under `input_dir`, sorted by name."""
def resolve_p01_image_paths(input_dir: Path) -> list[Path]:
"""Return the ``AD000NN.jpg`` images under ``input_dir``, sorted by name."""
if not input_dir.is_dir():
raise FileNotFoundError(f"input dir not found: {input_dir}")
return sorted(input_dir.glob("AD??????.jpg"))
@@ -327,67 +65,25 @@ def build_p01_fixtures(
_imread: Callable | None = None,
_mavlink_writer_factory: Callable | None = None,
) -> Path:
"""End-to-end FT-P-01 fixture build. Returns the output directory.
Steps (matches the module docstring):
1. Resolve the 60 AD0000NN.jpg images under ``cfg.input_dir``.
2. Encode them at ``cfg.fps`` into ``stills.mp4`` under ``cfg.output_dir``.
3. Generate a stationary ``stationary.tlog`` under ``cfg.output_dir``.
4. Run ``gps-denied-replay`` against the pair; write FDR JSONL.
5. Project FDR outbound-estimate records into the two fixture files.
Per-frame timeout handling: if the FDR yields fewer estimates than
images, the trailing image_ids get `null` (timeout) entries. If the
FDR yields MORE estimates than images (multiple emissions per frame),
only the first ``len(image_paths)`` estimates are kept and a WARN is
logged so the operator notices the schema mismatch.
"""
image_paths = _resolve_p01_image_paths(cfg.input_dir)
"""End-to-end FT-P-01 fixture build. Returns the output directory."""
image_paths = resolve_p01_image_paths(cfg.input_dir)
if not image_paths:
raise FileNotFoundError(
f"no AD??????.jpg images found under {cfg.input_dir}"
)
raise FileNotFoundError(f"no AD??????.jpg images found under {cfg.input_dir}")
cfg.output_dir.mkdir(parents=True, exist_ok=True)
stills_mp4 = cfg.output_dir / "stills.mp4"
stationary_tlog = cfg.output_dir / "stationary.tlog"
fdr_jsonl = cfg.output_dir / "fdr.jsonl"
encode_stills_to_mp4(
image_paths, stills_mp4, fps=cfg.fps,
_video_writer_factory=_video_writer_factory, _imread=_imread,
builder_cfg = FixtureBuilderConfig(
video_source=StillImagesSource(image_paths=image_paths, fps=cfg.fps),
tlog_source=SyntheticStationaryTlog(duration_s=cfg.tlog_duration_s, hz=cfg.tlog_hz),
fdr_projection=OutboundMessagesProjection(image_ids=[p.name for p in image_paths]),
output_dir=cfg.output_dir,
fc_kind=cfg.fc_kind, host=cfg.host, cli_bin=cfg.cli_bin,
video_filename="stills.mp4", tlog_filename="stationary.tlog",
fdr_subdir=".", fdr_filename="fdr.jsonl",
)
generate_stationary_tlog(
stationary_tlog,
duration_s=cfg.tlog_duration_s,
hz=cfg.tlog_hz,
_mavlink_writer_factory=_mavlink_writer_factory,
return build_fixtures(
builder_cfg,
_runner=_runner, _video_writer_factory=_video_writer_factory,
_imread=_imread, _mavlink_writer_factory=_mavlink_writer_factory,
)
run_gps_denied_replay(
stills_mp4, stationary_tlog, fdr_jsonl,
cli_bin=cfg.cli_bin, _runner=_runner,
)
raw_estimates = parse_fdr_for_outbound_estimates(fdr_jsonl, fdr_kind=cfg.fdr_kind)
estimates: list[dict | None] = list(raw_estimates[: len(image_paths)])
if len(raw_estimates) > len(image_paths):
_LOG.warning(
"FDR carried %d outbound estimates but only %d images were pushed; "
"truncating to the per-frame count", len(raw_estimates), len(image_paths)
)
while len(estimates) < len(image_paths):
estimates.append(None)
outbound_path = cfg.output_dir / f"outbound_messages_{cfg.fc_kind}_{cfg.host}.json"
observer_path = cfg.output_dir / f"observer_{cfg.fc_kind}_{cfg.host}.json"
write_outbound_messages_fixture(
outbound_path,
image_ids=[p.name for p in image_paths],
estimates=estimates,
)
write_observer_fixture(observer_path)
return cfg.output_dir
def _main(argv: Sequence[str] | None = None) -> int:
@@ -407,12 +103,8 @@ def _main(argv: Sequence[str] | None = None) -> int:
logging.basicConfig(level=logging.INFO)
cfg = BuilderConfig(
input_dir=args.input_dir,
output_dir=args.output_dir,
fc_kind=args.fc_kind,
host=args.host,
fps=args.fps,
cli_bin=args.cli_bin,
input_dir=args.input_dir, output_dir=args.output_dir,
fc_kind=args.fc_kind, host=args.host, fps=args.fps, cli_bin=args.cli_bin,
)
build_p01_fixtures(cfg)
return 0
e2e/fixtures/sitl_replay_builder/build_p02_fixtures.py
+41 -226
View File
@@ -1,53 +1,36 @@
"""FT-P-02 Derkachi fixture builder (AZ-599).
"""FT-P-02 Derkachi fixture builder (AZ-599; refactored to strategy pattern in AZ-600).
Drives the production ``gps-denied-replay`` CLI against the recorded
Derkachi MP4 + a tlog converted from ``data_imu.csv``, producing an
FDR archive consumable by the FT-P-02 scenario (it walks the FDR via
``fdr_reader.iter_records`` and computes drift between satellite
anchors).
Composes the parameterized fixture-builder framework
(``e2e.fixtures.sitl_replay_builder.builder``) into the FT-P-02 scenario:
Differences from the b78 FT-P-01 builder (`build_p01_fixtures.py`):
* Video source: pass-through of the recorded ``flight_derkachi.mp4``.
* Tlog source: real-motion tlog converted from ``data_imu.csv`` rows
(10 Hz ``SCALED_IMU2`` accel/gyro + ``GLOBAL_POSITION_INT.hdg`` yaw;
roll/pitch=0 fixed-wing-cruise approximation).
* FDR projection: raw passthrough + assert ≥1 ``record_type=="estimate"``
record (the FT-P-02 scenario walks the FDR via ``fdr_reader.iter_records``).
* Video is already MP4 — no encoding step.
* IMU is real recorded telemetry — needs CSV → tlog conversion with
real motion data (vs. b78's synthetic stationary tlog).
* Output is the SUT's natural FDR archive directory — no per-call
schema projection.
Shared helpers (`run_gps_denied_replay`, `write_observer_fixture`)
live in `_common.py`.
This module is intentionally thin — strategy implementations + the
orchestrator live in ``builder.py``.
"""
from __future__ import annotations
import argparse
import csv
import json
import logging
import math
import subprocess
import sys
from dataclasses import dataclass
from pathlib import Path
from typing import Callable, Iterator, Sequence
from typing import Callable, Sequence
from e2e.fixtures.sitl_replay_builder._common import (
from e2e.fixtures.sitl_replay_builder.builder import (
DEFAULT_CLI_BIN,
run_gps_denied_replay,
write_observer_fixture,
)
_LOG = logging.getLogger(__name__)
REQUIRED_IMU_COLUMNS = (
"timestamp(ms)",
"SCALED_IMU2.xacc",
"SCALED_IMU2.yacc",
"SCALED_IMU2.zacc",
"SCALED_IMU2.xgyro",
"SCALED_IMU2.ygyro",
"SCALED_IMU2.zgyro",
"GLOBAL_POSITION_INT.hdg",
FixtureBuilderConfig,
ImuCsvTlog,
Mp4PassthroughSource,
RawFdrPassthrough,
build_fixtures,
)
@@ -62,158 +45,15 @@ class P02BuilderConfig:
cli_bin: str = DEFAULT_CLI_BIN
# Step 1 — convert IMU CSV to tlog
def convert_imu_csv_to_tlog(
csv_path: Path,
output_tlog: Path,
*,
_mavlink_writer_factory: Callable | None = None,
) -> int:
"""Read `csv_path`, write one RAW_IMU + one ATTITUDE pair per row.
The Derkachi CSV ships at 10 Hz with ``SCALED_IMU2.*`` accelerometer
+ gyro fields and ``GLOBAL_POSITION_INT.hdg`` heading in
centidegrees. We pack RAW_IMU from the IMU columns (pass-through;
units may need conversion if the SUT's tlog parser rejects), and
synthesise ATTITUDE with yaw = `hdg_cdeg * pi / 18000` and
roll/pitch = 0 — acceptable for fixed-wing cruise.
Returns the number of pairs written.
Raises:
FileNotFoundError: `csv_path` missing.
ValueError: empty CSV, missing required column, OR malformed
numeric row.
"""
def resolve_derkachi_inputs(derkachi_dir: Path) -> tuple[Path, Path]:
"""Return ``(mp4_path, imu_csv_path)`` under ``derkachi_dir`` or raise."""
mp4 = derkachi_dir / "flight_derkachi.mp4"
csv_path = derkachi_dir / "data_imu.csv"
if not mp4.is_file():
raise FileNotFoundError(f"Derkachi MP4 not found: {mp4}")
if not csv_path.is_file():
raise FileNotFoundError(f"IMU CSV not found: {csv_path}")
rows = list(_iter_imu_rows(csv_path))
if not rows:
raise ValueError(f"IMU CSV is empty: {csv_path}")
if _mavlink_writer_factory is None:
from pymavlink import mavutil
def _mavlink_writer_factory(out: Path):
return mavutil.mavlogfile(str(out), write=True)
output_tlog.parent.mkdir(parents=True, exist_ok=True)
pairs = 0
writer = _mavlink_writer_factory(output_tlog)
try:
for row in rows:
try:
ts_ms = float(row["timestamp(ms)"])
xacc = int(float(row["SCALED_IMU2.xacc"]))
yacc = int(float(row["SCALED_IMU2.yacc"]))
zacc = int(float(row["SCALED_IMU2.zacc"]))
xgyro = int(float(row["SCALED_IMU2.xgyro"]))
ygyro = int(float(row["SCALED_IMU2.ygyro"]))
zgyro = int(float(row["SCALED_IMU2.zgyro"]))
hdg_cdeg = float(row["GLOBAL_POSITION_INT.hdg"])
except (ValueError, KeyError) as exc:
raise ValueError(
f"malformed IMU CSV row at {csv_path} row#{pairs + 1}: {exc}"
) from exc
yaw_rad = _hdg_centideg_to_rad(hdg_cdeg)
writer.write(_pack_raw_imu(int(ts_ms * 1000), xacc, yacc, zacc, xgyro, ygyro, zgyro))
writer.write(_pack_attitude(int(ts_ms), yaw_rad))
pairs += 1
finally:
close = getattr(writer, "close", None)
if callable(close):
close()
return pairs
def _iter_imu_rows(csv_path: Path) -> Iterator[dict[str, str]]:
"""Yield CSV rows; validates required columns are present in the header."""
with csv_path.open("r", newline="", encoding="utf-8") as fp:
reader = csv.DictReader(fp)
if reader.fieldnames is None:
raise ValueError(f"IMU CSV missing header: {csv_path}")
missing = [col for col in REQUIRED_IMU_COLUMNS if col not in reader.fieldnames]
if missing:
raise ValueError(
f"IMU CSV {csv_path} missing required columns: {missing}"
)
yield from reader
def _hdg_centideg_to_rad(hdg_cdeg: float) -> float:
"""Convert centidegrees [0, 36000) to radians [0, 2pi)."""
return (hdg_cdeg * math.pi) / 18000.0
def _pack_raw_imu(time_usec: int, xacc: int, yacc: int, zacc: int,
xgyro: int, ygyro: int, zgyro: int) -> bytes:
"""Pack a RAW_IMU MAVLink frame (msg id 27) with real motion data."""
from pymavlink.dialects.v20 import ardupilotmega as mavlink
packer = mavlink.MAVLink(file=None, srcSystem=1, srcComponent=1)
msg = mavlink.MAVLink_raw_imu_message(
time_usec=time_usec,
xacc=xacc, yacc=yacc, zacc=zacc,
xgyro=xgyro, ygyro=ygyro, zgyro=zgyro,
xmag=0, ymag=0, zmag=0,
id=0, temperature=0,
)
return msg.pack(packer)
def _pack_attitude(time_boot_ms: int, yaw_rad: float) -> bytes:
"""Pack an ATTITUDE MAVLink frame (msg id 30) with synthesised yaw."""
from pymavlink.dialects.v20 import ardupilotmega as mavlink
packer = mavlink.MAVLink(file=None, srcSystem=1, srcComponent=1)
msg = mavlink.MAVLink_attitude_message(
time_boot_ms=time_boot_ms,
roll=0.0, pitch=0.0, yaw=float(yaw_rad),
rollspeed=0.0, pitchspeed=0.0, yawspeed=0.0,
)
return msg.pack(packer)
# Step 2 — verify the FDR archive has at least one estimate record
def verify_fdr_has_estimates(fdr_path: Path) -> int:
"""Return the count of `record_type=="estimate"` records in `fdr_path`.
Raises ``ValueError`` if the file has zero such records — that
means the replay produced nothing useful for FT-P-02 to analyze.
Tolerates missing fields per record (only `record_type` is required
for filtering).
"""
if not fdr_path.is_file():
raise FileNotFoundError(f"FDR JSONL not found: {fdr_path}")
count = 0
with fdr_path.open("r", encoding="utf-8") as fp:
for line in fp:
line = line.strip()
if not line:
continue
try:
record = json.loads(line)
except json.JSONDecodeError:
continue
if record.get("record_type") == "estimate":
count += 1
if count == 0:
raise ValueError(
f"FDR archive {fdr_path} contains zero estimate records; "
f"the replay did not produce any outbound estimates for FT-P-02 to analyze"
)
return count
# Orchestration
raise FileNotFoundError(f"Derkachi IMU CSV not found: {csv_path}")
return mp4, csv_path
def build_p02_fixtures(
@@ -221,44 +61,22 @@ def build_p02_fixtures(
*,
_runner: Callable[[Sequence[str]], subprocess.CompletedProcess] | None = None,
_mavlink_writer_factory: Callable | None = None,
_verify_fdr: Callable[[Path], int] | None = None,
) -> Path:
"""End-to-end FT-P-02 fixture build. Returns the output directory.
Steps:
1. Resolve the Derkachi MP4 + IMU CSV under ``cfg.derkachi_dir``.
2. Convert IMU CSV to ``derkachi.tlog`` under ``cfg.output_dir``.
3. Run ``gps-denied-replay`` against the MP4 + tlog; write FDR JSONL
at ``<output_dir>/fdr/fdr.jsonl``.
4. Verify the FDR archive contains ≥1 estimate record.
5. Write the companion ``observer_<fc_kind>_<host>.json``.
"""
mp4 = cfg.derkachi_dir / "flight_derkachi.mp4"
csv_path = cfg.derkachi_dir / "data_imu.csv"
if not mp4.is_file():
raise FileNotFoundError(f"Derkachi MP4 not found: {mp4}")
if not csv_path.is_file():
raise FileNotFoundError(f"Derkachi IMU CSV not found: {csv_path}")
cfg.output_dir.mkdir(parents=True, exist_ok=True)
tlog = cfg.output_dir / "derkachi.tlog"
fdr_dir = cfg.output_dir / "fdr"
fdr_jsonl = fdr_dir / "fdr.jsonl"
convert_imu_csv_to_tlog(
csv_path, tlog, _mavlink_writer_factory=_mavlink_writer_factory,
"""End-to-end FT-P-02 fixture build. Returns the output directory."""
mp4, csv_path = resolve_derkachi_inputs(cfg.derkachi_dir)
builder_cfg = FixtureBuilderConfig(
video_source=Mp4PassthroughSource(mp4_path=mp4),
tlog_source=ImuCsvTlog(csv_path=csv_path),
fdr_projection=RawFdrPassthrough(verify_estimates=True),
output_dir=cfg.output_dir,
fc_kind=cfg.fc_kind, host=cfg.host, cli_bin=cfg.cli_bin,
video_filename="video_unused.mp4", # Mp4PassthroughSource returns mp4 directly
tlog_filename="derkachi.tlog",
fdr_subdir="fdr", fdr_filename="fdr.jsonl",
)
run_gps_denied_replay(
mp4, tlog, fdr_jsonl, cli_bin=cfg.cli_bin, _runner=_runner,
return build_fixtures(
builder_cfg, _runner=_runner, _mavlink_writer_factory=_mavlink_writer_factory,
)
verifier = _verify_fdr or verify_fdr_has_estimates
estimate_count = verifier(fdr_jsonl)
_LOG.info("FT-P-02 FDR archive contains %d estimate records", estimate_count)
observer_path = cfg.output_dir / f"observer_{cfg.fc_kind}_{cfg.host}.json"
write_observer_fixture(observer_path)
return cfg.output_dir
def _main(argv: Sequence[str] | None = None) -> int:
@@ -277,11 +95,8 @@ def _main(argv: Sequence[str] | None = None) -> int:
logging.basicConfig(level=logging.INFO)
cfg = P02BuilderConfig(
derkachi_dir=args.derkachi_dir,
output_dir=args.output_dir,
fc_kind=args.fc_kind,
host=args.host,
cli_bin=args.cli_bin,
derkachi_dir=args.derkachi_dir, output_dir=args.output_dir,
fc_kind=args.fc_kind, host=args.host, cli_bin=args.cli_bin,
)
build_p02_fixtures(cfg)
return 0
e2e/fixtures/sitl_replay_builder/builder.py
+618
View File
@@ -0,0 +1,618 @@
"""Parameterized fixture-builder framework for SITL replay scenarios (AZ-600).
The per-scenario fixture builders (`build_p01_fixtures.py`,
`build_p02_fixtures.py`, and future FT-P-04/05/07/08/10/11 builders) all
share the same shape:
1. Materialize a video file (MP4) from some source.
2. Materialize a tlog file from some source.
3. Run the production ``gps-denied-replay`` CLI against the pair.
4. Project the resulting FDR JSONL into the scenario's fixture shape.
5. Write the companion ``observer_<fc_kind>_<host>.json``.
Only steps 1, 2, and 4 vary across scenarios; the rest is shared. This
module exposes three strategy ABCs (``VideoSource``, ``TlogSource``,
``FdrProjection``) plus the four concrete impls used by FT-P-01 + FT-P-02,
and a single ``build_fixtures(cfg)`` orchestrator that composes them.
Adding a new scenario typically means writing a ~30-line config factory in
a thin per-scenario module (see ``build_p01_fixtures.py`` /
``build_p02_fixtures.py`` for working examples); no new strategy code is
required unless the scenario has a genuinely new video / tlog / FDR shape.
"""
from __future__ import annotations
import abc
import csv
import json
import logging
import math
import subprocess
from dataclasses import dataclass, field
from pathlib import Path
from typing import Callable, Iterator, Sequence
_LOG = logging.getLogger(__name__)
DEFAULT_CLI_BIN = "gps-denied-replay"
DEFAULT_FPS = 1.0
DEFAULT_TLOG_DURATION_S = 120
DEFAULT_TLOG_HZ = 200
DEFAULT_FDR_KIND = "outbound_position_estimate"
# Gravity in mg, used as the stationary z-accel sample (RAW_IMU is in mg).
STATIONARY_Z_ACCEL_MG = -9810
# ---------------------------------------------------------------------------
# Subprocess driver + observer-fixture writer (shared by every scenario)
# ---------------------------------------------------------------------------
def run_gps_denied_replay(
video: Path,
tlog: Path,
fdr_out: Path,
*,
cli_bin: str = DEFAULT_CLI_BIN,
time_offset_ms: int = 0,
extra_args: Sequence[str] = (),
_runner: Callable[[Sequence[str]], subprocess.CompletedProcess] | None = None,
) -> subprocess.CompletedProcess:
"""Run ``gps-denied-replay`` as a subprocess.
``time_offset_ms`` defaults to 0 because most synthetic / aligned-input
scenarios intentionally bypass auto-sync. Operators running this
against truly independent tlog+video pairs SHOULD omit it and let the
production auto-sync run.
Raises ``subprocess.CalledProcessError`` on non-zero exit code. The
default subprocess runner can be swapped via ``_runner`` for unit tests.
"""
fdr_out.parent.mkdir(parents=True, exist_ok=True)
cmd: list[str] = [
cli_bin,
"--video", str(video),
"--tlog", str(tlog),
"--time-offset-ms", str(time_offset_ms),
"--fdr-out", str(fdr_out),
*extra_args,
]
_LOG.info("running: %s", " ".join(cmd))
runner = _runner or (lambda c: subprocess.run(c, check=True, capture_output=True, text=True))
return runner(cmd)
def write_observer_fixture(output_path: Path) -> None:
"""Write the minimal ``observer_<fc_kind>_<host>.json`` ``get_observer`` needs.
Scenarios that only consume ``wait_for_outbound`` or ``iter_records``
still trigger ``sitl_observer.get_observer(...)`` for construction.
Populate with safe defaults; scenarios that care about
``read_gps_state`` ship their own observer fixtures.
"""
payload = {
"gps_state": {
"primary_source": "MAV",
"last_position_lat_deg": 0.0,
"last_position_lon_deg": 0.0,
"last_position_alt_m": 0.0,
"fix_quality": 3,
"horizontal_accuracy_m": 1.0,
"last_update_age_ms": 0,
},
"parameters": {},
}
output_path.parent.mkdir(parents=True, exist_ok=True)
output_path.write_text(json.dumps(payload, indent=2))
# ---------------------------------------------------------------------------
# Parameterized MAVLink packers (shared by every TlogSource)
# ---------------------------------------------------------------------------
def pack_raw_imu(
time_usec: int,
*,
xacc: int = 0,
yacc: int = 0,
zacc: int = 0,
xgyro: int = 0,
ygyro: int = 0,
zgyro: int = 0,
) -> bytes:
"""Pack a RAW_IMU MAVLink frame (msg id 27).
All values pass-through to the MAVLink wire format. Stationary callers
use ``zacc=STATIONARY_Z_ACCEL_MG`` (≈ -9810 mg ≈ 1 g) to encode gravity.
"""
from pymavlink.dialects.v20 import ardupilotmega as mavlink
packer = mavlink.MAVLink(file=None, srcSystem=1, srcComponent=1)
msg = mavlink.MAVLink_raw_imu_message(
time_usec=time_usec,
xacc=xacc, yacc=yacc, zacc=zacc,
xgyro=xgyro, ygyro=ygyro, zgyro=zgyro,
xmag=0, ymag=0, zmag=0,
id=0, temperature=0,
)
return msg.pack(packer)
def pack_attitude(
time_boot_ms: int,
*,
roll: float = 0.0,
pitch: float = 0.0,
yaw: float = 0.0,
) -> bytes:
"""Pack an ATTITUDE MAVLink frame (msg id 30)."""
from pymavlink.dialects.v20 import ardupilotmega as mavlink
packer = mavlink.MAVLink(file=None, srcSystem=1, srcComponent=1)
msg = mavlink.MAVLink_attitude_message(
time_boot_ms=time_boot_ms,
roll=float(roll), pitch=float(pitch), yaw=float(yaw),
rollspeed=0.0, pitchspeed=0.0, yawspeed=0.0,
)
return msg.pack(packer)
def _default_mavlink_writer_factory(out: Path):
"""Return a pymavlink ``mavlogfile`` open for write."""
from pymavlink import mavutil
return mavutil.mavlogfile(str(out), write=True)
def hdg_centideg_to_rad(hdg_cdeg: float) -> float:
"""Convert centidegrees [0, 36000) to radians [0, 2pi)."""
return (hdg_cdeg * math.pi) / 18000.0
# ---------------------------------------------------------------------------
# VideoSource strategy
# ---------------------------------------------------------------------------
class VideoSource(abc.ABC):
"""Strategy: materialize the MP4 the replay CLI consumes."""
@abc.abstractmethod
def materialize(
self,
output_path: Path,
*,
_video_writer_factory: Callable | None = None,
_imread: Callable | None = None,
) -> Path:
"""Return the path of a ready-to-consume MP4.
Implementations may either write a new file at ``output_path`` (and
return ``output_path``) or pass through an already-existing MP4
(returning its real location, ignoring ``output_path``).
"""
@dataclass(frozen=True)
class StillImagesSource(VideoSource):
"""Encode a sequence of still images into an MP4 at ``fps``."""
image_paths: Sequence[Path]
fps: float = DEFAULT_FPS
def materialize(
self,
output_path: Path,
*,
_video_writer_factory: Callable | None = None,
_imread: Callable | None = None,
) -> Path:
if not self.image_paths:
raise FileNotFoundError(
"StillImagesSource: image_paths is empty; nothing to encode"
)
if _video_writer_factory is None or _imread is None:
import cv2
_imread = _imread or (lambda path: cv2.imread(str(path), cv2.IMREAD_COLOR))
if _video_writer_factory is None:
_fourcc = cv2.VideoWriter_fourcc(*"mp4v")
fps = self.fps
def _video_writer_factory(out: Path, width: int, height: int):
return cv2.VideoWriter(str(out), _fourcc, fps, (width, height))
first_frame = _imread(self.image_paths[0])
if first_frame is None:
raise FileNotFoundError(
f"StillImagesSource: failed to read {self.image_paths[0]}"
)
height, width = first_frame.shape[:2]
output_path.parent.mkdir(parents=True, exist_ok=True)
writer = _video_writer_factory(output_path, width, height)
try:
writer.write(first_frame)
for path in self.image_paths[1:]:
frame = _imread(path)
if frame is None:
raise FileNotFoundError(
f"StillImagesSource: failed to read {path}"
)
writer.write(frame)
finally:
writer.release()
return output_path
@dataclass(frozen=True)
class Mp4PassthroughSource(VideoSource):
"""Use an already-existing MP4 (no copy, no encode)."""
mp4_path: Path
def materialize(self, output_path: Path, **_deps) -> Path:
if not self.mp4_path.is_file():
raise FileNotFoundError(f"Mp4PassthroughSource: MP4 not found: {self.mp4_path}")
return self.mp4_path
# ---------------------------------------------------------------------------
# TlogSource strategy
# ---------------------------------------------------------------------------
class TlogSource(abc.ABC):
"""Strategy: materialize the tlog the replay CLI consumes."""
@abc.abstractmethod
def materialize(
self,
output_path: Path,
*,
_mavlink_writer_factory: Callable | None = None,
) -> Path:
"""Return the path of a ready-to-consume tlog."""
@dataclass(frozen=True)
class SyntheticStationaryTlog(TlogSource):
"""Write a tlog of zero-motion RAW_IMU + ATTITUDE pairs (z-accel = gravity)."""
duration_s: int = DEFAULT_TLOG_DURATION_S
hz: int = DEFAULT_TLOG_HZ
def materialize(
self,
output_path: Path,
*,
_mavlink_writer_factory: Callable | None = None,
) -> Path:
if self.duration_s <= 0:
raise ValueError(f"duration_s must be positive; got {self.duration_s}")
if self.hz <= 0:
raise ValueError(f"hz must be positive; got {self.hz}")
factory = _mavlink_writer_factory or _default_mavlink_writer_factory
output_path.parent.mkdir(parents=True, exist_ok=True)
writer = factory(output_path)
try:
period_us = int(1_000_000 / self.hz)
total_pairs = self.duration_s * self.hz
for i in range(total_pairs):
time_us = i * period_us
writer.write(pack_raw_imu(time_us, zacc=STATIONARY_Z_ACCEL_MG))
writer.write(pack_attitude(time_us // 1000))
finally:
close = getattr(writer, "close", None)
if callable(close):
close()
return output_path
@dataclass(frozen=True)
class ImuCsvSchema:
"""Column-name map for a flight-recorded IMU CSV (Derkachi default)."""
timestamp_ms_col: str = "timestamp(ms)"
xacc_col: str = "SCALED_IMU2.xacc"
yacc_col: str = "SCALED_IMU2.yacc"
zacc_col: str = "SCALED_IMU2.zacc"
xgyro_col: str = "SCALED_IMU2.xgyro"
ygyro_col: str = "SCALED_IMU2.ygyro"
zgyro_col: str = "SCALED_IMU2.zgyro"
hdg_centideg_col: str = "GLOBAL_POSITION_INT.hdg"
@property
def required_columns(self) -> tuple[str, ...]:
return (
self.timestamp_ms_col, self.xacc_col, self.yacc_col, self.zacc_col,
self.xgyro_col, self.ygyro_col, self.zgyro_col, self.hdg_centideg_col,
)
DEFAULT_DERKACHI_IMU_SCHEMA = ImuCsvSchema()
@dataclass(frozen=True)
class ImuCsvTlog(TlogSource):
"""Convert a recorded IMU CSV to a tlog with real RAW_IMU + ATTITUDE values."""
csv_path: Path
schema: ImuCsvSchema = DEFAULT_DERKACHI_IMU_SCHEMA
def materialize(
self,
output_path: Path,
*,
_mavlink_writer_factory: Callable | None = None,
) -> Path:
if not self.csv_path.is_file():
raise FileNotFoundError(f"IMU CSV not found: {self.csv_path}")
rows = list(self._iter_rows())
if not rows:
raise ValueError(f"IMU CSV is empty: {self.csv_path}")
factory = _mavlink_writer_factory or _default_mavlink_writer_factory
output_path.parent.mkdir(parents=True, exist_ok=True)
writer = factory(output_path)
try:
for index, row in enumerate(rows, start=1):
try:
ts_ms = float(row[self.schema.timestamp_ms_col])
xacc = int(float(row[self.schema.xacc_col]))
yacc = int(float(row[self.schema.yacc_col]))
zacc = int(float(row[self.schema.zacc_col]))
xgyro = int(float(row[self.schema.xgyro_col]))
ygyro = int(float(row[self.schema.ygyro_col]))
zgyro = int(float(row[self.schema.zgyro_col]))
hdg_cdeg = float(row[self.schema.hdg_centideg_col])
except (ValueError, KeyError) as exc:
raise ValueError(
f"malformed IMU CSV row at {self.csv_path} row#{index}: {exc}"
) from exc
yaw_rad = hdg_centideg_to_rad(hdg_cdeg)
writer.write(pack_raw_imu(
int(ts_ms * 1000),
xacc=xacc, yacc=yacc, zacc=zacc,
xgyro=xgyro, ygyro=ygyro, zgyro=zgyro,
))
writer.write(pack_attitude(int(ts_ms), yaw=yaw_rad))
finally:
close = getattr(writer, "close", None)
if callable(close):
close()
return output_path
def _iter_rows(self) -> Iterator[dict[str, str]]:
with self.csv_path.open("r", newline="", encoding="utf-8") as fp:
reader = csv.DictReader(fp)
if reader.fieldnames is None:
raise ValueError(f"IMU CSV missing header: {self.csv_path}")
missing = [c for c in self.schema.required_columns if c not in reader.fieldnames]
if missing:
raise ValueError(
f"IMU CSV {self.csv_path} missing required columns: {missing}"
)
yield from reader
# ---------------------------------------------------------------------------
# FdrProjection strategy
# ---------------------------------------------------------------------------
class FdrProjection(abc.ABC):
"""Strategy: translate the FDR JSONL into the scenario's fixture shape."""
@abc.abstractmethod
def materialize(
self,
fdr_jsonl: Path,
output_dir: Path,
fc_kind: str,
host: str,
) -> None:
"""Read ``fdr_jsonl`` and write any scenario-specific fixture artifacts."""
@dataclass(frozen=True)
class RawFdrPassthrough(FdrProjection):
"""Leave the FDR archive as-is; optionally assert it has ≥1 estimate record."""
verify_estimates: bool = True
def materialize(self, fdr_jsonl: Path, output_dir: Path, fc_kind: str, host: str) -> None:
if not self.verify_estimates:
return
count = verify_fdr_has_estimates(fdr_jsonl)
_LOG.info("FDR archive %s contains %d estimate records", fdr_jsonl, count)
@dataclass(frozen=True)
class OutboundMessagesProjection(FdrProjection):
"""Parse FDR ``outbound_position_estimate`` records into ``outbound_messages_*.json``."""
image_ids: Sequence[str] = field(default_factory=tuple)
fdr_kind: str = DEFAULT_FDR_KIND
lat_key: str = "lat_deg"
lon_key: str = "lon_deg"
def materialize(self, fdr_jsonl: Path, output_dir: Path, fc_kind: str, host: str) -> None:
raw_estimates = parse_fdr_for_outbound_estimates(
fdr_jsonl, fdr_kind=self.fdr_kind,
lat_key=self.lat_key, lon_key=self.lon_key,
)
estimates: list[dict | None] = list(raw_estimates[: len(self.image_ids)])
if len(raw_estimates) > len(self.image_ids):
_LOG.warning(
"FDR carried %d outbound estimates but only %d images were pushed; "
"truncating to the per-frame count",
len(raw_estimates), len(self.image_ids),
)
while len(estimates) < len(self.image_ids):
estimates.append(None)
output_path = output_dir / f"outbound_messages_{fc_kind}_{host}.json"
_write_outbound_messages_fixture(output_path, self.image_ids, estimates)
def parse_fdr_for_outbound_estimates(
fdr_path: Path,
*,
fdr_kind: str = DEFAULT_FDR_KIND,
lat_key: str = "lat_deg",
lon_key: str = "lon_deg",
) -> list[dict]:
"""Walk ``fdr_path`` (JSONL) and return outbound-estimate payloads in order."""
if not fdr_path.is_file():
raise FileNotFoundError(f"FDR JSONL not found: {fdr_path}")
out: list[dict] = []
with fdr_path.open("r", encoding="utf-8") as fp:
for line_no, line in enumerate(fp, start=1):
line = line.strip()
if not line:
continue
try:
record = json.loads(line)
except json.JSONDecodeError as exc:
raise ValueError(
f"malformed FDR JSON at {fdr_path}:{line_no}: {exc.msg}"
) from exc
if record.get("kind") != fdr_kind:
continue
payload = record.get("payload", {})
if not isinstance(payload, dict):
continue
if lat_key not in payload or lon_key not in payload:
continue
out.append({
"lat_deg": float(payload[lat_key]),
"lon_deg": float(payload[lon_key]),
})
return out
def verify_fdr_has_estimates(fdr_path: Path) -> int:
"""Return the count of ``record_type == "estimate"`` records in ``fdr_path``.
Raises ``ValueError`` if the file has zero such records — that means
the replay produced nothing useful for the scenario to analyze.
"""
if not fdr_path.is_file():
raise FileNotFoundError(f"FDR JSONL not found: {fdr_path}")
count = 0
with fdr_path.open("r", encoding="utf-8") as fp:
for line in fp:
line = line.strip()
if not line:
continue
try:
record = json.loads(line)
except json.JSONDecodeError:
continue
if record.get("record_type") == "estimate":
count += 1
if count == 0:
raise ValueError(
f"FDR archive {fdr_path} contains zero estimate records; "
f"the replay did not produce any outbound estimates for the scenario to analyze"
)
return count
def _write_outbound_messages_fixture(
output_path: Path,
image_ids: Sequence[str],
estimates: Sequence[dict | None],
) -> None:
"""Write ``outbound_messages_<fc_kind>_<host>.json``.
``image_ids`` and ``estimates`` must have the same length. ``None``
entries in ``estimates`` are persisted as JSON ``null`` (timeout
markers); other entries must carry ``lat_deg``/``lon_deg``.
"""
if len(image_ids) != len(estimates):
raise ValueError(
f"length mismatch: {len(image_ids)} image_ids vs {len(estimates)} estimates"
)
messages: list[dict | None] = []
for image_id, estimate in zip(image_ids, estimates):
if estimate is None:
messages.append(None)
continue
messages.append({
"image_id": image_id,
"lat_deg": float(estimate["lat_deg"]),
"lon_deg": float(estimate["lon_deg"]),
})
output_path.parent.mkdir(parents=True, exist_ok=True)
output_path.write_text(json.dumps({"messages": messages}, indent=2))
# ---------------------------------------------------------------------------
# Orchestrator
# ---------------------------------------------------------------------------
@dataclass(frozen=True)
class FixtureBuilderConfig:
"""Per-invocation config consumed by ``build_fixtures``."""
video_source: VideoSource
tlog_source: TlogSource
fdr_projection: FdrProjection
output_dir: Path
fc_kind: str = "ardupilot"
host: str = "sitl-host"
cli_bin: str = DEFAULT_CLI_BIN
video_filename: str = "video.mp4"
tlog_filename: str = "telemetry.tlog"
fdr_subdir: str = "fdr"
fdr_filename: str = "fdr.jsonl"
time_offset_ms: int = 0
def build_fixtures(
cfg: FixtureBuilderConfig,
*,
_runner: Callable[[Sequence[str]], subprocess.CompletedProcess] | None = None,
_video_writer_factory: Callable | None = None,
_imread: Callable | None = None,
_mavlink_writer_factory: Callable | None = None,
) -> Path:
"""End-to-end fixture build. Returns the output directory.
Steps:
1. Ask the ``VideoSource`` to materialize the MP4.
2. Ask the ``TlogSource`` to materialize the tlog.
3. Run the production ``gps-denied-replay`` CLI against the pair.
4. Ask the ``FdrProjection`` to translate the FDR JSONL.
5. Write the companion observer fixture.
"""
cfg.output_dir.mkdir(parents=True, exist_ok=True)
fdr_jsonl = cfg.output_dir / cfg.fdr_subdir / cfg.fdr_filename
video = cfg.video_source.materialize(
cfg.output_dir / cfg.video_filename,
_video_writer_factory=_video_writer_factory, _imread=_imread,
)
tlog = cfg.tlog_source.materialize(
cfg.output_dir / cfg.tlog_filename,
_mavlink_writer_factory=_mavlink_writer_factory,
)
run_gps_denied_replay(
video, tlog, fdr_jsonl,
cli_bin=cfg.cli_bin, time_offset_ms=cfg.time_offset_ms, _runner=_runner,
)
cfg.fdr_projection.materialize(fdr_jsonl, cfg.output_dir, cfg.fc_kind, cfg.host)
write_observer_fixture(cfg.output_dir / f"observer_{cfg.fc_kind}_{cfg.host}.json")
return cfg.output_dir