mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-06-23 03:41:12 +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:
Oleksandr Bezdieniezhnykh
@@ -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())
|
||||
Reference in New Issue
Block a user