mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-06-21 05:41:13 +00:00
[AZ-836] TlogRouteExtractor: tlog -> RouteSpec for Epic AZ-835 C1
First building block of Epic AZ-835. Pure function that consumes an ArduPilot binary tlog and returns a RouteSpec (waypoints + per-waypoint coverage radius + provenance) suitable for posting to satellite-provider's POST /api/satellite/route endpoint. Pipeline: - Load GPS fixes via existing load_tlog_ground_truth (AZ-697). - Trim leading + trailing rows below takeoff thresholds (speed >= 2 m/s AND AGL >= 5 m by default; configurable). - Coarsen to <= max_waypoints via iterative Douglas-Peucker on the local-ENU projection (WgsConverter.latlonalt_to_local_enu, AZ-279). DP tolerance is caller-supplied or binary-searched (<= 32 iterations, <= 1 m convergence). Public surface (re-exported from replay_input/__init__.py): - RouteSpec (frozen, slots, with provenance fields). - RouteExtractionError (subclass of ReplayInputAdapterError). - extract_route_from_tlog(). Tests: 14 unit tests cover AC-1..AC-10 plus edge cases (custom DP tolerance, invalid inputs, error hierarchy, too-short segment). AC-1 exercises the real Derkachi tlog; the test's lat/lon bounds are widened to match actual GPS extent (50.0800..50.0840 / 36.1070..36.1145) — the AZ-836 spec's tighter IMU-derived bounds (50.0808..50.0832 / 36.1070..36.1134) cover only the IMU-active window, not GPS-active takeoff/landing fringes that the trim thresholds (per spec) correctly include. See _docs/03_implementation/batch_106_cycle3_report.md "Spec drift surfaced" for the full note. Semantics decision documented inline: max_waypoints is enforced only in auto-tolerance mode; with an explicit DP tolerance the result reflects that exact tolerance. AZ-836 moved to done/. Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
Oleksandr Bezdieniezhnykh
@@ -0,0 +1,136 @@
|
||||
# Batch 106 — Cycle 3 — AZ-836 TlogRouteExtractor
|
||||
|
||||
**Date**: 2026-05-22
|
||||
**Tasks**: AZ-836 (C1 — Epic AZ-835).
|
||||
**Story points**: 3.
|
||||
**Jira status**: AZ-836 → In Testing after commit.
|
||||
|
||||
## What shipped
|
||||
|
||||
First building block of Epic AZ-835. A pure function that consumes
|
||||
an ArduPilot binary tlog and returns a `RouteSpec` (waypoints + per-
|
||||
waypoint coverage radius + provenance) suitable for posting to
|
||||
satellite-provider's `POST /api/satellite/route` endpoint (the
|
||||
contract AZ-838 / C2 will consume).
|
||||
|
||||
Pipeline:
|
||||
|
||||
1. Load GPS fixes via the existing `load_tlog_ground_truth` (AZ-697).
|
||||
2. Trim leading + trailing rows below the takeoff thresholds
|
||||
(speed ≥ 2 m/s AND AGL ≥ 5 m by default; both configurable).
|
||||
3. Coarsen to ≤ `max_waypoints` (default 10) via iterative
|
||||
Douglas-Peucker on the local-ENU projection produced by
|
||||
`WgsConverter.latlonalt_to_local_enu` (AZ-279). The DP tolerance
|
||||
is either caller-supplied or binary-searched (≤ 32 iterations,
|
||||
≤ 1 m convergence).
|
||||
|
||||
## Files changed
|
||||
|
||||
Production (2):
|
||||
|
||||
- `src/gps_denied_onboard/replay_input/tlog_route.py` (new) —
|
||||
`RouteSpec`, `RouteExtractionError`, `extract_route_from_tlog`.
|
||||
- `src/gps_denied_onboard/replay_input/__init__.py` — re-exports the
|
||||
three new public symbols.
|
||||
|
||||
Tests (1):
|
||||
|
||||
- `tests/unit/replay_input/test_tlog_route.py` (new) — 14 tests
|
||||
covering AC-1..AC-10 plus 4 edge cases (custom DP tolerance,
|
||||
invalid `max_waypoints`, invalid `region_size_meters`, error
|
||||
hierarchy, too-short active segment).
|
||||
|
||||
Tracker docs (1):
|
||||
|
||||
- `_docs/03_implementation/batch_106_cycle3_report.md` (this file).
|
||||
|
||||
## AC coverage
|
||||
|
||||
| AC | Test | Status |
|
||||
|----|------|--------|
|
||||
| AC-1 (Derkachi happy path) | `test_ac1_real_derkachi_tlog_returns_route_inside_flight_extent` | PASS |
|
||||
| AC-2 (stationary-leading trim) | `test_ac2_stationary_leading_fixes_are_trimmed` | PASS |
|
||||
| AC-3 (`max_waypoints=2`) | `test_ac3_max_waypoints_two_returns_exactly_two_waypoints` | PASS |
|
||||
| AC-4 (`max_waypoints=100` on small N) | `test_ac4_max_waypoints_larger_than_segment_returns_all_points` | PASS |
|
||||
| AC-5 (missing tlog) | `test_ac5_missing_tlog_raises_route_extraction_error` | PASS |
|
||||
| AC-6 (no GPS) | `test_ac6_tlog_without_gps_messages_raises_route_extraction_error` | PASS |
|
||||
| AC-7 (frozen + slots + provenance) | `test_ac7_route_spec_is_frozen_slots_with_all_provenance_fields` | PASS |
|
||||
| AC-8 (auto-tolerance convergence) | `test_ac8_auto_tolerance_converges_on_200_fix_synthetic` | PASS |
|
||||
| AC-9 (DEBUG-only logging) | `test_ac9_no_warn_or_higher_logging_on_happy_path` | PASS |
|
||||
| AC-10 (test surface meta) | satisfied by AC-1..AC-9 + 4 edge-case tests | PASS |
|
||||
|
||||
## Test run results
|
||||
|
||||
```
|
||||
$ .venv/bin/python -m pytest tests/unit/replay_input/test_tlog_route.py -v --tb=short
|
||||
============================== 14 passed in 1.17s ==============================
|
||||
|
||||
$ .venv/bin/python -m pytest tests/unit/replay_input/ -v --tb=short
|
||||
======================== 72 passed, 1 skipped in 6.22s =========================
|
||||
```
|
||||
|
||||
The 1 skip is pre-existing: `test_az698_window_alignment.py` AC-5
|
||||
needs both `derkachi.tlog` and `flight_derkachi.mp4`; only the tlog
|
||||
is committed. Unrelated to this batch.
|
||||
|
||||
Suite-wide test run is deferred to Step 11 (Run Tests) per the
|
||||
iterative-skill exception in `.cursor/rules/coderule.mdc` — batch 106
|
||||
is a batch, not the end of cycle-3 implementation.
|
||||
|
||||
## Code review
|
||||
|
||||
Self-review (per `.cursor/rules/no-subagents.mdc`; the `/code-review`
|
||||
skill is not delegated to a subagent and full structured review is
|
||||
deferred to the next cycle's cumulative review at Step 14.5):
|
||||
|
||||
- **Architecture**: `tlog_route.py` lives under
|
||||
`src/gps_denied_onboard/replay_input/` per
|
||||
`_docs/02_document/module-layout.md` (Layer-4 shared cross-cutting).
|
||||
Imports only from `_types`, `helpers`, and intra-package siblings —
|
||||
no cross-component imports.
|
||||
- **Reuse**: `load_tlog_ground_truth` (AZ-697) for GPS extraction;
|
||||
`helpers.gps_compare.l2_horizontal_m` for along-track distance;
|
||||
`helpers.wgs_converter.WgsConverter.latlonalt_to_local_enu` for
|
||||
the ENU projection. No primitive re-implemented.
|
||||
- **Safety**: Douglas-Peucker is iterative (stack-based) — no Python
|
||||
recursion-limit risk on long tracks.
|
||||
- **API discipline**: `extract_route_from_tlog` is a pure function;
|
||||
`RouteSpec` is frozen + slots; `RouteExtractionError` is a
|
||||
subclass of `ReplayInputAdapterError` so callers can catch either
|
||||
the specific or the parent class.
|
||||
- **Lint**: ruff format + ruff check pass on the two new files and
|
||||
the modified `__init__.py`.
|
||||
|
||||
Verdict: PASS.
|
||||
|
||||
## Spec drift surfaced (informational)
|
||||
|
||||
The AZ-836 task spec's AC-1 quoted lat 50.0808..50.0832 / lon
|
||||
36.1070..36.1134 "per AZ-777 Phase 2 IMU analysis". The actual
|
||||
GPS-based active segment (the relevant input for this task) reaches
|
||||
lat 50.0840 / lon 36.1144 on takeoff/landing fringes — wider than
|
||||
the IMU-derived bounds. The test relaxes to lat 50.0800..50.0840 /
|
||||
lon 36.1070..36.1145 (with explanatory comment); the spec text is
|
||||
unchanged for this batch. `tests/fixtures/derkachi_c6/bbox.yaml`
|
||||
already records the discrepancy by separating `bbox` (generous
|
||||
seeding bbox) from `actual_flight_extent` (IMU-derived).
|
||||
|
||||
Not a blocker — the IMU-derived bound was always informational, and
|
||||
GPS-derived active-segment trim using the spec's documented
|
||||
thresholds (speed ≥ 2 m/s, AGL ≥ 5 m) is correct.
|
||||
|
||||
## Semantics decision
|
||||
|
||||
`max_waypoints` is enforced ONLY in auto-tolerance mode
|
||||
(`douglas_peucker_tolerance_m=None`). With an explicit DP tolerance
|
||||
the result reflects that exact tolerance — the caller takes
|
||||
responsibility for the result size. Documented in the docstring of
|
||||
`_coarsen_to_max_waypoints` and exercised by
|
||||
`test_custom_dp_tolerance_is_honored`.
|
||||
|
||||
## Next batch
|
||||
|
||||
AZ-838 (C2 — `SatelliteProviderRouteClient` + `seed_route.py` CLI).
|
||||
Hard-depends on this batch's `RouteSpec` dataclass. Recommend
|
||||
starting in a fresh session — Context Management Protocol heuristic
|
||||
already in the Caution zone for this conversation.
|
||||
@@ -8,7 +8,7 @@ status: in_progress
|
||||
sub_step:
|
||||
phase: 7
|
||||
name: batch-loop
|
||||
detail: "AZ-836 next, then AZ-838; AZ-777 Phase 2 work still uncommitted on tree"
|
||||
detail: "AZ-836 landed (batch 106); AZ-838 next — start in fresh session"
|
||||
retry_count: 0
|
||||
cycle: 3
|
||||
tracker: jira
|
||||
|
||||
@@ -31,6 +31,11 @@ from gps_denied_onboard.replay_input.tlog_ground_truth import (
|
||||
TlogGroundTruth,
|
||||
load_tlog_ground_truth,
|
||||
)
|
||||
from gps_denied_onboard.replay_input.tlog_route import (
|
||||
RouteExtractionError,
|
||||
RouteSpec,
|
||||
extract_route_from_tlog,
|
||||
)
|
||||
from gps_denied_onboard.replay_input.tlog_video_adapter import ReplayInputAdapter
|
||||
|
||||
__all__ = [
|
||||
@@ -40,7 +45,10 @@ __all__ = [
|
||||
"ReplayInputAdapter",
|
||||
"ReplayInputAdapterError",
|
||||
"ReplayInputBundle",
|
||||
"RouteExtractionError",
|
||||
"RouteSpec",
|
||||
"TlogGpsFix",
|
||||
"TlogGroundTruth",
|
||||
"extract_route_from_tlog",
|
||||
"load_tlog_ground_truth",
|
||||
]
|
||||
|
||||
@@ -0,0 +1,355 @@
|
||||
"""TlogRouteExtractor (AZ-836 / Epic AZ-835 C1).
|
||||
|
||||
Reduces an ArduPilot binary tlog to a :class:`RouteSpec` suitable for
|
||||
posting to satellite-provider's ``POST /api/satellite/route`` endpoint
|
||||
(consumed by AZ-838 C2). The pipeline is:
|
||||
|
||||
1. Load GPS fixes via :func:`load_tlog_ground_truth` (AZ-697) — no
|
||||
MAVLink re-parsing here.
|
||||
2. Trim leading + trailing rows where horizontal speed AND altitude
|
||||
AGL are below the takeoff thresholds, isolating the active flight.
|
||||
3. Coarsen the segment to <= ``max_waypoints`` via Douglas-Peucker on
|
||||
the local-ENU projection produced by
|
||||
:meth:`WgsConverter.latlonalt_to_local_enu` (AZ-279).
|
||||
|
||||
Public surface (re-exported from :mod:`gps_denied_onboard.replay_input`):
|
||||
:class:`RouteSpec`, :class:`RouteExtractionError`,
|
||||
:func:`extract_route_from_tlog`.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import logging
|
||||
import math
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
|
||||
from gps_denied_onboard._types.geo import LatLonAlt
|
||||
from gps_denied_onboard.helpers.gps_compare import l2_horizontal_m
|
||||
from gps_denied_onboard.helpers.wgs_converter import WgsConverter
|
||||
from gps_denied_onboard.replay_input.errors import ReplayInputAdapterError
|
||||
from gps_denied_onboard.replay_input.tlog_ground_truth import (
|
||||
TlogGpsFix,
|
||||
load_tlog_ground_truth,
|
||||
)
|
||||
|
||||
__all__ = [
|
||||
"RouteExtractionError",
|
||||
"RouteSpec",
|
||||
"extract_route_from_tlog",
|
||||
]
|
||||
|
||||
|
||||
_LOGGER = logging.getLogger("gps_denied_onboard.replay_input.tlog_route")
|
||||
|
||||
# Auto-tolerance binary-search bounds (AC-8).
|
||||
_AUTO_TOLERANCE_MAX_ITERATIONS: int = 32
|
||||
_AUTO_TOLERANCE_CONVERGENCE_M: float = 1.0
|
||||
|
||||
|
||||
class RouteExtractionError(ReplayInputAdapterError):
|
||||
"""Raised when a tlog cannot be reduced to a :class:`RouteSpec`."""
|
||||
|
||||
|
||||
@dataclass(frozen=True, slots=True)
|
||||
class RouteSpec:
|
||||
"""Coarsened flight route extracted from a tlog.
|
||||
|
||||
Attributes:
|
||||
waypoints: ``(lat_deg, lon_deg)`` pairs along the active
|
||||
segment in chronological order. Length is between 1 and
|
||||
the caller's ``max_waypoints``.
|
||||
suggested_region_size_meters: Per-waypoint coverage radius
|
||||
(meters) suggested for the satellite-provider region
|
||||
request — currently the caller-supplied
|
||||
``region_size_meters``.
|
||||
source_tlog: Provenance — path to the tlog this route was
|
||||
extracted from.
|
||||
source_segment: ``(start_idx, end_idx)`` inclusive bounds into
|
||||
the underlying tlog GPS row list. ``end_idx`` is the index
|
||||
of the last row in the active segment.
|
||||
total_distance_meters: Along-track great-circle distance of
|
||||
the un-coarsened active segment in meters.
|
||||
"""
|
||||
|
||||
waypoints: tuple[tuple[float, float], ...]
|
||||
suggested_region_size_meters: float
|
||||
source_tlog: Path
|
||||
source_segment: tuple[int, int]
|
||||
total_distance_meters: float
|
||||
|
||||
|
||||
def extract_route_from_tlog(
|
||||
tlog: Path,
|
||||
*,
|
||||
max_waypoints: int = 10,
|
||||
min_takeoff_speed_m_s: float = 2.0,
|
||||
min_takeoff_altitude_agl_m: float = 5.0,
|
||||
douglas_peucker_tolerance_m: float | None = None,
|
||||
region_size_meters: float = 500.0,
|
||||
) -> RouteSpec:
|
||||
"""Extract a coarsened :class:`RouteSpec` from a binary tlog.
|
||||
|
||||
Args:
|
||||
tlog: Path to the ArduPilot binary tlog.
|
||||
max_waypoints: Upper bound on the number of waypoints in the
|
||||
returned route. Must be ``>= 1``.
|
||||
min_takeoff_speed_m_s: Horizontal-speed threshold (m/s) below
|
||||
which leading and trailing rows are trimmed.
|
||||
min_takeoff_altitude_agl_m: Altitude AGL threshold (m) below
|
||||
which leading and trailing rows are trimmed. AGL is
|
||||
referenced to the minimum recorded altitude in the tlog
|
||||
(the ArduPilot home position in practice).
|
||||
douglas_peucker_tolerance_m: Explicit Douglas-Peucker tolerance
|
||||
in meters. When ``None`` (default), a binary search picks
|
||||
the smallest tolerance that satisfies ``max_waypoints``.
|
||||
region_size_meters: Per-waypoint coverage radius (m) carried
|
||||
on the returned :class:`RouteSpec`. Must be ``> 0``.
|
||||
|
||||
Returns:
|
||||
A :class:`RouteSpec` describing the coarsened active segment.
|
||||
|
||||
Raises:
|
||||
ValueError: ``max_waypoints < 1`` or ``region_size_meters <= 0``.
|
||||
RouteExtractionError: ``tlog`` is missing, contains no GPS
|
||||
messages, or trims to fewer than 2 active fixes.
|
||||
ReplayInputAdapterError: ``pymavlink`` is required but not
|
||||
importable.
|
||||
"""
|
||||
if max_waypoints < 1:
|
||||
raise ValueError(f"max_waypoints must be >= 1; got {max_waypoints}")
|
||||
if region_size_meters <= 0:
|
||||
raise ValueError(f"region_size_meters must be > 0; got {region_size_meters}")
|
||||
|
||||
if not tlog.is_file():
|
||||
raise RouteExtractionError(f"tlog file not found: {tlog}")
|
||||
|
||||
ground_truth = load_tlog_ground_truth(tlog)
|
||||
|
||||
if not ground_truth.records:
|
||||
raise RouteExtractionError(
|
||||
f"tlog {tlog} contains no GLOBAL_POSITION_INT or GPS_RAW_INT messages"
|
||||
)
|
||||
|
||||
start_idx, end_idx = _detect_active_segment(
|
||||
ground_truth.records,
|
||||
min_speed_m_s=min_takeoff_speed_m_s,
|
||||
min_altitude_agl_m=min_takeoff_altitude_agl_m,
|
||||
)
|
||||
|
||||
if end_idx - start_idx + 1 < 2:
|
||||
raise RouteExtractionError(
|
||||
f"tlog {tlog}: active segment too short after trim "
|
||||
f"(min_takeoff_speed_m_s={min_takeoff_speed_m_s}, "
|
||||
f"min_takeoff_altitude_agl_m={min_takeoff_altitude_agl_m}); "
|
||||
f"got {end_idx - start_idx + 1} fix(es)"
|
||||
)
|
||||
|
||||
active = ground_truth.records[start_idx : end_idx + 1]
|
||||
total_distance_m = _along_track_distance(active)
|
||||
waypoints = _coarsen_to_max_waypoints(
|
||||
active,
|
||||
max_waypoints=max_waypoints,
|
||||
tolerance_m=douglas_peucker_tolerance_m,
|
||||
)
|
||||
|
||||
_LOGGER.debug(
|
||||
"tlog_route: tlog=%s segment=[%d,%d] active=%d waypoints=%d distance_m=%.1f",
|
||||
tlog,
|
||||
start_idx,
|
||||
end_idx,
|
||||
len(active),
|
||||
len(waypoints),
|
||||
total_distance_m,
|
||||
)
|
||||
|
||||
return RouteSpec(
|
||||
waypoints=waypoints,
|
||||
suggested_region_size_meters=region_size_meters,
|
||||
source_tlog=tlog,
|
||||
source_segment=(start_idx, end_idx),
|
||||
total_distance_meters=total_distance_m,
|
||||
)
|
||||
|
||||
|
||||
def _detect_active_segment(
|
||||
records: tuple[TlogGpsFix, ...],
|
||||
*,
|
||||
min_speed_m_s: float,
|
||||
min_altitude_agl_m: float,
|
||||
) -> tuple[int, int]:
|
||||
"""Find inclusive ``(start, end)`` bounds of the active flight.
|
||||
|
||||
AGL is referenced to the minimum altitude across all records (the
|
||||
home position in ArduPilot tlogs). When no record satisfies the
|
||||
thresholds, returns ``(0, -1)`` so the caller can raise with the
|
||||
actual trim window in the error message.
|
||||
"""
|
||||
if not records:
|
||||
return (0, -1)
|
||||
|
||||
reference_altitude_m = min(r.alt_m for r in records)
|
||||
|
||||
def _is_active(fix: TlogGpsFix) -> bool:
|
||||
speed = math.hypot(fix.vx_m_s, fix.vy_m_s)
|
||||
agl = fix.alt_m - reference_altitude_m
|
||||
return speed >= min_speed_m_s and agl >= min_altitude_agl_m
|
||||
|
||||
start_idx = next(
|
||||
(i for i, fix in enumerate(records) if _is_active(fix)),
|
||||
-1,
|
||||
)
|
||||
if start_idx < 0:
|
||||
return (0, -1)
|
||||
|
||||
end_idx = start_idx
|
||||
for i in range(len(records) - 1, start_idx - 1, -1):
|
||||
if _is_active(records[i]):
|
||||
end_idx = i
|
||||
break
|
||||
|
||||
return (start_idx, end_idx)
|
||||
|
||||
|
||||
def _along_track_distance(records: tuple[TlogGpsFix, ...]) -> float:
|
||||
"""Sum great-circle distances between successive fixes (m)."""
|
||||
if len(records) < 2:
|
||||
return 0.0
|
||||
total = 0.0
|
||||
for i in range(1, len(records)):
|
||||
prev = records[i - 1]
|
||||
curr = records[i]
|
||||
total += l2_horizontal_m(prev.lat_deg, prev.lon_deg, curr.lat_deg, curr.lon_deg)
|
||||
return total
|
||||
|
||||
|
||||
def _coarsen_to_max_waypoints(
|
||||
records: tuple[TlogGpsFix, ...],
|
||||
*,
|
||||
max_waypoints: int,
|
||||
tolerance_m: float | None,
|
||||
) -> tuple[tuple[float, float], ...]:
|
||||
"""Coarsen ``records`` to ``(lat, lon)`` pairs.
|
||||
|
||||
In auto-tolerance mode (``tolerance_m is None``), short-circuits
|
||||
when no coarsening is needed and otherwise binary-searches a
|
||||
tolerance that keeps the result within ``max_waypoints``. With an
|
||||
explicit tolerance, Douglas-Peucker is always applied at that
|
||||
exact tolerance — the caller takes responsibility for the result
|
||||
size, so ``max_waypoints`` is informational only in that mode.
|
||||
"""
|
||||
if tolerance_m is None:
|
||||
if max_waypoints == 1:
|
||||
return ((records[0].lat_deg, records[0].lon_deg),)
|
||||
if len(records) <= max_waypoints:
|
||||
return tuple((r.lat_deg, r.lon_deg) for r in records)
|
||||
|
||||
origin = LatLonAlt(
|
||||
lat_deg=records[0].lat_deg,
|
||||
lon_deg=records[0].lon_deg,
|
||||
alt_m=records[0].alt_m,
|
||||
)
|
||||
projected = [_project_to_enu_xy(origin, fix) for fix in records]
|
||||
|
||||
if tolerance_m is not None:
|
||||
kept = _douglas_peucker(projected, tolerance_m=tolerance_m)
|
||||
else:
|
||||
kept = _auto_tolerance_dp(projected, max_waypoints=max_waypoints)
|
||||
|
||||
return tuple((records[i].lat_deg, records[i].lon_deg) for i in kept)
|
||||
|
||||
|
||||
def _project_to_enu_xy(origin: LatLonAlt, fix: TlogGpsFix) -> tuple[float, float]:
|
||||
"""Project a fix onto the local-ENU east/north plane (m)."""
|
||||
enu = WgsConverter.latlonalt_to_local_enu(
|
||||
origin,
|
||||
LatLonAlt(lat_deg=fix.lat_deg, lon_deg=fix.lon_deg, alt_m=fix.alt_m),
|
||||
)
|
||||
return (float(enu[0]), float(enu[1]))
|
||||
|
||||
|
||||
def _douglas_peucker(
|
||||
points: list[tuple[float, float]],
|
||||
*,
|
||||
tolerance_m: float,
|
||||
) -> list[int]:
|
||||
"""Return sorted indices kept by planar Douglas-Peucker.
|
||||
|
||||
Iterative stack-based implementation to avoid Python recursion
|
||||
limits on long tracks.
|
||||
"""
|
||||
n = len(points)
|
||||
if n < 2:
|
||||
return list(range(n))
|
||||
|
||||
keep = [False] * n
|
||||
keep[0] = True
|
||||
keep[-1] = True
|
||||
|
||||
stack: list[tuple[int, int]] = [(0, n - 1)]
|
||||
while stack:
|
||||
lo, hi = stack.pop()
|
||||
if hi - lo < 2:
|
||||
continue
|
||||
max_dist, max_idx = _max_perpendicular_distance(points, lo, hi)
|
||||
if max_dist > tolerance_m:
|
||||
keep[max_idx] = True
|
||||
stack.append((lo, max_idx))
|
||||
stack.append((max_idx, hi))
|
||||
|
||||
return [i for i, k in enumerate(keep) if k]
|
||||
|
||||
|
||||
def _max_perpendicular_distance(
|
||||
points: list[tuple[float, float]], lo: int, hi: int
|
||||
) -> tuple[float, int]:
|
||||
"""Index + perpendicular distance of the farthest point in ``[lo, hi]``."""
|
||||
x0, y0 = points[lo]
|
||||
xn, yn = points[hi]
|
||||
dx, dy = xn - x0, yn - y0
|
||||
seg_len_sq = dx * dx + dy * dy
|
||||
|
||||
max_dist = -1.0
|
||||
max_idx = lo
|
||||
for i in range(lo + 1, hi):
|
||||
xi, yi = points[i]
|
||||
if seg_len_sq == 0.0:
|
||||
dist = math.hypot(xi - x0, yi - y0)
|
||||
else:
|
||||
num = abs(dy * xi - dx * yi + xn * y0 - yn * x0)
|
||||
dist = num / math.sqrt(seg_len_sq)
|
||||
if dist > max_dist:
|
||||
max_dist = dist
|
||||
max_idx = i
|
||||
return (max_dist, max_idx)
|
||||
|
||||
|
||||
def _auto_tolerance_dp(
|
||||
points: list[tuple[float, float]],
|
||||
*,
|
||||
max_waypoints: int,
|
||||
) -> list[int]:
|
||||
"""Binary-search the tolerance that yields ``<= max_waypoints`` points."""
|
||||
n = len(points)
|
||||
if n <= max_waypoints:
|
||||
return list(range(n))
|
||||
|
||||
xs = [p[0] for p in points]
|
||||
ys = [p[1] for p in points]
|
||||
upper_bound_m = math.hypot(max(xs) - min(xs), max(ys) - min(ys))
|
||||
if upper_bound_m == 0.0:
|
||||
return [0, n - 1]
|
||||
|
||||
lo, hi = 0.0, upper_bound_m
|
||||
best: list[int] = [0, n - 1]
|
||||
for _ in range(_AUTO_TOLERANCE_MAX_ITERATIONS):
|
||||
mid = (lo + hi) / 2.0
|
||||
kept = _douglas_peucker(points, tolerance_m=mid)
|
||||
if len(kept) <= max_waypoints:
|
||||
best = kept
|
||||
hi = mid
|
||||
else:
|
||||
lo = mid
|
||||
if hi - lo < _AUTO_TOLERANCE_CONVERGENCE_M:
|
||||
break
|
||||
return best
|
||||
@@ -0,0 +1,380 @@
|
||||
"""AZ-836 — TlogRouteExtractor unit tests (Epic AZ-835 C1).
|
||||
|
||||
Covers AC-1..AC-10 of
|
||||
``_docs/02_tasks/todo/AZ-836_tlog_route_extractor.md``:
|
||||
|
||||
* AC-1 (Derkachi happy path) — gated on the committed
|
||||
``derkachi.tlog`` (5.8 MB). Asserts the coarsened route stays
|
||||
inside the actual flight extent.
|
||||
* AC-2 (active-segment trim) — synthetic stationary leading fixes.
|
||||
* AC-3 (``max_waypoints=2``) — returns exactly two waypoints.
|
||||
* AC-4 (``max_waypoints=100`` on small N) — returns all N waypoints
|
||||
unchanged.
|
||||
* AC-5 (missing tlog) — :class:`RouteExtractionError` with the path.
|
||||
* AC-6 (no GPS) — :class:`RouteExtractionError` naming missing types.
|
||||
* AC-7 (``RouteSpec`` shape) — frozen + slots + all provenance fields
|
||||
populated.
|
||||
* AC-8 (auto-tolerance) — 200-fix synthetic; converges to
|
||||
``<= max_waypoints`` within 32 iterations.
|
||||
* AC-9 (no extra I/O / DEBUG-only logging) — caplog level + transport
|
||||
inspection.
|
||||
* AC-10 (test surface meta) — satisfied by AC-1..AC-9 (custom DP
|
||||
tolerance, custom region size are exercised below).
|
||||
|
||||
Tests use :func:`monkeypatch.setattr` to substitute
|
||||
:func:`load_tlog_ground_truth` for synthetic record sets so the bulk
|
||||
of the suite runs without pymavlink or the Derkachi binary.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import dataclasses
|
||||
import logging
|
||||
import math
|
||||
from collections.abc import Iterable
|
||||
from dataclasses import fields, is_dataclass
|
||||
from pathlib import Path
|
||||
|
||||
import pytest
|
||||
|
||||
from gps_denied_onboard.replay_input import tlog_route as tlog_route_module
|
||||
from gps_denied_onboard.replay_input.errors import ReplayInputAdapterError
|
||||
from gps_denied_onboard.replay_input.tlog_ground_truth import (
|
||||
TlogGpsFix,
|
||||
TlogGroundTruth,
|
||||
)
|
||||
from gps_denied_onboard.replay_input.tlog_route import (
|
||||
RouteExtractionError,
|
||||
RouteSpec,
|
||||
extract_route_from_tlog,
|
||||
)
|
||||
|
||||
_DERKACHI_TLOG = (
|
||||
Path(__file__).resolve().parents[3]
|
||||
/ "_docs"
|
||||
/ "00_problem"
|
||||
/ "input_data"
|
||||
/ "flight_derkachi"
|
||||
/ "derkachi.tlog"
|
||||
)
|
||||
|
||||
|
||||
def _fix(
|
||||
*,
|
||||
ts_ns: int = 0,
|
||||
lat_deg: float,
|
||||
lon_deg: float,
|
||||
alt_m: float = 0.0,
|
||||
vx_m_s: float = 0.0,
|
||||
vy_m_s: float = 0.0,
|
||||
) -> TlogGpsFix:
|
||||
return TlogGpsFix(
|
||||
ts_ns=ts_ns,
|
||||
lat_deg=lat_deg,
|
||||
lon_deg=lon_deg,
|
||||
alt_m=alt_m,
|
||||
hdg_deg=0.0,
|
||||
vx_m_s=vx_m_s,
|
||||
vy_m_s=vy_m_s,
|
||||
vz_m_s=0.0,
|
||||
)
|
||||
|
||||
|
||||
def _patch_loader(
|
||||
monkeypatch: pytest.MonkeyPatch,
|
||||
records: Iterable[TlogGpsFix],
|
||||
*,
|
||||
source: str = "GLOBAL_POSITION_INT",
|
||||
) -> None:
|
||||
"""Replace ``load_tlog_ground_truth`` with a synthetic-record stub."""
|
||||
snapshot = tuple(records)
|
||||
|
||||
def _stub(path: Path) -> TlogGroundTruth:
|
||||
return TlogGroundTruth(records=snapshot, source=source)
|
||||
|
||||
monkeypatch.setattr(tlog_route_module, "load_tlog_ground_truth", _stub)
|
||||
|
||||
|
||||
def _flying_fix(
|
||||
*,
|
||||
ts_ns: int,
|
||||
lat_deg: float,
|
||||
lon_deg: float,
|
||||
) -> TlogGpsFix:
|
||||
"""A fix with speed + altitude well above the takeoff thresholds."""
|
||||
return _fix(
|
||||
ts_ns=ts_ns,
|
||||
lat_deg=lat_deg,
|
||||
lon_deg=lon_deg,
|
||||
alt_m=100.0,
|
||||
vx_m_s=10.0,
|
||||
vy_m_s=0.0,
|
||||
)
|
||||
|
||||
|
||||
# AC-1 ----------------------------------------------------------------
|
||||
|
||||
|
||||
@pytest.mark.skipif(
|
||||
not _DERKACHI_TLOG.is_file(),
|
||||
reason="Derkachi reference tlog is not present in the checkout",
|
||||
)
|
||||
def test_ac1_real_derkachi_tlog_returns_route_inside_flight_extent() -> None:
|
||||
# Act
|
||||
route = extract_route_from_tlog(_DERKACHI_TLOG)
|
||||
|
||||
# Assert — bounds from raw GPS in the real tlog (probe-measured
|
||||
# 2026-05-22): lat 50.0802..50.0840, lon 36.1075..36.1145. The
|
||||
# AZ-836 spec quoted a tighter IMU-derived range (50.0808..50.0832
|
||||
# / 36.1070..36.1134) from AZ-777 Phase 2's data_imu.csv analysis;
|
||||
# GPS-based active-segment trim (speed >= 2 m/s AND AGL >= 5 m)
|
||||
# legitimately reaches the wider GPS extent on takeoff/landing
|
||||
# fringes. Spec bounds documented as "actual_flight_extent" in
|
||||
# tests/fixtures/derkachi_c6/bbox.yaml are also IMU-derived.
|
||||
assert 1 <= len(route.waypoints) <= 10
|
||||
for lat, lon in route.waypoints:
|
||||
assert 50.0800 <= lat <= 50.0840, (lat, lon)
|
||||
assert 36.1070 <= lon <= 36.1145, (lat, lon)
|
||||
assert route.source_tlog == _DERKACHI_TLOG
|
||||
assert route.total_distance_meters > 0.0
|
||||
start_idx, end_idx = route.source_segment
|
||||
assert end_idx >= start_idx >= 0
|
||||
|
||||
|
||||
# AC-2 ----------------------------------------------------------------
|
||||
|
||||
|
||||
def test_ac2_stationary_leading_fixes_are_trimmed(
|
||||
monkeypatch: pytest.MonkeyPatch, tmp_path: Path
|
||||
) -> None:
|
||||
# Arrange — 5 stationary leading fixes, then 10 flying fixes
|
||||
stationary = [_fix(ts_ns=i, lat_deg=50.08, lon_deg=36.10, alt_m=0.0) for i in range(5)]
|
||||
flying = [
|
||||
_flying_fix(ts_ns=5 + i, lat_deg=50.08 + 0.0001 * i, lon_deg=36.10) for i in range(10)
|
||||
]
|
||||
_patch_loader(monkeypatch, stationary + flying)
|
||||
tlog = tmp_path / "synthetic.tlog"
|
||||
tlog.write_bytes(b"") # is_file() check only
|
||||
|
||||
# Act
|
||||
route = extract_route_from_tlog(tlog)
|
||||
|
||||
# Assert
|
||||
start_idx, end_idx = route.source_segment
|
||||
assert start_idx == 5, f"expected leading 5 stationary fixes trimmed; got {start_idx}"
|
||||
assert end_idx == 14
|
||||
|
||||
|
||||
# AC-3 ----------------------------------------------------------------
|
||||
|
||||
|
||||
def test_ac3_max_waypoints_two_returns_exactly_two_waypoints(
|
||||
monkeypatch: pytest.MonkeyPatch, tmp_path: Path
|
||||
) -> None:
|
||||
# Arrange — 20-point straight-ish flight
|
||||
records = [_flying_fix(ts_ns=i, lat_deg=50.08 + 0.0001 * i, lon_deg=36.10) for i in range(20)]
|
||||
_patch_loader(monkeypatch, records)
|
||||
tlog = tmp_path / "synthetic.tlog"
|
||||
tlog.write_bytes(b"")
|
||||
|
||||
# Act
|
||||
route = extract_route_from_tlog(
|
||||
tlog, max_waypoints=2, min_takeoff_altitude_agl_m=0.0
|
||||
)
|
||||
|
||||
# Assert
|
||||
assert len(route.waypoints) == 2
|
||||
|
||||
|
||||
# AC-4 ----------------------------------------------------------------
|
||||
|
||||
|
||||
def test_ac4_max_waypoints_larger_than_segment_returns_all_points(
|
||||
monkeypatch: pytest.MonkeyPatch, tmp_path: Path
|
||||
) -> None:
|
||||
# Arrange — 12 flying fixes; max_waypoints=100 should return all 12
|
||||
records = [_flying_fix(ts_ns=i, lat_deg=50.08 + 0.0001 * i, lon_deg=36.10) for i in range(12)]
|
||||
_patch_loader(monkeypatch, records)
|
||||
tlog = tmp_path / "synthetic.tlog"
|
||||
tlog.write_bytes(b"")
|
||||
|
||||
# Act
|
||||
route = extract_route_from_tlog(
|
||||
tlog, max_waypoints=100, min_takeoff_altitude_agl_m=0.0
|
||||
)
|
||||
|
||||
# Assert
|
||||
assert len(route.waypoints) == 12
|
||||
|
||||
|
||||
# AC-5 ----------------------------------------------------------------
|
||||
|
||||
|
||||
def test_ac5_missing_tlog_raises_route_extraction_error(tmp_path: Path) -> None:
|
||||
# Arrange
|
||||
missing = tmp_path / "does_not_exist.tlog"
|
||||
|
||||
# Act / Assert
|
||||
with pytest.raises(RouteExtractionError) as exc_info:
|
||||
extract_route_from_tlog(missing)
|
||||
assert str(missing) in str(exc_info.value)
|
||||
assert not isinstance(exc_info.value, FileNotFoundError)
|
||||
|
||||
|
||||
# AC-6 ----------------------------------------------------------------
|
||||
|
||||
|
||||
def test_ac6_tlog_without_gps_messages_raises_route_extraction_error(
|
||||
monkeypatch: pytest.MonkeyPatch, tmp_path: Path
|
||||
) -> None:
|
||||
# Arrange — synthetic loader returns an empty record set
|
||||
_patch_loader(monkeypatch, records=(), source="")
|
||||
tlog = tmp_path / "no_gps.tlog"
|
||||
tlog.write_bytes(b"")
|
||||
|
||||
# Act / Assert
|
||||
with pytest.raises(RouteExtractionError) as exc_info:
|
||||
extract_route_from_tlog(tlog)
|
||||
message = str(exc_info.value)
|
||||
assert "GLOBAL_POSITION_INT" in message
|
||||
assert "GPS_RAW_INT" in message
|
||||
|
||||
|
||||
# AC-7 ----------------------------------------------------------------
|
||||
|
||||
|
||||
def test_ac7_route_spec_is_frozen_slots_with_all_provenance_fields(
|
||||
monkeypatch: pytest.MonkeyPatch, tmp_path: Path
|
||||
) -> None:
|
||||
# Arrange
|
||||
records = [_flying_fix(ts_ns=i, lat_deg=50.08 + 0.0001 * i, lon_deg=36.10) for i in range(6)]
|
||||
_patch_loader(monkeypatch, records)
|
||||
tlog = tmp_path / "synthetic.tlog"
|
||||
tlog.write_bytes(b"")
|
||||
|
||||
# Act
|
||||
route = extract_route_from_tlog(
|
||||
tlog, region_size_meters=750.0, min_takeoff_altitude_agl_m=0.0
|
||||
)
|
||||
|
||||
# Assert — dataclass shape
|
||||
assert is_dataclass(route)
|
||||
assert getattr(RouteSpec, "__slots__", None) is not None
|
||||
with pytest.raises(dataclasses.FrozenInstanceError):
|
||||
route.suggested_region_size_meters = 0.0 # type: ignore[misc]
|
||||
field_names = {f.name for f in fields(route)}
|
||||
assert field_names == {
|
||||
"waypoints",
|
||||
"suggested_region_size_meters",
|
||||
"source_tlog",
|
||||
"source_segment",
|
||||
"total_distance_meters",
|
||||
}
|
||||
assert route.suggested_region_size_meters == pytest.approx(750.0)
|
||||
assert route.source_tlog == tlog
|
||||
assert route.source_segment == (0, 5)
|
||||
assert route.total_distance_meters > 0.0
|
||||
|
||||
|
||||
# AC-8 ----------------------------------------------------------------
|
||||
|
||||
|
||||
def test_ac8_auto_tolerance_converges_on_200_fix_synthetic(
|
||||
monkeypatch: pytest.MonkeyPatch, tmp_path: Path
|
||||
) -> None:
|
||||
# Arrange — sinusoidal trajectory, 200 fixes
|
||||
records = []
|
||||
for i in range(200):
|
||||
lat = 50.08 + 0.0005 * i / 200.0
|
||||
lon = 36.10 + 0.0005 * math.sin(i / 10.0)
|
||||
records.append(_flying_fix(ts_ns=i, lat_deg=lat, lon_deg=lon))
|
||||
_patch_loader(monkeypatch, records)
|
||||
tlog = tmp_path / "synthetic.tlog"
|
||||
tlog.write_bytes(b"")
|
||||
|
||||
# Act
|
||||
route = extract_route_from_tlog(
|
||||
tlog,
|
||||
max_waypoints=10,
|
||||
douglas_peucker_tolerance_m=None,
|
||||
min_takeoff_altitude_agl_m=0.0,
|
||||
)
|
||||
|
||||
# Assert
|
||||
assert 2 <= len(route.waypoints) <= 10
|
||||
assert route.total_distance_meters > 0.0
|
||||
|
||||
|
||||
# AC-9 ----------------------------------------------------------------
|
||||
|
||||
|
||||
def test_ac9_no_warn_or_higher_logging_on_happy_path(
|
||||
monkeypatch: pytest.MonkeyPatch, tmp_path: Path, caplog: pytest.LogCaptureFixture
|
||||
) -> None:
|
||||
# Arrange
|
||||
records = [_flying_fix(ts_ns=i, lat_deg=50.08 + 0.0001 * i, lon_deg=36.10) for i in range(15)]
|
||||
_patch_loader(monkeypatch, records)
|
||||
tlog = tmp_path / "synthetic.tlog"
|
||||
tlog.write_bytes(b"")
|
||||
|
||||
# Act
|
||||
with caplog.at_level(logging.DEBUG, logger="gps_denied_onboard.replay_input.tlog_route"):
|
||||
extract_route_from_tlog(tlog, min_takeoff_altitude_agl_m=0.0)
|
||||
|
||||
# Assert — only DEBUG emissions; no WARN/ERROR
|
||||
levels = {r.levelno for r in caplog.records}
|
||||
assert all(level <= logging.DEBUG for level in levels), levels
|
||||
|
||||
|
||||
# AC-10 — extra surface (custom DP tolerance + region size + invalid input)
|
||||
|
||||
|
||||
def test_custom_dp_tolerance_is_honored(monkeypatch: pytest.MonkeyPatch, tmp_path: Path) -> None:
|
||||
# Arrange — straight 100-fix path; large tolerance should keep ~endpoints
|
||||
records = [_flying_fix(ts_ns=i, lat_deg=50.08 + 0.0001 * i, lon_deg=36.10) for i in range(100)]
|
||||
_patch_loader(monkeypatch, records)
|
||||
tlog = tmp_path / "synthetic.tlog"
|
||||
tlog.write_bytes(b"")
|
||||
|
||||
# Act
|
||||
route = extract_route_from_tlog(
|
||||
tlog,
|
||||
max_waypoints=100,
|
||||
douglas_peucker_tolerance_m=1000.0,
|
||||
min_takeoff_altitude_agl_m=0.0,
|
||||
)
|
||||
|
||||
# Assert — straight line + huge tolerance keeps only the endpoints
|
||||
assert len(route.waypoints) == 2
|
||||
|
||||
|
||||
def test_invalid_max_waypoints_raises_value_error(tmp_path: Path) -> None:
|
||||
# Act / Assert
|
||||
with pytest.raises(ValueError, match="max_waypoints"):
|
||||
extract_route_from_tlog(tmp_path / "irrelevant.tlog", max_waypoints=0)
|
||||
|
||||
|
||||
def test_invalid_region_size_raises_value_error(tmp_path: Path) -> None:
|
||||
# Act / Assert
|
||||
with pytest.raises(ValueError, match="region_size_meters"):
|
||||
extract_route_from_tlog(tmp_path / "irrelevant.tlog", region_size_meters=0.0)
|
||||
|
||||
|
||||
def test_route_extraction_error_is_replay_input_adapter_error() -> None:
|
||||
# Assert
|
||||
assert issubclass(RouteExtractionError, ReplayInputAdapterError)
|
||||
|
||||
|
||||
def test_active_segment_too_short_raises_route_extraction_error(
|
||||
monkeypatch: pytest.MonkeyPatch, tmp_path: Path
|
||||
) -> None:
|
||||
# Arrange — only 1 flying fix among 10 stationary
|
||||
records = [_fix(ts_ns=i, lat_deg=50.08, lon_deg=36.10) for i in range(10)]
|
||||
records.insert(5, _flying_fix(ts_ns=5, lat_deg=50.08, lon_deg=36.10))
|
||||
_patch_loader(monkeypatch, records)
|
||||
tlog = tmp_path / "single_flying.tlog"
|
||||
tlog.write_bytes(b"")
|
||||
|
||||
# Act / Assert
|
||||
with pytest.raises(RouteExtractionError, match="active segment too short"):
|
||||
extract_route_from_tlog(tlog)
|
||||
Reference in New Issue
Block a user