mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-06-22 10:51:13 +00:00
[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:
@@ -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
|
||||
@@ -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",
|
||||
|
||||
Reference in New Issue
Block a user