[AZ-599] Batch 79: FT-P-02 Derkachi builder + _common.py extraction

- Add build_p02_fixtures.py: IMU CSV → tlog conversion (RAW_IMU +
  ATTITUDE pairs, centidegrees→radians yaw) and orchestrator that
  runs gps-denied replay against Derkachi MP4 + generated tlog,
  verifying ≥1 record_type="estimate" in the FDR archive.
- Extract run_gps_denied_replay + FDR-parent-dir helpers into
  sitl_replay_builder/_common.py; refactor build_p01_fixtures.py
  to import from _common (b78 tests preserved).
- Add 20 unit tests under e2e/_unit_tests/fixtures/test_sitl_
  replay_builder_p02.py covering AC-1..AC-5; total unit suite
  686/686 passing (regression gate AC-6).
- README updated to document FT-P-01 + FT-P-02 builders.
- Advance autodev state: last_completed_batch=79, current_batch=80;
  prune verbose detail blob.

Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
Oleksandr Bezdieniezhnykh
2026-05-17 13:40:07 +03:00
parent 2f1fb4d0d0
commit 4e0717e543
10 changed files with 1111 additions and 76 deletions
@@ -0,0 +1,324 @@
"""Unit tests for `e2e/fixtures/sitl_replay_builder/build_p02_fixtures.py` (AZ-599).
All external dependencies (pymavlink, subprocess) are injected via the
underscore-prefixed parameters. The IMU CSV is small enough that we
hand-author it inline for each test rather than depending on the real
Derkachi data file.
"""
from __future__ import annotations
import csv
import json
import math
import subprocess
from pathlib import Path
from typing import Sequence
from unittest.mock import MagicMock
import pytest
import e2e.fixtures.sitl_replay_builder.build_p02_fixtures as bp02
_HEADER_ROW = (
"timestamp(ms),Time,SCALED_IMU2.xacc,SCALED_IMU2.yacc,SCALED_IMU2.zacc,"
"SCALED_IMU2.xgyro,SCALED_IMU2.ygyro,SCALED_IMU2.zgyro,"
"SCALED_IMU2.xmag,SCALED_IMU2.ymag,SCALED_IMU2.zmag,"
"GLOBAL_POSITION_INT.lat,GLOBAL_POSITION_INT.lon,GLOBAL_POSITION_INT.alt,"
"GLOBAL_POSITION_INT.relative_alt,GLOBAL_POSITION_INT.vx,GLOBAL_POSITION_INT.vy,"
"GLOBAL_POSITION_INT.vz,GLOBAL_POSITION_INT.hdg"
)
def _write_imu_csv(path: Path, rows: list[list]) -> None:
"""Write a CSV with the full Derkachi header + the supplied data rows."""
path.parent.mkdir(parents=True, exist_ok=True)
with path.open("w", newline="", encoding="utf-8") as fp:
fp.write(_HEADER_ROW + "\n")
writer = csv.writer(fp)
for row in rows:
writer.writerow(row)
def _good_row(ts_ms: float, hdg: int = 35041) -> list:
"""One well-formed Derkachi row at `ts_ms` and heading `hdg` cdeg."""
return [
ts_ms, 0.0,
21, -3, -984,
52, 32, -5,
312, -1048, 442,
50_080_963_4, 36_111_544_2, 141_290, 23_182,
-4, -6, -88,
hdg,
]
# convert_imu_csv_to_tlog
def test_convert_imu_csv_missing_file_raises(tmp_path: Path):
# Assert
with pytest.raises(FileNotFoundError, match="IMU CSV not found"):
bp02.convert_imu_csv_to_tlog(
tmp_path / "missing.csv", tmp_path / "out.tlog",
_mavlink_writer_factory=lambda out: MagicMock(),
)
def test_convert_imu_csv_empty_raises(tmp_path: Path):
# Arrange — header only, no data rows
csv_path = tmp_path / "imu.csv"
_write_imu_csv(csv_path, [])
# Assert
with pytest.raises(ValueError, match="IMU CSV is empty"):
bp02.convert_imu_csv_to_tlog(
csv_path, tmp_path / "out.tlog",
_mavlink_writer_factory=lambda out: MagicMock(),
)
def test_convert_imu_csv_missing_required_column_raises(tmp_path: Path):
# Arrange — header missing `GLOBAL_POSITION_INT.hdg`
csv_path = tmp_path / "imu.csv"
csv_path.write_text("timestamp(ms),Time\n0,0\n")
# Assert
with pytest.raises(ValueError, match="missing required columns"):
bp02.convert_imu_csv_to_tlog(
csv_path, tmp_path / "out.tlog",
_mavlink_writer_factory=lambda out: MagicMock(),
)
def test_convert_imu_csv_malformed_numeric_raises(tmp_path: Path):
# Arrange — second-to-last value (xacc) is non-numeric
csv_path = tmp_path / "imu.csv"
row = _good_row(0.0)
row[2] = "not-a-number"
_write_imu_csv(csv_path, [row])
# Assert
with pytest.raises(ValueError, match="malformed IMU CSV row"):
bp02.convert_imu_csv_to_tlog(
csv_path, tmp_path / "out.tlog",
_mavlink_writer_factory=lambda out: MagicMock(),
)
def test_convert_imu_csv_writes_pair_per_row(tmp_path: Path):
# Arrange
csv_path = tmp_path / "imu.csv"
_write_imu_csv(csv_path, [_good_row(0.0), _good_row(100.0), _good_row(200.0)])
writer = MagicMock(write=MagicMock(), close=MagicMock())
# Act
pairs = bp02.convert_imu_csv_to_tlog(
csv_path, tmp_path / "out.tlog",
_mavlink_writer_factory=lambda out: writer,
)
# Assert — 3 rows → 3 pairs → 6 message writes
assert pairs == 3
assert writer.write.call_count == 6
assert writer.close.call_count == 1
def test_convert_imu_csv_real_pymavlink_round_trip(tmp_path: Path):
"""Sanity-check the real packers; tlog file is well-formed."""
# Arrange
csv_path = tmp_path / "imu.csv"
_write_imu_csv(csv_path, [_good_row(0.0), _good_row(100.0)])
# Act — use real pymavlink (it's in pyproject.toml deps)
pairs = bp02.convert_imu_csv_to_tlog(csv_path, tmp_path / "out.tlog")
# Assert
assert pairs == 2
assert (tmp_path / "out.tlog").stat().st_size > 0
# _hdg_centideg_to_rad
@pytest.mark.parametrize(
"centideg,expected_rad",
[
(0, 0.0),
(9000, math.pi / 2),
(18000, math.pi),
(27000, 3 * math.pi / 2),
(35990, 35990 * math.pi / 18000),
],
)
def test_hdg_centideg_to_rad(centideg: int, expected_rad: float):
# Assert
assert bp02._hdg_centideg_to_rad(centideg) == pytest.approx(expected_rad)
# verify_fdr_has_estimates
def _write_jsonl(path: Path, records: list[dict]) -> None:
path.parent.mkdir(parents=True, exist_ok=True)
path.write_text("\n".join(json.dumps(r) for r in records))
def test_verify_fdr_missing_file_raises(tmp_path: Path):
# Assert
with pytest.raises(FileNotFoundError, match="FDR JSONL not found"):
bp02.verify_fdr_has_estimates(tmp_path / "missing.jsonl")
def test_verify_fdr_no_estimates_raises(tmp_path: Path):
# Arrange
fdr = tmp_path / "fdr.jsonl"
_write_jsonl(fdr, [
{"record_type": "other", "payload": {}},
{"record_type": "imu_tick", "payload": {}},
])
# Assert
with pytest.raises(ValueError, match="zero estimate records"):
bp02.verify_fdr_has_estimates(fdr)
def test_verify_fdr_counts_estimates(tmp_path: Path):
# Arrange
fdr = tmp_path / "fdr.jsonl"
_write_jsonl(fdr, [
{"record_type": "estimate", "payload": {}},
{"record_type": "other", "payload": {}},
{"record_type": "estimate", "payload": {}},
{"record_type": "estimate", "payload": {}},
])
# Act
count = bp02.verify_fdr_has_estimates(fdr)
# Assert
assert count == 3
def test_verify_fdr_tolerates_malformed_lines(tmp_path: Path):
# Arrange — one bad JSON line interleaved with good estimate records
fdr = tmp_path / "fdr.jsonl"
fdr.write_text(
json.dumps({"record_type": "estimate"}) + "\n"
+ "{not valid json\n"
+ json.dumps({"record_type": "estimate"}) + "\n"
)
# Act
count = bp02.verify_fdr_has_estimates(fdr)
# Assert
assert count == 2
# build_p02_fixtures end-to-end (mocked)
def test_build_p02_missing_video_raises(tmp_path: Path):
# Arrange
derkachi_dir = tmp_path / "derkachi"
derkachi_dir.mkdir()
(derkachi_dir / "data_imu.csv").write_text(_HEADER_ROW + "\n")
cfg = bp02.P02BuilderConfig(
derkachi_dir=derkachi_dir, output_dir=tmp_path / "out",
)
# Assert
with pytest.raises(FileNotFoundError, match="Derkachi MP4 not found"):
bp02.build_p02_fixtures(cfg)
def test_build_p02_missing_csv_raises(tmp_path: Path):
# Arrange
derkachi_dir = tmp_path / "derkachi"
derkachi_dir.mkdir()
(derkachi_dir / "flight_derkachi.mp4").touch()
cfg = bp02.P02BuilderConfig(
derkachi_dir=derkachi_dir, output_dir=tmp_path / "out",
)
# Assert
with pytest.raises(FileNotFoundError, match="Derkachi IMU CSV not found"):
bp02.build_p02_fixtures(cfg)
def test_build_p02_end_to_end_with_mocks(tmp_path: Path):
# Arrange
derkachi_dir = tmp_path / "derkachi"
output_dir = tmp_path / "out"
derkachi_dir.mkdir()
(derkachi_dir / "flight_derkachi.mp4").touch()
_write_imu_csv(derkachi_dir / "data_imu.csv", [_good_row(0.0), _good_row(100.0)])
def fake_runner(cmd):
fdr_path = Path(cmd[cmd.index("--fdr-out") + 1])
_write_jsonl(fdr_path, [
{"record_type": "estimate", "payload": {"lat_deg": 50.0, "lon_deg": 36.0}},
{"record_type": "estimate", "payload": {"lat_deg": 50.1, "lon_deg": 36.1}},
])
return subprocess.CompletedProcess(cmd, 0)
cfg = bp02.P02BuilderConfig(
derkachi_dir=derkachi_dir, output_dir=output_dir,
fc_kind="ardupilot", host="sitl-host",
)
# Act
result = bp02.build_p02_fixtures(
cfg,
_runner=fake_runner,
_mavlink_writer_factory=lambda out: MagicMock(write=MagicMock(), close=MagicMock()),
)
# Assert
assert result == output_dir
assert (output_dir / "fdr" / "fdr.jsonl").is_file()
observer = json.loads((output_dir / "observer_ardupilot_sitl-host.json").read_text())
assert observer["gps_state"]["primary_source"] == "MAV"
def test_build_p02_propagates_verify_failure(tmp_path: Path):
# Arrange — fake runner writes an FDR with no estimates; default verifier raises.
derkachi_dir = tmp_path / "derkachi"
derkachi_dir.mkdir()
(derkachi_dir / "flight_derkachi.mp4").touch()
_write_imu_csv(derkachi_dir / "data_imu.csv", [_good_row(0.0)])
def fake_runner(cmd):
fdr_path = Path(cmd[cmd.index("--fdr-out") + 1])
_write_jsonl(fdr_path, [{"record_type": "imu_tick"}])
return subprocess.CompletedProcess(cmd, 0)
cfg = bp02.P02BuilderConfig(
derkachi_dir=derkachi_dir, output_dir=tmp_path / "out",
)
# Assert
with pytest.raises(ValueError, match="zero estimate records"):
bp02.build_p02_fixtures(
cfg,
_runner=fake_runner,
_mavlink_writer_factory=lambda out: MagicMock(write=MagicMock(), close=MagicMock()),
)
# `_common.py` is shared with b78
def test_common_module_exports_used_by_b01():
"""AC-5: b78 builder still imports from _common.py after refactor."""
# Arrange
import e2e.fixtures.sitl_replay_builder.build_p01_fixtures as bp01
import e2e.fixtures.sitl_replay_builder._common as common
# Assert
assert bp01.run_gps_denied_replay is common.run_gps_denied_replay
assert bp01.write_observer_fixture is common.write_observer_fixture
+2
View File
@@ -58,7 +58,9 @@ E2E_ROOT = Path(__file__).resolve().parents[1]
"runner/helpers/fc_proxy_runtime.py",
"runner/helpers/replay_mode.py",
"fixtures/sitl_replay_builder/__init__.py",
"fixtures/sitl_replay_builder/_common.py",
"fixtures/sitl_replay_builder/build_p01_fixtures.py",
"fixtures/sitl_replay_builder/build_p02_fixtures.py",
"fixtures/sitl_replay_builder/README.md",
"fixtures/mock-suite-sat/Dockerfile",
"fixtures/mock-suite-sat/app.py",
+74 -13
View File
@@ -1,16 +1,33 @@
# SITL Replay Fixture Builder (AZ-598)
# SITL Replay Fixture Builder (AZ-598, AZ-599)
Produces the `outbound_messages_<fc_kind>_<host>.json` +
`observer_<fc_kind>_<host>.json` fixtures consumed by the b75
`sitl_observer` module in offline FDR-replay mode (b75/b78).
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.
## Vertical-slice scope (this batch)
| 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-02 (Derkachi drift) | `build_p02_fixtures.py` | `flight_derkachi.mp4` + `data_imu.csv` | `derkachi.tlog` + `fdr/fdr.jsonl` (FDR archive) + `observer_<fc>_<host>.json` |
Only the FT-P-01 still-image accuracy scenario is supported. Other
scenarios (FT-P-02 Derkachi continuous flight, FT-N-04 blackout-spoof,
etc.) 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 / 02 / 03 / 04)
need their own capture flows and will land as follow-up tickets.
## Strategy
## Shared helpers (`_common.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`:
* `run_gps_denied_replay(video, tlog, fdr_out, *, time_offset_ms=0, ...)`
* `write_observer_fixture(output_path)`
Future per-scenario builders should import from `_common.py` rather
than re-implementing.
## FT-P-01 (`build_p01_fixtures.py`)
### Strategy
Rather than spinning up a SITL container, this builder reuses the
production `gps-denied-replay` CLI + `ReplayInputAdapter`:
@@ -30,10 +47,10 @@ 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.
## Usage
### Usage
```bash
gps-denied-build-p01-fixtures \
python -m e2e.fixtures.sitl_replay_builder.build_p01_fixtures \
--input-dir _docs/00_problem/input_data \
--output-dir e2e/fixtures/sitl_replay/p01 \
--fc-kind ardupilot \
@@ -55,7 +72,7 @@ E2E_SITL_REPLAY_DIR=e2e/fixtures/sitl_replay/p01 \
pytest e2e/tests/positive/test_ft_p_01_still_image_accuracy.py
```
## Limitations
### Limitations
* The synthetic tlog encodes zero motion — auto-sync MUST be bypassed
via `--time-offset-ms 0` (the builder does this automatically).
@@ -67,9 +84,53 @@ E2E_SITL_REPLAY_DIR=e2e/fixtures/sitl_replay/p01 \
* 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
```bash
python -m e2e.fixtures.sitl_replay_builder.build_p02_fixtures \
--derkachi-dir _docs/00_problem/input_data/flight_derkachi \
--output-dir e2e/fixtures/sitl_replay/p02 \
--fc-kind ardupilot \
--host sitl-host
```
Output:
* `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.
## Testing
Unit tests under `e2e/_unit_tests/fixtures/test_sitl_replay_builder.py`
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
@@ -0,0 +1,85 @@
"""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))
@@ -34,13 +34,18 @@ from dataclasses import dataclass
from pathlib import Path
from typing import Callable, Iterable, Sequence
from e2e.fixtures.sitl_replay_builder._common import (
DEFAULT_CLI_BIN,
run_gps_denied_replay,
write_observer_fixture,
)
_LOG = logging.getLogger(__name__)
DEFAULT_FPS = 1.0
DEFAULT_TLOG_DURATION_S = 120
DEFAULT_TLOG_HZ = 200
DEFAULT_FDR_KIND = "outbound_position_estimate"
DEFAULT_CLI_BIN = "gps-denied-replay"
@dataclass(frozen=True)
@@ -214,40 +219,7 @@ def _pack_attitude_zero(time_boot_ms: int) -> bytes:
# Step 3 — drive `gps-denied-replay` against the generated video+tlog
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.
Bypasses auto-sync via ``--time-offset-ms 0`` because the synthetic
stationary tlog has no take-off signal to detect.
Raises ``subprocess.CalledProcessError`` on non-zero exit code (with
the FDR path included in the error message). The default subprocess
runner can be swapped via the underscore-prefixed parameter for 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)
# (`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
@@ -334,28 +306,7 @@ def write_outbound_messages_fixture(
output_path.write_text(json.dumps({"messages": messages}, indent=2))
def write_observer_fixture(output_path: Path) -> None:
"""Write minimal `observer_<fc_kind>_<host>.json` so `get_observer` succeeds.
The FT-P-01 scenario only consumes `wait_for_outbound`, but
`get_observer` still requires a valid observer fixture for
construction. Populate with safe defaults; per-scenario tests 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))
# `write_observer_fixture` is re-exported from `_common.py` (used by both b78 + b79).
# Orchestration
@@ -0,0 +1,291 @@
"""FT-P-02 Derkachi fixture builder (AZ-599).
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).
Differences from the b78 FT-P-01 builder (`build_p01_fixtures.py`):
* 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`.
"""
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 e2e.fixtures.sitl_replay_builder._common 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",
)
@dataclass(frozen=True)
class P02BuilderConfig:
"""Per-invocation Derkachi builder configuration."""
derkachi_dir: Path
output_dir: Path
fc_kind: str = "ardupilot"
host: str = "sitl-host"
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.
"""
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
def build_p02_fixtures(
cfg: P02BuilderConfig,
*,
_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,
)
run_gps_denied_replay(
mp4, tlog, fdr_jsonl, cli_bin=cfg.cli_bin, _runner=_runner,
)
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:
parser = argparse.ArgumentParser(
prog="build_p02_fixtures",
description="Build FT-P-02 Derkachi replay fixtures via gps-denied-replay.",
)
parser.add_argument("--derkachi-dir", type=Path, required=True,
help="Directory containing flight_derkachi.mp4 + data_imu.csv")
parser.add_argument("--output-dir", type=Path, required=True,
help="Output dir for derkachi.tlog + fdr/ archive + observer fixture")
parser.add_argument("--fc-kind", choices=("ardupilot", "inav"), default="ardupilot")
parser.add_argument("--host", default="sitl-host")
parser.add_argument("--cli-bin", default=DEFAULT_CLI_BIN)
args = parser.parse_args(argv)
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,
)
build_p02_fixtures(cfg)
return 0
if __name__ == "__main__": # pragma: no cover
sys.exit(_main())