azaion/gps-denied-onboard
1
0
Fork 0
mirror of https://github.com/azaion/gps-denied-onboard.git synced 2026-06-23 02:31:14 +00:00
Code Issues Packages Projects Releases Wiki Activity

[AZ-836] TlogRouteExtractor: tlog -> RouteSpec for Epic AZ-835 C1

Browse Source 
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
2026-05-23 13:09:38 +03:00
parent 63c0217e3d
commit 5e52779056
6 changed files with 880 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/gps_denied_onboard/replay_input/__init__.py
+8
View File
@@ -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",
]
src/gps_denied_onboard/replay_input/tlog_route.py
+355
View File
@@ -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