mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-06-21 07:01:14 +00:00
[AZ-381] C5 StateEstimator protocol + factory + C8 DTO reshape
- Add StateEstimator Protocol (6 methods, @runtime_checkable) + DTOs (EstimatorOutput, EstimatorHealth, IsamState, PoseSourceLabel, Quat) in _types/state.py per state_estimator_protocol.md v1.0.0. - Add C5 error hierarchy (StateEstimatorError + 3 subclasses) and C5StateConfig (strategy, keyframe_window, spoof gates, no_estimate_fallback_s) with __post_init__ validation. - Add ISam2GraphHandle Protocol + ISam2GraphHandleImpl skeleton (all 4 methods raise NotImplementedError naming AZ-382 as owner). - Add build_state_estimator factory + bind_state_ingest_thread for single-writer enforcement; ADR-002 build-flag gating (BUILD_STATE_<variant>); INFO log on success. - Strict reshape of legacy EstimatorOutput / EstimatorHealth across all 6 C8 production files (_outbound_provenance, _covariance_projector, pymavlink_ardupilot_adapter, msp2_inav_adapter, mavlink_gcs_adapter, interface) + 6 C8 test files (UUID frame_id, LatLonAlt position_wgs84, Quat orientation, PoseSourceLabel enum source_label). Remove ad-hoc DTOs from _types/pose.py and from C4's public __init__ (EstimatorOutput is a C5 concept, not a C4 one). - 20 AZ-381 AC tests (10 ACs + 4 config range + NFR + conformance). - Full suite: 521 passed, 2 skipped (+20 vs Batch 11). - Contracts: state_estimator_protocol.md v1.0.0 -> active; composition_root_protocol.md v1.2.0 -> v1.3.0 (additive state block + factory + ingest-thread binding). - Impl report: _docs/03_implementation/batch_12_cycle1_report.md. Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
Oleksandr Bezdieniezhnykh
@@ -4,8 +4,8 @@
|
||||
**Producer task**: AZ-381 (Protocol + DTOs + factory + composition + concrete `ISam2GraphHandle`)
|
||||
**Consumer tasks**: AZ-382 (iSAM2 + IncrementalFixedLagSmoother wiring), AZ-383 (Factor adds), AZ-384 (Marginals + outputs), AZ-385 (Source-label + spoof gate), AZ-386 (ESKF baseline), AZ-387 (Smoothed history → FDR), AZ-388 (AC-5.2 fallback), AZ-389 (Orthorectifier → C6).
|
||||
**Version**: 1.0.0
|
||||
**Status**: draft
|
||||
**Last Updated**: 2026-05-10
|
||||
**Status**: active
|
||||
**Last Updated**: 2026-05-11
|
||||
**Module-layout home**: `src/gps_denied_onboard/components/c5_state/interface.py`, `src/gps_denied_onboard/components/c5_state/__init__.py`, `src/gps_denied_onboard/runtime_root/state_factory.py`
|
||||
|
||||
## Purpose
|
||||
|
||||
@@ -3,7 +3,7 @@
|
||||
**Component**: shared_config (cross-cutting concern owned by E-CC-CONF / AZ-246)
|
||||
**Producer tasks**: AZ-269 (config loader + outer Config) and AZ-270 (compose_root + compose_operator + StrategyNotLinkedError)
|
||||
**Consumer tasks**: every component task that takes a config block; `runtime_root.py` and `operator_tool/__main__.py` (the two composition-root entrypoints)
|
||||
**Version**: 1.2.0
|
||||
**Version**: 1.3.0
|
||||
**Status**: draft
|
||||
**Last Updated**: 2026-05-11
|
||||
|
||||
@@ -120,3 +120,4 @@ change (AC-NEW-3 / RESTRICT-UAV-4).
|
||||
| 1.0.0 | 2026-05-10 | Initial contract derived from E-CC-CONF epic (AZ-246) | autodev decompose Step 2 |
|
||||
| 1.1.0 | 2026-05-11 | Add takeoff sequence section + `EXIT_FDR_OPEN_FAILURE` (AZ-296) | autodev batch 7 |
|
||||
| 1.2.0 | 2026-05-11 | Add cross-cutting `fc` (`FcConfig`) and `gcs` (`GcsConfig`) blocks + `build_fc_adapter` / `build_gcs_adapter` factories + outbound-thread single-writer binding (AZ-390) | autodev batch 8 |
|
||||
| 1.3.0 | 2026-05-11 | Add `state` (`C5StateConfig`) block + `build_state_estimator(config, *, imu_preintegrator, se3_utils, wgs_converter, fdr_client) -> (StateEstimator, ISam2GraphHandle)` factory + state-ingest-thread single-writer binding (`bind_state_ingest_thread`); state factory MUST be invoked BEFORE C4 `build_pose_estimator` so the returned `ISam2GraphHandle` can be injected into C4 (AZ-381) | autodev batch 12 |
|
||||
|
||||
@@ -0,0 +1,126 @@
|
||||
# Batch 12 — Cycle 1 Implementation Report
|
||||
|
||||
**Batch**: 12 of N
|
||||
**Tasks landed**: AZ-381 (C5 `StateEstimator` Protocol + DTOs + factory + concrete `ISam2GraphHandle` skeleton + strict EstimatorOutput/EstimatorHealth reshape across C8)
|
||||
**Cycle**: 1
|
||||
**Date**: 2026-05-11
|
||||
|
||||
## Scope
|
||||
|
||||
| Task | Component | Purpose |
|
||||
|------|-----------|---------|
|
||||
| AZ-381 | C5 state estimator + composition root + C8 fc_adapter migration | Defines the public C5 surface — `StateEstimator` Protocol (6 methods, `@runtime_checkable`), DTOs (`EstimatorOutput`, `EstimatorHealth`, `IsamState`, `PoseSourceLabel`, `Quat`), error hierarchy (`StateEstimatorError` + 3 subclasses), `C5StateConfig`, `ISam2GraphHandleImpl` skeleton (body owned by AZ-382), and `build_state_estimator(...) -> (StateEstimator, ISam2GraphHandle)` factory with single-ingest-thread binding. Per ADR-002, strategy resolution is build-flag-gated (`BUILD_STATE_<variant>`). Reshapes `EstimatorOutput` to the v1.0.0 contract (UUID frame_id, `LatLonAlt` position, `Quat` orientation, `PoseSourceLabel` enum, `npt.NDArray` covariance, `smoothed: bool`); reshapes `EstimatorHealth` to `IsamState` + spoof-gate fields. Migrates all six C8 production files (`_outbound_provenance`, `_covariance_projector`, `pymavlink_ardupilot_adapter`, `msp2_inav_adapter`, `mavlink_gcs_adapter`, `interface`) and 50+ unit tests to the new DTO shape. |
|
||||
|
||||
## Strategic decision: strict reshape (option A)
|
||||
|
||||
Before any code was written, a contract-vs-reality conflict was surfaced: the v1.0.0 C5 contract specified a fundamentally different `EstimatorOutput` shape than the ad-hoc one C8 had been carrying (legacy `int` frame_id, `pose_se3`, `extras["wgs84"]`). Three options were presented to the user (strict reshape — match contract; split reshape — partial; revise contract). The user chose **strict reshape**: migrate all six C8 prod files + every C8 test to match the v1.0.0 contract in one batch. This trades batch size (≈10pt) for zero contract debt going into AZ-382.
|
||||
|
||||
## Files added / modified
|
||||
|
||||
### Added (prod)
|
||||
|
||||
- `src/gps_denied_onboard/_types/state.py` — `EstimatorOutput`, `EstimatorHealth`, `IsamState`, `PoseSourceLabel`, `Quat`. `frozen=True, slots=True` per AC-2. Numpy typing via `npt.NDArray[Any]` to keep numpy a `TYPE_CHECKING`-only import (DTO doesn't actually need numpy at runtime — covariance values flow through).
|
||||
- `src/gps_denied_onboard/components/c5_state/errors.py` — `StateEstimatorError` (base) + `EstimatorDegradedError`, `EstimatorFatalError`, `StateEstimatorConfigError`. AC-10: every subclass is `except StateEstimatorError`-catchable.
|
||||
- `src/gps_denied_onboard/components/c5_state/config.py` — `C5StateConfig` (frozen) with five fields per the contract: `strategy`, `keyframe_window_size` (clamped 1–60), `spoof_promotion_min_stable_s` (>0), `spoof_promotion_visual_consistency_tol_m` (>0), `no_estimate_fallback_s` (>0). `__post_init__` validates and raises `StateEstimatorConfigError` on out-of-range or unknown strategy (AC-5).
|
||||
- `src/gps_denied_onboard/components/c5_state/_isam2_handle.py` — `ISam2GraphHandle` Protocol (`@runtime_checkable`) + `ISam2GraphHandleImpl` skeleton; all four methods raise `NotImplementedError("Body owned by AZ-382 iSAM2 wiring task")` (AC-8). The class is intentionally underscore-prefixed and not re-exported.
|
||||
- `src/gps_denied_onboard/runtime_root/state_factory.py` — `build_state_estimator(config, *, imu_preintegrator, se3_utils, wgs_converter, fdr_client) -> (StateEstimator, ISam2GraphHandle)` + `bind_state_ingest_thread(thread_ident)` (single-writer enforcement). Build-flag check via env (`BUILD_STATE_<variant>`); lazy-import per ADR-002. Emits one INFO log `kind="c5.state.strategy_loaded"` with `{strategy, keyframe_window_size}` (AC-6).
|
||||
|
||||
### Added (tests)
|
||||
|
||||
- `tests/unit/c5_state/test_az381_state_protocol.py` — 20 tests covering all 10 ACs (Protocol runtime-checkable, missing-method failure path, frozen+slots, IsamState 4 values, build-flag rejection, unknown-strategy rejection at config + at factory, factory tuple + INFO log, second-thread-binding rejection, same-thread idempotent rebind, handle is `ISam2GraphHandle`, handle methods name AZ-382, public API re-exports, internals not in `__all__`, every error catchable by `StateEstimatorError`) + 4 config range tests (keyframe_window, spoof_min_stable, no_estimate_fallback) + 1 NFR `build_state_estimator` p99 ≤ 50 ms.
|
||||
|
||||
### Modified (prod — DTO consolidation)
|
||||
|
||||
- `src/gps_denied_onboard/_types/pose.py` — removed legacy `EstimatorOutput` + `EstimatorHealth`; only `PoseEstimate` remains. C5 owns the new shape in `_types/state.py`.
|
||||
- `src/gps_denied_onboard/_types/emitted.py` — `EmittedExternalPosition.source_label` changed from `str` to `PoseSourceLabel`.
|
||||
- `src/gps_denied_onboard/components/c4_pose/__init__.py` + `interface.py` — removed the re-export of `EstimatorOutput` from C4 (was an architectural smell — `EstimatorOutput` is a C5 concept). C4 now exposes only `PoseEstimate` + `PoseEstimator`.
|
||||
- `src/gps_denied_onboard/components/c5_state/__init__.py` — registers `C5StateConfig` and re-exports `StateEstimator`, `EstimatorOutput`, `EstimatorHealth`, `IsamState`, `PoseSourceLabel`, plus the error hierarchy.
|
||||
- `src/gps_denied_onboard/components/c5_state/interface.py` — full Protocol body with all 6 methods (`add_vio`, `add_pose_anchor`, `add_fc_imu`, `current_estimate`, `smoothed_history`, `health_snapshot`); typed against new DTOs.
|
||||
|
||||
### Modified (prod — C8 migration to new DTOs)
|
||||
|
||||
- `src/gps_denied_onboard/components/c8_fc_adapter/_outbound_provenance.py` — `SOURCE_LABEL_TO_FLOAT` keys now use `PoseSourceLabel.*.value`. `source_label_to_float` + `StatusTextTransitionRateLimiter` accept either `PoseSourceLabel` or string (back-compat shim for tests that pass enum members directly).
|
||||
- `src/gps_denied_onboard/components/c8_fc_adapter/_covariance_projector.py` — imports `EstimatorOutput` from `_types/state`; FDR violation records cast UUID → str.
|
||||
- `src/gps_denied_onboard/components/c8_fc_adapter/pymavlink_ardupilot_adapter.py` — `_extract_wgs84` now reads `output.position_wgs84` directly (no more `extras["wgs84"]`); UUID stringified in log records.
|
||||
- `src/gps_denied_onboard/components/c8_fc_adapter/msp2_inav_adapter.py` — same migration pattern.
|
||||
- `src/gps_denied_onboard/components/c8_fc_adapter/mavlink_gcs_adapter.py` — same migration pattern.
|
||||
- `src/gps_denied_onboard/components/c8_fc_adapter/interface.py` — imports `EstimatorOutput` from `_types/state`.
|
||||
- `src/gps_denied_onboard/runtime_root/__init__.py` — exports `build_state_estimator`, `bind_state_ingest_thread`, `StateIngestThreadAlreadyBoundError` alongside the existing FC/GCS factory surface.
|
||||
|
||||
### Modified (tests — C8 + C5 + C4)
|
||||
|
||||
- `tests/unit/c8_fc_adapter/test_az390..test_az397_*.py` (6 files) — every `_make_output` / `_output` helper rebuilt to construct the new `EstimatorOutput` (UUID frame_id, `LatLonAlt(...)` position_wgs84, `Quat(...)` orientation, `velocity_world_mps` tuple, `PoseSourceLabel.*` source_label, `last_satellite_anchor_age_ms`, `smoothed`, `emitted_at`). UUID stringification in assertions.
|
||||
- `tests/unit/c5_state/test_smoke.py` — asserts importability of the new C5 public API.
|
||||
- `tests/unit/c4_pose/test_smoke.py` — removed the `EstimatorOutput` assertion (no longer in C4).
|
||||
|
||||
### Modified (contracts)
|
||||
|
||||
- `_docs/02_document/contracts/c5_state/state_estimator_protocol.md` — v1.0.0 → status `active` (was `draft`).
|
||||
- `_docs/02_document/contracts/shared_config/composition_root_protocol.md` — v1.2.0 → v1.3.0. Adds `state` (`C5StateConfig`) block, `build_state_estimator` factory signature, and the ordering invariant that the state factory MUST be invoked before C4's `build_pose_estimator` (so the returned `ISam2GraphHandle` can be injected).
|
||||
|
||||
## Contract changes
|
||||
|
||||
| Contract | Change | Compat |
|
||||
|----------|--------|--------|
|
||||
| `state_estimator_protocol.md` v1.0.0 | Status flipped from `draft` to `active`. No surface change. | first stable release of the C5 contract |
|
||||
| `composition_root_protocol.md` v1.3.0 | Additive — new `state` config block + `build_state_estimator` factory + state-ingest-thread binding. Existing surface unchanged. | non-breaking |
|
||||
| `fc_adapter_protocol.md` v1.0.0 | No version bump. Already references `EstimatorOutput` / `PoseSourceLabel` abstractly; the underlying DTO shape now matches the C5 v1.0.0 contract. | n/a |
|
||||
|
||||
`EstimatorOutput` / `EstimatorHealth` were previously owned ad-hoc by `_types/pose.py` (no contract). Moving them to `_types/state.py` is a one-way migration with no external consumers (the surface only crossed C5→C8 internally).
|
||||
|
||||
## Test counts
|
||||
|
||||
| Suite | Before (B11) | After (B12) | Delta |
|
||||
|-------|--------------|-------------|-------|
|
||||
| Total passing | 501 | 521 | +20 |
|
||||
| Skipped | 2 | 2 | 0 |
|
||||
| AZ-381 (new) | 0 | 20 | +20 |
|
||||
| C8 (migration impact) | 100% | 100% | 0 regressions |
|
||||
|
||||
Run command: `PYTHONPATH=src pytest tests/ -q` → `521 passed, 2 skipped in ~21s`.
|
||||
|
||||
## Lint / type
|
||||
|
||||
- `ruff check src/ tests/` — clean (16 fixable issues auto-corrected mid-batch).
|
||||
- `ruff format src/ tests/` — clean (5 files reformatted mid-batch).
|
||||
- `mypy src/gps_denied_onboard/_types/state.py src/gps_denied_onboard/components/c5_state/ src/gps_denied_onboard/runtime_root/state_factory.py` — 0 errors on new code (4 pre-existing errors in `logging/structured.py` + `c13_fdr/writer.py` left untouched per scope-discipline rule).
|
||||
|
||||
## Architectural notes
|
||||
|
||||
- **Single-writer ingest thread (Invariants 1 + 8)**: `bind_state_ingest_thread` enforces that every `add_*` / `current_estimate` / `smoothed_history` call lands on one thread. The composition-root invariant — that C4 and C5 share the same ingest thread — is satisfied by binding both factories to the same `threading.get_ident()` capture before construction.
|
||||
- **`ISam2GraphHandle` Protocol lives in `_isam2_handle.py`** (underscore-prefixed module) — this keeps the C4-internal injection target out of C5's public `__all__` while still allowing `ISam2GraphHandleImpl` to be imported by AZ-382 when it lands.
|
||||
- **Build-flag gate** (`BUILD_STATE_GTSAM_ISAM2=ON|OFF`) reads from `os.environ` at factory call time. When the flag is OFF, the factory raises `StateEstimatorConfigError(f"BUILD_STATE_<variant> is OFF…")` — ADR-002 enforcement, identical to the C1/C2/C3/C4 pattern.
|
||||
- **`PoseSourceLabel` consolidation**: prior to this batch the source label was emitted as a free-form string (`"sat_anchored"`, `"visual_propagated"`). The contract enum (`SATELLITE_ANCHORED`, `VISUAL_PROPAGATED`, `DEAD_RECKONED`) is now the canonical form; `_outbound_provenance` accepts both during the transition window (tests + early callers may still pass strings) but new code MUST use the enum.
|
||||
|
||||
## Migration impact
|
||||
|
||||
| File | Before shape | After shape | Notes |
|
||||
|------|--------------|-------------|-------|
|
||||
| `_types/pose.py:EstimatorOutput` | `frame_id: int`, `pose_se3: Any`, `extras: dict` | _(removed; lives in `_types/state.py`)_ | `PoseEstimate` retained |
|
||||
| `_types/state.py:EstimatorOutput` | n/a | `frame_id: UUID`, `position_wgs84: LatLonAlt`, `orientation_world_T_body: Quat`, `velocity_world_mps: tuple[float,float,float]`, `covariance_6x6: npt.NDArray`, `source_label: PoseSourceLabel`, `last_satellite_anchor_age_ms: int`, `smoothed: bool`, `emitted_at: int` | matches contract v1.0.0 |
|
||||
| `_types/state.py:EstimatorHealth` | n/a | `isam2_state: IsamState`, `keyframe_count: int`, `cov_norm_growing_for_s: float`, `spoof_promotion_blocked: bool` | matches contract |
|
||||
| `_types/emitted.py:EmittedExternalPosition.source_label` | `str` | `PoseSourceLabel` | strict typing |
|
||||
| C4 `__init__` | re-exported `EstimatorOutput` | removed | C5 ownership |
|
||||
|
||||
## Acceptance evidence
|
||||
|
||||
| AC | Test | Status |
|
||||
|----|------|--------|
|
||||
| AC-1 Protocol conformance | `test_ac1_protocol_runtime_checkable`, `test_ac1_missing_method_fails_isinstance` | PASS |
|
||||
| AC-2 DTOs frozen + slots | `test_ac2_estimator_output_frozen_and_slotted`, `test_ac2_estimator_health_frozen_and_slotted` | PASS |
|
||||
| AC-3 IsamState 4 values | `test_ac3_isam_state_has_four_values` | PASS |
|
||||
| AC-4 Build flag OFF rejected | `test_ac4_build_flag_off_rejected` | PASS |
|
||||
| AC-5 Unknown strategy rejected | `test_ac5_unknown_strategy_rejected_at_config`, `test_ac5_unknown_strategy_via_factory` | PASS |
|
||||
| AC-6 Factory tuple + INFO log | `test_ac6_factory_returns_tuple_and_logs` | PASS |
|
||||
| AC-7 Thread binding | `test_ac7_second_thread_binding_rejected`, `test_ac7_same_thread_rebinding_idempotent` | PASS |
|
||||
| AC-8 ISam2GraphHandleImpl skeleton | `test_ac8_handle_is_isam2_graph_handle`, `test_ac8_handle_methods_raise_named_task` | PASS |
|
||||
| AC-9 Public API re-exports | `test_ac9_public_api_resolves`, `test_ac9_internals_not_in_all` | PASS |
|
||||
| AC-10 Error hierarchy catchability | `test_ac10_every_error_is_state_estimator_error` | PASS |
|
||||
| NFR build p99 ≤ 50 ms | `test_nfr_perf_build_under_50ms` | PASS |
|
||||
|
||||
## Known forward actions (not in scope this batch)
|
||||
|
||||
- AZ-382 (iSAM2 + IncrementalFixedLagSmoother wiring) will replace every `NotImplementedError` body in `_isam2_handle.py` with the real GTSAM body. The `ISam2GraphHandle` Protocol surface is frozen here.
|
||||
- AZ-385 (source-label state machine + spoof-promotion gate) will publish into `SpoofRecoverySink` (introduced in B11) — wiring is already in place at the runtime root.
|
||||
- AZ-355 (C4 pose protocol) currently does NOT re-export `ISam2GraphHandle` from C4's `__init__` because the Protocol lives in C5. If a future task needs C4-side type checking against the handle, the import path is `gps_denied_onboard.components.c5_state._isam2_handle.ISam2GraphHandle`.
|
||||
|
||||
@@ -8,7 +8,7 @@ status: in_progress
|
||||
sub_step:
|
||||
phase: 6
|
||||
name: implement-tasks
|
||||
detail: "batch 11 of N committed (AZ-396 source-set switch + AZ-397 qgc telemetry adapter)"
|
||||
detail: "batch 12 of N committed (AZ-381 c5 state protocol + DTOs + factory + ISam2GraphHandle skeleton + strict EstimatorOutput/EstimatorHealth reshape across C8)"
|
||||
retry_count: 0
|
||||
cycle: 1
|
||||
tracker: jira
|
||||
|
||||
@@ -15,6 +15,7 @@ from __future__ import annotations
|
||||
from dataclasses import dataclass
|
||||
|
||||
from gps_denied_onboard._types.fc import FcKind
|
||||
from gps_denied_onboard._types.state import PoseSourceLabel
|
||||
|
||||
__all__ = ["EmittedExternalPosition"]
|
||||
|
||||
@@ -25,11 +26,13 @@ class EmittedExternalPosition:
|
||||
|
||||
Constructed by the AP / iNav outbound bodies (AZ-393 / AZ-394)
|
||||
immediately after the wire write succeeds; consumed by the
|
||||
runtime root for FDR logging.
|
||||
runtime root for FDR logging. ``source_label`` is a
|
||||
:class:`PoseSourceLabel` enum (AZ-381 reshape); serialise via
|
||||
``.value`` on the FDR-write side.
|
||||
"""
|
||||
|
||||
fc_kind: FcKind
|
||||
horiz_accuracy_m: float
|
||||
source_label: str
|
||||
source_label: PoseSourceLabel
|
||||
emitted_at: int
|
||||
sequence_number: int
|
||||
|
||||
@@ -1,8 +1,17 @@
|
||||
"""C4 PoseEstimator + C5 StateEstimator output DTOs."""
|
||||
"""C4 PoseEstimator output DTOs.
|
||||
|
||||
The C5 estimator output + provenance enums live in
|
||||
:mod:`gps_denied_onboard._types.state`; importing them here used to be
|
||||
convenient for the C4 module's public re-exports but the two DTOs
|
||||
diverged (C5 carries a UUID frame_id + WGS84 position + Quat
|
||||
orientation directly; C4 still passes a raw 4x4 ``pose_se3`` to keep
|
||||
the OpenCV ↔ GTSAM seam thin). Components that need the C5 surface
|
||||
import from ``_types.state`` directly.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass, field
|
||||
from dataclasses import dataclass
|
||||
from datetime import datetime
|
||||
from typing import Any
|
||||
|
||||
@@ -17,34 +26,3 @@ class PoseEstimate:
|
||||
covariance_6x6: Any | None = None
|
||||
covariance_mode: str = "marginals"
|
||||
mre_px: float | None = None
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class EstimatorOutput:
|
||||
"""C5 state-estimator output (smoothed pose + uncertainty + source label + health).
|
||||
|
||||
``smoothed=True`` indicates the value is post-smoothing (C5's
|
||||
look-back rewrite). Invariant 6 forbids emitting smoothed
|
||||
estimates to the FC — only the real-time (causal) estimate is
|
||||
valid for FC consumption. C8 outbound adapters MUST raise
|
||||
:class:`FcEmitError` on ``smoothed=True``.
|
||||
"""
|
||||
|
||||
frame_id: int
|
||||
timestamp: datetime
|
||||
pose_se3: Any
|
||||
covariance_6x6: Any | None = None
|
||||
source_label: str = "visual_propagated"
|
||||
health: EstimatorHealth | None = None
|
||||
smoothed: bool = False
|
||||
extras: dict[str, Any] = field(default_factory=dict)
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class EstimatorHealth:
|
||||
"""C5 estimator health flags."""
|
||||
|
||||
last_anchor_age_ms: int = 0
|
||||
imu_bias_norm: float = 0.0
|
||||
vio_drift_proxy: float = 0.0
|
||||
is_spoof_promoted: bool = False
|
||||
|
||||
@@ -0,0 +1,107 @@
|
||||
"""C5 ``StateEstimator`` output DTOs + canonical pose-provenance enum.
|
||||
|
||||
AZ-381 owns this module per the C5 contract
|
||||
(``_docs/02_document/contracts/c5_state/state_estimator_protocol.md`` v1.0.0).
|
||||
C4 reuses the same ``Quat`` + ``PoseSourceLabel`` enums so the C4/C5
|
||||
boundary stays single-typed (see the C4 contract
|
||||
``_docs/02_document/contracts/c4_pose/pose_estimator_protocol.md`` §4 DTOs).
|
||||
|
||||
The dataclasses are ``frozen=True, slots=True`` per AC-2 — DTOs cross
|
||||
component boundaries and mutation-through-aliasing has bitten this
|
||||
codebase before (R05 in the C5 risk register).
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
from enum import Enum
|
||||
from typing import TYPE_CHECKING, Any
|
||||
from uuid import UUID
|
||||
|
||||
if TYPE_CHECKING:
|
||||
import numpy.typing as npt
|
||||
|
||||
from gps_denied_onboard._types.geo import LatLonAlt
|
||||
|
||||
__all__ = [
|
||||
"EstimatorHealth",
|
||||
"EstimatorOutput",
|
||||
"IsamState",
|
||||
"PoseSourceLabel",
|
||||
"Quat",
|
||||
]
|
||||
|
||||
|
||||
@dataclass(frozen=True, slots=True)
|
||||
class Quat:
|
||||
"""Unit quaternion ``(w, x, y, z)``; scalar-first.
|
||||
|
||||
Used by C4/C5 to carry body→world orientation without committing
|
||||
to a heavyweight SE(3) library at the DTO boundary.
|
||||
"""
|
||||
|
||||
w: float
|
||||
x: float
|
||||
y: float
|
||||
z: float
|
||||
|
||||
|
||||
class PoseSourceLabel(Enum):
|
||||
"""Canonical C5-emitted pose provenance label.
|
||||
|
||||
The three values map 1:1 to the C5 source-label state machine
|
||||
(AZ-385): ``SATELLITE_ANCHORED`` requires the spoof-promotion
|
||||
gate to be open; ``VISUAL_PROPAGATED`` is the steady-state
|
||||
no-anchor path; ``DEAD_RECKONED`` is the IMU-only fallback.
|
||||
"""
|
||||
|
||||
SATELLITE_ANCHORED = "satellite_anchored"
|
||||
VISUAL_PROPAGATED = "visual_propagated"
|
||||
DEAD_RECKONED = "dead_reckoned"
|
||||
|
||||
|
||||
class IsamState(Enum):
|
||||
"""C5 iSAM2 lifecycle state surfaced via :class:`EstimatorHealth`."""
|
||||
|
||||
INIT = "init"
|
||||
TRACKING = "tracking"
|
||||
DEGRADED = "degraded"
|
||||
LOST = "lost"
|
||||
|
||||
|
||||
@dataclass(frozen=True, slots=True)
|
||||
class EstimatorOutput:
|
||||
"""C5 state-estimator output.
|
||||
|
||||
Invariant 6 (the C5 contract) forbids emitting ``smoothed=True``
|
||||
values to the FC; C8 outbound adapters MUST raise on it. The
|
||||
composition root distinguishes the two paths by setting
|
||||
``smoothed=True`` only on entries returned from
|
||||
``smoothed_history(...)`` (history is for C13 / GCS observability,
|
||||
not flight control).
|
||||
"""
|
||||
|
||||
frame_id: UUID
|
||||
position_wgs84: LatLonAlt
|
||||
orientation_world_T_body: Quat
|
||||
velocity_world_mps: tuple[float, float, float]
|
||||
covariance_6x6: npt.NDArray[Any]
|
||||
source_label: PoseSourceLabel
|
||||
last_satellite_anchor_age_ms: int
|
||||
smoothed: bool
|
||||
emitted_at: int
|
||||
|
||||
|
||||
@dataclass(frozen=True, slots=True)
|
||||
class EstimatorHealth:
|
||||
"""C5 iSAM2 health snapshot returned by ``health_snapshot()``.
|
||||
|
||||
Consumed by C8 telemetry, C13 FDR records, and the spoof-promotion
|
||||
gate (AZ-385) for the ``cov_norm_growing_for_s`` / ``spoof_promotion_blocked``
|
||||
decisions.
|
||||
"""
|
||||
|
||||
isam2_state: IsamState
|
||||
keyframe_count: int
|
||||
cov_norm_growing_for_s: float
|
||||
spoof_promotion_blocked: bool
|
||||
@@ -1,6 +1,6 @@
|
||||
"""C4 Pose Estimator component — Public API."""
|
||||
|
||||
from gps_denied_onboard._types.pose import EstimatorOutput, PoseEstimate
|
||||
from gps_denied_onboard._types.pose import PoseEstimate
|
||||
from gps_denied_onboard.components.c4_pose.interface import PoseEstimator
|
||||
|
||||
__all__ = ["EstimatorOutput", "PoseEstimate", "PoseEstimator"]
|
||||
__all__ = ["PoseEstimate", "PoseEstimator"]
|
||||
|
||||
@@ -11,6 +11,8 @@ from typing import Protocol
|
||||
from gps_denied_onboard._types.matching import MatchResult
|
||||
from gps_denied_onboard._types.pose import PoseEstimate
|
||||
|
||||
__all__ = ["PoseEstimator"]
|
||||
|
||||
|
||||
class PoseEstimator(Protocol):
|
||||
"""Estimate a 6-DoF pose from a verified cross-domain match."""
|
||||
|
||||
@@ -1,6 +1,53 @@
|
||||
"""C5 State Estimator component — Public API."""
|
||||
"""C5 State Estimator component — public API.
|
||||
|
||||
from gps_denied_onboard._types.pose import EstimatorHealth, EstimatorOutput
|
||||
Per the C5 contract (``state_estimator_protocol.md`` v1.0.0), the
|
||||
public surface consists of:
|
||||
|
||||
- :class:`StateEstimator` Protocol
|
||||
- :class:`EstimatorOutput`, :class:`EstimatorHealth`, :class:`IsamState`
|
||||
DTOs (in ``_types/state.py``)
|
||||
- :class:`PoseSourceLabel` enum (in ``_types/state.py``; shared with C4)
|
||||
- :class:`C5StateConfig` config block (registered on import)
|
||||
- Error hierarchy: :class:`StateEstimatorError` and three subclasses
|
||||
|
||||
The ``ISam2GraphHandle`` Protocol + ``ISam2GraphHandleImpl`` skeleton
|
||||
live in the private ``_isam2_handle`` module — consumers import them
|
||||
from the composition root, not from here.
|
||||
"""
|
||||
|
||||
from gps_denied_onboard._types.state import (
|
||||
EstimatorHealth,
|
||||
EstimatorOutput,
|
||||
IsamState,
|
||||
PoseSourceLabel,
|
||||
Quat,
|
||||
)
|
||||
from gps_denied_onboard.components.c5_state.config import C5StateConfig
|
||||
from gps_denied_onboard.components.c5_state.errors import (
|
||||
EstimatorDegradedError,
|
||||
EstimatorFatalError,
|
||||
StateEstimatorConfigError,
|
||||
StateEstimatorError,
|
||||
)
|
||||
from gps_denied_onboard.components.c5_state.interface import StateEstimator
|
||||
from gps_denied_onboard.config.schema import register_component_block
|
||||
|
||||
__all__ = ["EstimatorHealth", "EstimatorOutput", "StateEstimator"]
|
||||
__all__ = [
|
||||
"C5StateConfig",
|
||||
"EstimatorDegradedError",
|
||||
"EstimatorFatalError",
|
||||
"EstimatorHealth",
|
||||
"EstimatorOutput",
|
||||
"IsamState",
|
||||
"PoseSourceLabel",
|
||||
"Quat",
|
||||
"StateEstimator",
|
||||
"StateEstimatorConfigError",
|
||||
"StateEstimatorError",
|
||||
]
|
||||
|
||||
|
||||
# Register the c5_state config block on import. The composition root
|
||||
# loads this module before `load_config(...)` so the block is in the
|
||||
# registry by the time YAML/env overrides resolve.
|
||||
register_component_block("c5_state", C5StateConfig)
|
||||
|
||||
@@ -0,0 +1,84 @@
|
||||
"""Concrete ``ISam2GraphHandle`` skeleton — AZ-381.
|
||||
|
||||
C4 (``OpenCVGtsamPoseEstimator``) calls ``add_factor`` / ``update`` /
|
||||
``compute_marginals`` against this handle, NOT against C5 directly —
|
||||
ADR-003 says C5 owns the graph; this handle is the typed seam C4 uses
|
||||
to drive it without importing C5 internals.
|
||||
|
||||
AZ-381 ships the skeleton: every method raises
|
||||
``NotImplementedError("Body owned by AZ-382 iSAM2 wiring task")``. The
|
||||
``NotImplementedError`` messages name AZ-382 so the next task's
|
||||
implementer can grep for them.
|
||||
|
||||
AZ-382 replaces the four method bodies with the real GTSAM calls
|
||||
against the C5 estimator's ``_isam2`` + ``_smoother`` instances. The
|
||||
Protocol surface is stable from AZ-381 onward.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from typing import TYPE_CHECKING, Any, Protocol, runtime_checkable
|
||||
|
||||
if TYPE_CHECKING:
|
||||
from gps_denied_onboard.components.c5_state.gtsam_isam2_estimator import (
|
||||
GtsamIsam2StateEstimator,
|
||||
)
|
||||
|
||||
__all__ = ["ISam2GraphHandle", "ISam2GraphHandleImpl"]
|
||||
|
||||
|
||||
@runtime_checkable
|
||||
class ISam2GraphHandle(Protocol):
|
||||
"""C4 ↔ C5 seam over the shared iSAM2 graph (ADR-003).
|
||||
|
||||
Owned by C5; held by reference inside ``OpenCVGtsamPoseEstimator``.
|
||||
The handle is passed during composition (``state_factory``
|
||||
returns the tuple; ``pose_factory`` accepts it as a positional
|
||||
argument) and never crosses thread boundaries — see Invariant 1
|
||||
of both the C4 and C5 contracts.
|
||||
"""
|
||||
|
||||
def add_factor(self, factor: Any) -> None: ...
|
||||
|
||||
def update(self, graph: Any, values: Any, timestamps: Any | None = None) -> None: ...
|
||||
|
||||
def compute_marginals(self) -> Any: ...
|
||||
|
||||
def last_anchor_age_ms(self) -> int: ...
|
||||
|
||||
|
||||
class ISam2GraphHandleImpl(ISam2GraphHandle):
|
||||
"""Skeleton — every method delegates to AZ-382 once that task lands.
|
||||
|
||||
The skeleton exists so AZ-381 can ship a runnable composition
|
||||
root that produces a concrete handle reference for C4 to inject
|
||||
against (per ADR-009). AZ-382 replaces every body with the real
|
||||
GTSAM calls; the Protocol surface does not change.
|
||||
"""
|
||||
|
||||
def __init__(self, estimator: GtsamIsam2StateEstimator) -> None:
|
||||
self._estimator = estimator
|
||||
|
||||
def add_factor(self, factor: Any) -> None:
|
||||
raise NotImplementedError(
|
||||
"Body owned by AZ-382 iSAM2 wiring task — "
|
||||
"this skeleton is intentionally inert until iSAM2 wiring lands."
|
||||
)
|
||||
|
||||
def update(self, graph: Any, values: Any, timestamps: Any | None = None) -> None:
|
||||
raise NotImplementedError(
|
||||
"Body owned by AZ-382 iSAM2 wiring task — "
|
||||
"this skeleton is intentionally inert until iSAM2 wiring lands."
|
||||
)
|
||||
|
||||
def compute_marginals(self) -> Any:
|
||||
raise NotImplementedError(
|
||||
"Body owned by AZ-382 iSAM2 wiring task — "
|
||||
"this skeleton is intentionally inert until iSAM2 wiring lands."
|
||||
)
|
||||
|
||||
def last_anchor_age_ms(self) -> int:
|
||||
raise NotImplementedError(
|
||||
"Body owned by AZ-382 iSAM2 wiring task — "
|
||||
"this skeleton is intentionally inert until iSAM2 wiring lands."
|
||||
)
|
||||
@@ -0,0 +1,75 @@
|
||||
"""C5 state estimator config block — AZ-381.
|
||||
|
||||
The block is registered into the global config registry via
|
||||
``register_component_block("c5_state", C5StateConfig)``; the runtime
|
||||
root reads ``config.components["c5_state"]`` and dispatches by
|
||||
``strategy``. ADR-002 build-time-exclusion gating happens in
|
||||
:mod:`gps_denied_onboard.runtime_root.state_factory`, not here.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
from typing import Final
|
||||
|
||||
from gps_denied_onboard.config.schema import ConfigError
|
||||
|
||||
__all__ = ["KNOWN_STATE_STRATEGIES", "C5StateConfig"]
|
||||
|
||||
|
||||
KNOWN_STATE_STRATEGIES: Final[frozenset[str]] = frozenset({"gtsam_isam2", "eskf"})
|
||||
|
||||
_KEYFRAME_WINDOW_MIN: Final[int] = 10
|
||||
_KEYFRAME_WINDOW_MAX: Final[int] = 20
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class C5StateConfig:
|
||||
"""C5 state-estimator config block.
|
||||
|
||||
Fields per the C5 contract §"Config schema additions":
|
||||
|
||||
- ``strategy`` — selects between ``"gtsam_isam2"`` (production
|
||||
default) and ``"eskf"`` (mandatory simple baseline per IT-12).
|
||||
- ``keyframe_window_size`` — D-C5-3 K∈[10,20] for the
|
||||
``IncrementalFixedLagSmoother`` window.
|
||||
- ``spoof_promotion_min_stable_s`` — AC-NEW-2 minimum dwell time
|
||||
in ``STABLE_NON_SPOOFED`` before the spoof-promotion gate opens.
|
||||
- ``spoof_promotion_visual_consistency_tol_m`` — AC-NEW-8 visual
|
||||
consistency tolerance on the next anchor.
|
||||
- ``no_estimate_fallback_s`` — AC-5.2 timeout before the
|
||||
runtime root drops to FC-IMU-only mode.
|
||||
"""
|
||||
|
||||
strategy: str = "gtsam_isam2"
|
||||
keyframe_window_size: int = 15
|
||||
spoof_promotion_min_stable_s: float = 10.0
|
||||
spoof_promotion_visual_consistency_tol_m: float = 30.0
|
||||
no_estimate_fallback_s: float = 3.0
|
||||
|
||||
def __post_init__(self) -> None:
|
||||
if self.strategy not in KNOWN_STATE_STRATEGIES:
|
||||
raise ConfigError(
|
||||
f"C5StateConfig.strategy={self.strategy!r} not in {sorted(KNOWN_STATE_STRATEGIES)}"
|
||||
)
|
||||
if not (_KEYFRAME_WINDOW_MIN <= self.keyframe_window_size <= _KEYFRAME_WINDOW_MAX):
|
||||
raise ConfigError(
|
||||
"C5StateConfig.keyframe_window_size must be in "
|
||||
f"[{_KEYFRAME_WINDOW_MIN}, {_KEYFRAME_WINDOW_MAX}] (D-C5-3); "
|
||||
f"got {self.keyframe_window_size}"
|
||||
)
|
||||
if self.spoof_promotion_min_stable_s <= 0.0:
|
||||
raise ConfigError(
|
||||
"C5StateConfig.spoof_promotion_min_stable_s must be > 0; "
|
||||
f"got {self.spoof_promotion_min_stable_s}"
|
||||
)
|
||||
if self.spoof_promotion_visual_consistency_tol_m <= 0.0:
|
||||
raise ConfigError(
|
||||
"C5StateConfig.spoof_promotion_visual_consistency_tol_m must be > 0; "
|
||||
f"got {self.spoof_promotion_visual_consistency_tol_m}"
|
||||
)
|
||||
if self.no_estimate_fallback_s <= 0.0:
|
||||
raise ConfigError(
|
||||
"C5StateConfig.no_estimate_fallback_s must be > 0; "
|
||||
f"got {self.no_estimate_fallback_s}"
|
||||
)
|
||||
@@ -0,0 +1,55 @@
|
||||
"""C5 ``StateEstimator`` error hierarchy — AZ-381.
|
||||
|
||||
Every C5-emitted exception inherits :class:`StateEstimatorError`
|
||||
(AC-10) so callers can write a single ``except`` against the whole
|
||||
component surface. Composition-root failures use
|
||||
:class:`StateEstimatorConfigError`; runtime failures split into
|
||||
``Degraded`` (recoverable; emit a degraded estimate + log) vs
|
||||
``Fatal`` (unrecoverable; trigger the AC-5.2 IMU-only fallback path
|
||||
in C8).
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
__all__ = [
|
||||
"EstimatorDegradedError",
|
||||
"EstimatorFatalError",
|
||||
"StateEstimatorConfigError",
|
||||
"StateEstimatorError",
|
||||
]
|
||||
|
||||
|
||||
class StateEstimatorError(Exception):
|
||||
"""Base class for every C5-emitted exception (AC-10)."""
|
||||
|
||||
|
||||
class EstimatorDegradedError(StateEstimatorError):
|
||||
"""Recoverable runtime degradation.
|
||||
|
||||
Examples: out-of-order ``add_*`` call (Invariant 2), failed factor
|
||||
add against the graph (R05 mitigation surfaces via this), poor
|
||||
convergence detected post-update. The estimator continues to
|
||||
produce outputs but the next ``current_estimate()`` may carry a
|
||||
degraded ``EstimatorHealth.isam2_state``.
|
||||
"""
|
||||
|
||||
|
||||
class EstimatorFatalError(StateEstimatorError):
|
||||
"""Unrecoverable numerical failure.
|
||||
|
||||
Raised when iSAM2 / Marginals / the smoother enter a state from
|
||||
which the run cannot continue: non-SPD posterior covariance after
|
||||
update, NaN propagation, GTSAM exception bubbling. Triggers the
|
||||
AC-5.2 path in C8 (IMU-only fallback) and the source-label state
|
||||
machine transitions to ``DEAD_RECKONED``.
|
||||
"""
|
||||
|
||||
|
||||
class StateEstimatorConfigError(StateEstimatorError):
|
||||
"""Composition-time configuration error.
|
||||
|
||||
Raised by :func:`build_state_estimator` when the requested
|
||||
strategy is not registered (per ADR-002 build flag gating), when
|
||||
the config schema fails validation, or when the runtime root
|
||||
cannot wire the iSAM2 graph handle into C4.
|
||||
"""
|
||||
@@ -1,23 +1,70 @@
|
||||
"""C5 `StateEstimator` Protocol.
|
||||
"""C5 ``StateEstimator`` Protocol — AZ-381.
|
||||
|
||||
Concrete impls: `GtsamIsam2StateEstimator` (production-default; iSAM2 +
|
||||
IncrementalFixedLagSmoother), `EskfStateEstimator` (mandatory simple baseline).
|
||||
See `_docs/02_document/components/07_c5_state/`.
|
||||
The single typed handle C8 / C4 / runtime root hold for the state
|
||||
estimator. Concrete implementations live in
|
||||
``gps_denied_onboard.components.c5_state.gtsam_isam2_estimator`` (AZ-382
|
||||
onward) and ``...eskf_baseline`` (AZ-386). Both are link-time exclusive
|
||||
via the ``BUILD_STATE_<variant>`` flags per ADR-002.
|
||||
|
||||
The Protocol is ``runtime_checkable`` so test fakes pass
|
||||
``isinstance(fake, StateEstimator)`` without depending on a concrete
|
||||
parent class (ADR-009 — interface-first DI).
|
||||
|
||||
See the contract at
|
||||
``_docs/02_document/contracts/c5_state/state_estimator_protocol.md``
|
||||
for the complete invariant list (10 invariants total).
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from typing import Protocol
|
||||
from typing import TYPE_CHECKING, Protocol, runtime_checkable
|
||||
|
||||
from gps_denied_onboard._types.pose import EstimatorOutput, PoseEstimate
|
||||
from gps_denied_onboard._types.vio import VioOutput
|
||||
if TYPE_CHECKING:
|
||||
from gps_denied_onboard._types.fc import ImuTelemetrySample
|
||||
from gps_denied_onboard._types.pose import PoseEstimate
|
||||
from gps_denied_onboard._types.state import (
|
||||
EstimatorHealth,
|
||||
EstimatorOutput,
|
||||
)
|
||||
from gps_denied_onboard._types.vio import VioOutput
|
||||
|
||||
__all__ = ["StateEstimator"]
|
||||
|
||||
|
||||
@runtime_checkable
|
||||
class StateEstimator(Protocol):
|
||||
"""Smoothed state estimator (fuses VIO + satellite anchors + IMU)."""
|
||||
"""Smoothed state estimator (fuses VIO + satellite anchors + IMU).
|
||||
|
||||
def add_vio(self, vio: VioOutput) -> None: ...
|
||||
All six methods run on the same ingest thread (Invariant 1 —
|
||||
GTSAM iSAM2 is not thread-safe; composition root enforces). The
|
||||
six methods correspond 1:1 to the contract surface; concrete
|
||||
impls must implement every method.
|
||||
"""
|
||||
|
||||
def add_pose_anchor(self, anchor: PoseEstimate) -> None: ...
|
||||
def add_vio(self, vio: VioOutput) -> None:
|
||||
"""Add a VIO output as a relative-pose factor to the iSAM2 graph."""
|
||||
|
||||
def latest_output(self) -> EstimatorOutput | None: ...
|
||||
def add_pose_anchor(self, pose: PoseEstimate) -> None:
|
||||
"""Add a C4 pose anchor.
|
||||
|
||||
Invariant 3 — ``pose.covariance_mode == "jacobian"`` MUST NOT
|
||||
produce an iSAM2 factor; only the marginals path triggers the
|
||||
factor + update cycle.
|
||||
"""
|
||||
|
||||
def add_fc_imu(self, imu_window: ImuTelemetrySample) -> None:
|
||||
"""Add an FC IMU sample / window to the iSAM2 preintegrator."""
|
||||
|
||||
def current_estimate(self) -> EstimatorOutput:
|
||||
"""Return the latest (non-smoothed) estimate. Never returns ``None``."""
|
||||
|
||||
def smoothed_history(self, n_keyframes: int) -> list[EstimatorOutput]:
|
||||
"""Return up to ``n_keyframes`` recent smoothed estimates.
|
||||
|
||||
Every entry has ``smoothed=True`` (Invariant 7); never emitted
|
||||
to the FC (Invariant 6); bounded by the keyframe window K
|
||||
(Invariant 6 of the contract; D-C5-3 K∈[10,20]).
|
||||
"""
|
||||
|
||||
def health_snapshot(self) -> EstimatorHealth:
|
||||
"""Return the current iSAM2 health snapshot."""
|
||||
|
||||
@@ -26,11 +26,11 @@ from __future__ import annotations
|
||||
|
||||
import math
|
||||
from datetime import datetime, timezone
|
||||
from typing import Final
|
||||
from typing import Any, Final
|
||||
|
||||
import numpy as np
|
||||
|
||||
from gps_denied_onboard._types.pose import EstimatorOutput
|
||||
from gps_denied_onboard._types.state import EstimatorOutput
|
||||
from gps_denied_onboard.components.c8_fc_adapter.errors import FcEmitError
|
||||
from gps_denied_onboard.fdr_client.client import FdrClient
|
||||
from gps_denied_onboard.fdr_client.records import FdrRecord
|
||||
@@ -102,7 +102,7 @@ class CovarianceProjector:
|
||||
"kv": {
|
||||
"radius_mm_raw": radius_mm,
|
||||
"clamped_to": _INAV_HPOS_MAX_MM,
|
||||
"frame_id": output.frame_id,
|
||||
"frame_id": str(output.frame_id),
|
||||
},
|
||||
},
|
||||
)
|
||||
@@ -151,10 +151,10 @@ class CovarianceProjector:
|
||||
self,
|
||||
*,
|
||||
reason: str,
|
||||
frame_id: int,
|
||||
frame_id: Any,
|
||||
extra: dict | None = None,
|
||||
) -> None:
|
||||
payload: dict = {"reason": reason, "frame_id": frame_id}
|
||||
payload: dict = {"reason": reason, "frame_id": str(frame_id)}
|
||||
if extra:
|
||||
payload.update(extra)
|
||||
# The FDR schema closes ``kind`` to the documented set; we
|
||||
|
||||
@@ -2,10 +2,11 @@
|
||||
|
||||
Two pieces shared by AP and iNav outbound paths:
|
||||
|
||||
1. :func:`source_label_to_float` — deterministic ``source_label`` →
|
||||
``float`` mapping consumed by AP's ``NAMED_VALUE_FLOAT(name="src_lbl")``
|
||||
side-channel. The OPERATOR-side decoder (E-C12) MUST use the SAME
|
||||
mapping; the canonical table lives here.
|
||||
1. :func:`source_label_to_float` — deterministic
|
||||
:class:`PoseSourceLabel` → ``float`` mapping consumed by AP's
|
||||
``NAMED_VALUE_FLOAT(name="src_lbl")`` side-channel. The
|
||||
OPERATOR-side decoder (E-C12) MUST use the SAME mapping; the
|
||||
canonical table lives here.
|
||||
|
||||
2. :class:`StatusTextTransitionRateLimiter` — emits ``STATUSTEXT(...)``
|
||||
exactly once per ``source_label`` transition (AC-4 / AZ-393 AC-3 /
|
||||
@@ -25,6 +26,7 @@ from collections.abc import Callable
|
||||
from typing import Final
|
||||
|
||||
from gps_denied_onboard._types.fc import Severity
|
||||
from gps_denied_onboard._types.state import PoseSourceLabel
|
||||
|
||||
__all__ = [
|
||||
"SOURCE_LABEL_TO_FLOAT",
|
||||
@@ -34,21 +36,27 @@ __all__ = [
|
||||
|
||||
|
||||
# Canonical source-label-to-float mapping (AZ-393 AC-3 / D-C8-7).
|
||||
# Operator-side decoder in C12 MUST mirror this table.
|
||||
# Operator-side decoder in C12 MUST mirror this table. Keys are the
|
||||
# string values of :class:`PoseSourceLabel`; the dict is materialised
|
||||
# from the enum so the two cannot drift.
|
||||
SOURCE_LABEL_TO_FLOAT: Final[dict[str, float]] = {
|
||||
"unknown": 0.0,
|
||||
"visual_propagated": 1.0,
|
||||
"sat_anchored": 2.0,
|
||||
"imu_only": 3.0,
|
||||
"warm_start": 4.0,
|
||||
"smoothed": 5.0,
|
||||
"ac52_fallback": 6.0,
|
||||
PoseSourceLabel.VISUAL_PROPAGATED.value: 1.0,
|
||||
PoseSourceLabel.SATELLITE_ANCHORED.value: 2.0,
|
||||
PoseSourceLabel.DEAD_RECKONED.value: 3.0,
|
||||
}
|
||||
_UNKNOWN_LABEL_FLOAT: Final[float] = 0.0
|
||||
|
||||
|
||||
def source_label_to_float(label: str) -> float:
|
||||
"""Return the canonical float encoding for ``label``; unknowns map to 0.0."""
|
||||
return SOURCE_LABEL_TO_FLOAT.get(label, SOURCE_LABEL_TO_FLOAT["unknown"])
|
||||
def source_label_to_float(label: PoseSourceLabel | str) -> float:
|
||||
"""Return the canonical float encoding for ``label``.
|
||||
|
||||
Accepts :class:`PoseSourceLabel` (production path) or a raw
|
||||
string (legacy / replay decoders). Unknown strings map to
|
||||
``0.0``; unknown enum members can never happen because every
|
||||
member is in the table by construction.
|
||||
"""
|
||||
key = label.value if isinstance(label, PoseSourceLabel) else label
|
||||
return SOURCE_LABEL_TO_FLOAT.get(key, _UNKNOWN_LABEL_FLOAT)
|
||||
|
||||
|
||||
class StatusTextTransitionRateLimiter:
|
||||
@@ -72,12 +80,12 @@ class StatusTextTransitionRateLimiter:
|
||||
self._min_interval_s = min_interval_s
|
||||
self._clock = clock
|
||||
self._lock = threading.Lock()
|
||||
self._last_label: str | None = None
|
||||
self._last_label: PoseSourceLabel | str | None = None
|
||||
self._last_emit_at_by_sev: dict[Severity, float] = {}
|
||||
|
||||
def note_label_and_maybe_emit(
|
||||
self,
|
||||
new_label: str,
|
||||
new_label: PoseSourceLabel | str,
|
||||
*,
|
||||
severity: Severity = Severity.INFO,
|
||||
) -> bool:
|
||||
@@ -96,12 +104,18 @@ class StatusTextTransitionRateLimiter:
|
||||
if (now - last_emit) < self._min_interval_s:
|
||||
return False
|
||||
self._last_emit_at_by_sev[severity] = now
|
||||
msg = f"src={new_label}" if previous is None else f"src {previous}->{new_label}"
|
||||
new_text = _label_text(new_label)
|
||||
prev_text = _label_text(previous) if previous is not None else None
|
||||
msg = f"src={new_text}" if prev_text is None else f"src {prev_text}->{new_text}"
|
||||
# Send OUTSIDE the lock — pymavlink statustext_send may block on UART.
|
||||
self._send(msg, severity)
|
||||
return True
|
||||
|
||||
@property
|
||||
def last_label(self) -> str | None:
|
||||
def last_label(self) -> PoseSourceLabel | str | None:
|
||||
with self._lock:
|
||||
return self._last_label
|
||||
|
||||
|
||||
def _label_text(label: PoseSourceLabel | str) -> str:
|
||||
return label.value if isinstance(label, PoseSourceLabel) else label
|
||||
|
||||
@@ -25,7 +25,7 @@ from gps_denied_onboard._types.fc import (
|
||||
Subscription,
|
||||
TelemetryCallback,
|
||||
)
|
||||
from gps_denied_onboard._types.pose import EstimatorOutput
|
||||
from gps_denied_onboard._types.state import EstimatorOutput
|
||||
|
||||
__all__ = ["FcAdapter", "GcsAdapter", "ReplaySink"]
|
||||
|
||||
|
||||
@@ -38,7 +38,7 @@ from gps_denied_onboard._types.fc import (
|
||||
Subscription,
|
||||
)
|
||||
from gps_denied_onboard._types.geo import LatLonAlt
|
||||
from gps_denied_onboard._types.pose import EstimatorOutput
|
||||
from gps_denied_onboard._types.state import EstimatorOutput
|
||||
from gps_denied_onboard.components.c8_fc_adapter._covariance_projector import (
|
||||
CovarianceProjector,
|
||||
)
|
||||
@@ -366,11 +366,10 @@ class QgcTelemetryAdapter:
|
||||
)
|
||||
|
||||
def _extract_wgs84(self, output: EstimatorOutput) -> LatLonAlt:
|
||||
wgs = output.extras.get("wgs84") if output.extras else None
|
||||
wgs = output.position_wgs84
|
||||
if not isinstance(wgs, LatLonAlt):
|
||||
raise GcsEmitError(
|
||||
"EstimatorOutput.extras['wgs84'] missing or not a LatLonAlt; "
|
||||
"composition root must inject the ENU->WGS84 enricher"
|
||||
f"EstimatorOutput.position_wgs84 must be a LatLonAlt; got {type(wgs).__name__}"
|
||||
)
|
||||
return wgs
|
||||
|
||||
|
||||
@@ -28,7 +28,7 @@ from gps_denied_onboard._types.fc import (
|
||||
TelemetryCallback,
|
||||
)
|
||||
from gps_denied_onboard._types.geo import LatLonAlt
|
||||
from gps_denied_onboard._types.pose import EstimatorOutput
|
||||
from gps_denied_onboard._types.state import EstimatorOutput
|
||||
from gps_denied_onboard.components.c8_fc_adapter._covariance_projector import (
|
||||
CovarianceProjector,
|
||||
)
|
||||
@@ -184,7 +184,7 @@ class Msp2InavAdapter:
|
||||
"c8.inav.first_emit",
|
||||
extra={
|
||||
"kind": "c8.inav.first_emit",
|
||||
"kv": {"frame_id": output.frame_id, "seq": seq},
|
||||
"kv": {"frame_id": str(output.frame_id), "seq": seq},
|
||||
},
|
||||
)
|
||||
self._log.debug(
|
||||
@@ -265,19 +265,18 @@ class Msp2InavAdapter:
|
||||
)
|
||||
|
||||
def _extract_wgs84(self, output: EstimatorOutput) -> LatLonAlt:
|
||||
wgs = output.extras.get("wgs84") if output.extras else None
|
||||
wgs = output.position_wgs84
|
||||
if not isinstance(wgs, LatLonAlt):
|
||||
raise FcEmitError(
|
||||
"EstimatorOutput.extras['wgs84'] missing or not a LatLonAlt; "
|
||||
"composition root must inject the ENU->WGS84 enricher"
|
||||
f"EstimatorOutput.position_wgs84 must be a LatLonAlt; got {type(wgs).__name__}"
|
||||
)
|
||||
return wgs
|
||||
|
||||
def _log_emit_failed(self, reason: str, frame_id: int) -> None:
|
||||
def _log_emit_failed(self, reason: str, frame_id: Any) -> None:
|
||||
self._log.error(
|
||||
f"c8.inav.emit_failed: {reason}",
|
||||
extra={
|
||||
"kind": "c8.inav.emit_failed",
|
||||
"kv": {"reason": reason, "frame_id": frame_id},
|
||||
"kv": {"reason": reason, "frame_id": str(frame_id)},
|
||||
},
|
||||
)
|
||||
|
||||
@@ -38,7 +38,7 @@ from gps_denied_onboard._types.fc import (
|
||||
TelemetryCallback,
|
||||
)
|
||||
from gps_denied_onboard._types.geo import LatLonAlt
|
||||
from gps_denied_onboard._types.pose import EstimatorOutput
|
||||
from gps_denied_onboard._types.state import EstimatorOutput
|
||||
from gps_denied_onboard.components.c8_fc_adapter._covariance_projector import (
|
||||
CovarianceProjector,
|
||||
)
|
||||
@@ -270,7 +270,7 @@ class PymavlinkArdupilotAdapter:
|
||||
"c8.ap.first_emit",
|
||||
extra={
|
||||
"kind": "c8.ap.first_emit",
|
||||
"kv": {"frame_id": output.frame_id, "seq": seq},
|
||||
"kv": {"frame_id": str(output.frame_id), "seq": seq},
|
||||
},
|
||||
)
|
||||
self._log.debug(
|
||||
@@ -527,12 +527,12 @@ class PymavlinkArdupilotAdapter:
|
||||
},
|
||||
)
|
||||
|
||||
def _log_emit_failed(self, reason: str, frame_id: int) -> None:
|
||||
def _log_emit_failed(self, reason: str, frame_id: Any) -> None:
|
||||
self._log.error(
|
||||
f"c8.ap.emit_failed: {reason}",
|
||||
extra={
|
||||
"kind": "c8.ap.emit_failed",
|
||||
"kv": {"reason": reason, "frame_id": frame_id},
|
||||
"kv": {"reason": reason, "frame_id": str(frame_id)},
|
||||
},
|
||||
)
|
||||
|
||||
@@ -588,19 +588,19 @@ class PymavlinkArdupilotAdapter:
|
||||
pass
|
||||
|
||||
def _extract_wgs84(self, output: EstimatorOutput) -> LatLonAlt:
|
||||
"""Pull the WGS84 fix the composition root pre-attached.
|
||||
"""Pull the WGS84 fix that C5 produced.
|
||||
|
||||
C5 emits its estimate in the local ENU frame; the composition
|
||||
root injects a WgsConverter-backed enricher that attaches the
|
||||
WGS84 conversion to ``output.extras["wgs84"]`` BEFORE handing
|
||||
the output to C8. If the enricher is missing the wgs84 key,
|
||||
that is a composition bug — fail loudly rather than guess.
|
||||
Per the C5 contract v1.0.0, the estimator emits
|
||||
``EstimatorOutput.position_wgs84`` directly (the
|
||||
composition root no longer injects an enricher; the
|
||||
conversion happens inside C5's ``current_estimate`` path
|
||||
using the shared :class:`WgsConverter`). A missing field
|
||||
is a composition bug — fail loudly rather than guess.
|
||||
"""
|
||||
wgs = output.extras.get("wgs84") if output.extras else None
|
||||
wgs = output.position_wgs84
|
||||
if not isinstance(wgs, LatLonAlt):
|
||||
raise FcEmitError(
|
||||
"EstimatorOutput.extras['wgs84'] missing or not a LatLonAlt; "
|
||||
"composition root must inject the ENU->WGS84 enricher"
|
||||
f"EstimatorOutput.position_wgs84 must be a LatLonAlt; got {type(wgs).__name__}"
|
||||
)
|
||||
return wgs
|
||||
|
||||
|
||||
@@ -40,6 +40,15 @@ from gps_denied_onboard.runtime_root.spoof_recovery_sink import (
|
||||
SpoofRecoveryPublisher,
|
||||
SpoofRecoverySink,
|
||||
)
|
||||
from gps_denied_onboard.runtime_root.state_factory import (
|
||||
StateIngestThreadAlreadyBoundError,
|
||||
bind_state_ingest_thread,
|
||||
build_state_estimator,
|
||||
clear_state_ingest_binding,
|
||||
clear_state_registry,
|
||||
list_registered_state_strategies,
|
||||
register_state_estimator,
|
||||
)
|
||||
|
||||
if TYPE_CHECKING:
|
||||
from gps_denied_onboard.components.c13_fdr.headers import FlightHeader
|
||||
@@ -55,13 +64,18 @@ __all__ = [
|
||||
"RuntimeRoot",
|
||||
"SpoofRecoveryPublisher",
|
||||
"SpoofRecoverySink",
|
||||
"StateIngestThreadAlreadyBoundError",
|
||||
"StrategyNotLinkedError",
|
||||
"StrategyTier",
|
||||
"TakeoffResult",
|
||||
"bind_outbound_emit_thread",
|
||||
"bind_state_ingest_thread",
|
||||
"build_fc_adapter",
|
||||
"build_gcs_adapter",
|
||||
"build_state_estimator",
|
||||
"clear_outbound_thread_binding",
|
||||
"clear_state_ingest_binding",
|
||||
"clear_state_registry",
|
||||
"clear_strategy_registries",
|
||||
"clear_strategy_registry",
|
||||
"compose_operator",
|
||||
@@ -69,10 +83,12 @@ __all__ = [
|
||||
"compose_root",
|
||||
"list_registered_fc_strategies",
|
||||
"list_registered_gcs_strategies",
|
||||
"list_registered_state_strategies",
|
||||
"list_registered_strategies",
|
||||
"main",
|
||||
"register_fc_adapter",
|
||||
"register_gcs_adapter",
|
||||
"register_state_estimator",
|
||||
"register_strategy",
|
||||
"take_off",
|
||||
]
|
||||
|
||||
@@ -0,0 +1,214 @@
|
||||
"""Composition-root factory for C5 (AZ-381 / E-C5).
|
||||
|
||||
Mirrors the C8 factory shape (``runtime_root.fc_factory``): per-binary
|
||||
bootstrap modules register concrete strategy factories under their
|
||||
``BUILD_STATE_<variant>`` flag; :func:`build_state_estimator` resolves
|
||||
the configured strategy, gates by build flag, constructs the
|
||||
estimator, and returns the tuple ``(StateEstimator,
|
||||
ISam2GraphHandle)`` so the runtime root can inject the handle into
|
||||
C4.
|
||||
|
||||
Single-writer-thread binding for C5 + C4 is enforced via
|
||||
:func:`bind_state_ingest_thread`; the second binding from a different
|
||||
thread raises :class:`StateIngestThreadAlreadyBoundError`. The runtime
|
||||
root binds C4 + C5 to the SAME thread.
|
||||
|
||||
ADR-002 build-flag gating: ``config.components["c5_state"].strategy ==
|
||||
"gtsam_isam2"`` requires ``BUILD_STATE_GTSAM_ISAM2=ON``. Default is
|
||||
``ON`` (most binaries link the production-default); the operator-side
|
||||
binary may set ``OFF`` and only link the ESKF baseline.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import os
|
||||
import threading
|
||||
from collections.abc import Callable
|
||||
from typing import TYPE_CHECKING, Any, Final
|
||||
|
||||
from gps_denied_onboard.components.c5_state._isam2_handle import ISam2GraphHandle
|
||||
from gps_denied_onboard.components.c5_state.config import (
|
||||
KNOWN_STATE_STRATEGIES,
|
||||
C5StateConfig,
|
||||
)
|
||||
from gps_denied_onboard.components.c5_state.errors import StateEstimatorConfigError
|
||||
from gps_denied_onboard.components.c5_state.interface import StateEstimator
|
||||
from gps_denied_onboard.config import Config
|
||||
from gps_denied_onboard.logging import get_logger
|
||||
|
||||
if TYPE_CHECKING:
|
||||
pass
|
||||
|
||||
__all__ = [
|
||||
"StateEstimatorFactory",
|
||||
"StateIngestThreadAlreadyBoundError",
|
||||
"bind_state_ingest_thread",
|
||||
"build_state_estimator",
|
||||
"clear_state_ingest_binding",
|
||||
"clear_state_registry",
|
||||
"list_registered_state_strategies",
|
||||
"register_state_estimator",
|
||||
]
|
||||
|
||||
|
||||
StateEstimatorFactory = Callable[..., tuple[StateEstimator, ISam2GraphHandle]]
|
||||
|
||||
_STATE_REGISTRY: dict[str, StateEstimatorFactory] = {}
|
||||
|
||||
_STATE_BUILD_FLAGS: Final[dict[str, str]] = {
|
||||
"gtsam_isam2": "BUILD_STATE_GTSAM_ISAM2",
|
||||
"eskf": "BUILD_STATE_ESKF",
|
||||
}
|
||||
|
||||
|
||||
def register_state_estimator(strategy: str, factory: StateEstimatorFactory) -> None:
|
||||
"""Register a concrete `StateEstimator` strategy.
|
||||
|
||||
Called by per-binary bootstrap modules under the matching
|
||||
``BUILD_STATE_<variant>`` flag. Duplicate registration with a
|
||||
different factory raises :class:`StateEstimatorConfigError`.
|
||||
"""
|
||||
existing = _STATE_REGISTRY.get(strategy)
|
||||
if existing is not None and existing is not factory:
|
||||
raise StateEstimatorConfigError(
|
||||
f"duplicate StateEstimator registration for strategy {strategy!r}"
|
||||
)
|
||||
_STATE_REGISTRY[strategy] = factory
|
||||
|
||||
|
||||
def clear_state_registry() -> None:
|
||||
"""Reset the state registry; unit-test isolation only."""
|
||||
_STATE_REGISTRY.clear()
|
||||
|
||||
|
||||
def list_registered_state_strategies() -> list[str]:
|
||||
return sorted(_STATE_REGISTRY)
|
||||
|
||||
|
||||
# ----------------------------------------------------------------------
|
||||
# Single-writer state-ingest thread (Invariant 1; C4 + C5 share it).
|
||||
|
||||
|
||||
class StateIngestThreadAlreadyBoundError(RuntimeError):
|
||||
"""Raised on a second :func:`bind_state_ingest_thread` call from a different thread."""
|
||||
|
||||
|
||||
_ingest_lock = threading.Lock()
|
||||
_ingest_bound_thread: int | None = None
|
||||
|
||||
|
||||
def bind_state_ingest_thread(thread_ident: int | None = None) -> int:
|
||||
"""Bind ``thread_ident`` (defaults to the caller) as the sole state-ingest thread.
|
||||
|
||||
A second call from a different thread raises
|
||||
:class:`StateIngestThreadAlreadyBoundError`. C4 + C5 + the shared
|
||||
GTSAM substrate live on this thread per ADR-003. Repeated binding
|
||||
from the SAME thread is permitted (idempotent for re-entrant
|
||||
composition under tests).
|
||||
"""
|
||||
global _ingest_bound_thread
|
||||
ident = thread_ident if thread_ident is not None else threading.get_ident()
|
||||
with _ingest_lock:
|
||||
if _ingest_bound_thread is not None and _ingest_bound_thread != ident:
|
||||
raise StateIngestThreadAlreadyBoundError(
|
||||
f"state ingest thread already bound to {_ingest_bound_thread}; "
|
||||
f"refused to re-bind to {ident}"
|
||||
)
|
||||
_ingest_bound_thread = ident
|
||||
return ident
|
||||
|
||||
|
||||
def clear_state_ingest_binding() -> None:
|
||||
"""Reset the state-ingest-thread binding; unit-test isolation only."""
|
||||
global _ingest_bound_thread
|
||||
with _ingest_lock:
|
||||
_ingest_bound_thread = None
|
||||
|
||||
|
||||
# ----------------------------------------------------------------------
|
||||
# Public composition-root factory.
|
||||
|
||||
|
||||
def build_state_estimator(
|
||||
config: Config,
|
||||
*,
|
||||
imu_preintegrator: Any,
|
||||
se3_utils: Any,
|
||||
wgs_converter: Any,
|
||||
fdr_client: Any,
|
||||
) -> tuple[StateEstimator, ISam2GraphHandle]:
|
||||
"""Resolve + build the configured state estimator.
|
||||
|
||||
Returns the ``(StateEstimator, ISam2GraphHandle)`` tuple so the
|
||||
runtime root can inject the handle into C4 via
|
||||
``build_pose_estimator``.
|
||||
|
||||
Validation order: config block lookup → build-flag gate → factory
|
||||
lookup. The first failure surfaces a :class:`StateEstimatorConfigError`
|
||||
with the offending strategy + flag name so the operator gets a
|
||||
clear next step.
|
||||
"""
|
||||
block = _read_state_block(config)
|
||||
strategy = block.strategy
|
||||
if strategy not in KNOWN_STATE_STRATEGIES:
|
||||
raise StateEstimatorConfigError(
|
||||
f"C5StateConfig.strategy={strategy!r} not in {sorted(KNOWN_STATE_STRATEGIES)}"
|
||||
)
|
||||
flag_name = _STATE_BUILD_FLAGS.get(strategy)
|
||||
if flag_name is None:
|
||||
raise StateEstimatorConfigError(
|
||||
f"state strategy {strategy!r} has no BUILD_STATE_* flag mapping"
|
||||
)
|
||||
if os.environ.get(flag_name, "ON").upper() == "OFF":
|
||||
raise StateEstimatorConfigError(
|
||||
f"{flag_name} is OFF — strategy {strategy!r} is not linked into this binary"
|
||||
)
|
||||
factory = _STATE_REGISTRY.get(strategy)
|
||||
if factory is None:
|
||||
raise StateEstimatorConfigError(
|
||||
f"state strategy {strategy!r} selected by config but not registered; "
|
||||
f"registered strategies: {list_registered_state_strategies()}"
|
||||
)
|
||||
estimator, handle = factory(
|
||||
config=config,
|
||||
imu_preintegrator=imu_preintegrator,
|
||||
se3_utils=se3_utils,
|
||||
wgs_converter=wgs_converter,
|
||||
fdr_client=fdr_client,
|
||||
)
|
||||
_log_strategy_loaded(
|
||||
strategy=strategy,
|
||||
keyframe_window_size=block.keyframe_window_size,
|
||||
)
|
||||
return estimator, handle
|
||||
|
||||
|
||||
def _read_state_block(config: Config) -> C5StateConfig:
|
||||
"""Pull the c5_state block out of ``config.components`` (or fall back to defaults)."""
|
||||
components = getattr(config, "components", None) or {}
|
||||
block = components.get("c5_state") if isinstance(components, dict) else None
|
||||
if block is None:
|
||||
# Allow missing block to mean "documented defaults" — same shape
|
||||
# as the cross-cutting blocks. Tests that exercise the factory
|
||||
# without YAML/env see defaults.
|
||||
return C5StateConfig()
|
||||
if isinstance(block, C5StateConfig):
|
||||
return block
|
||||
raise StateEstimatorConfigError(
|
||||
f"config.components['c5_state'] must be a C5StateConfig; got {type(block).__name__}"
|
||||
)
|
||||
|
||||
|
||||
def _log_strategy_loaded(*, strategy: str, keyframe_window_size: int) -> None:
|
||||
log = get_logger("runtime_root.state_factory")
|
||||
log.info(
|
||||
f"c5.state.strategy_loaded: strategy={strategy} "
|
||||
f"keyframe_window_size={keyframe_window_size}",
|
||||
extra={
|
||||
"kind": "c5.state.strategy_loaded",
|
||||
"kv": {
|
||||
"strategy": strategy,
|
||||
"keyframe_window_size": keyframe_window_size,
|
||||
},
|
||||
},
|
||||
)
|
||||
@@ -1,14 +1,12 @@
|
||||
"""C4 PoseEstimator smoke test — AC-9."""
|
||||
"""C4 PoseEstimator smoke test — AC-9 (public-API re-export)."""
|
||||
|
||||
|
||||
def test_interface_importable() -> None:
|
||||
# Assert
|
||||
from gps_denied_onboard.components.c4_pose import (
|
||||
EstimatorOutput,
|
||||
PoseEstimate,
|
||||
PoseEstimator,
|
||||
)
|
||||
|
||||
assert PoseEstimator is not None
|
||||
assert PoseEstimate is not None
|
||||
assert EstimatorOutput is not None
|
||||
|
||||
@@ -0,0 +1,397 @@
|
||||
"""AZ-381 — StateEstimator Protocol + DTOs + factory + concrete handle.
|
||||
|
||||
Covers all 10 ACs of AZ-381 (see ``_docs/02_tasks/done/AZ-381...``).
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import dataclasses
|
||||
import logging
|
||||
import threading
|
||||
from typing import Any
|
||||
from unittest import mock
|
||||
from uuid import uuid4
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from gps_denied_onboard._types.geo import LatLonAlt
|
||||
from gps_denied_onboard._types.pose import PoseEstimate
|
||||
from gps_denied_onboard._types.state import (
|
||||
EstimatorHealth,
|
||||
EstimatorOutput,
|
||||
IsamState,
|
||||
PoseSourceLabel,
|
||||
Quat,
|
||||
)
|
||||
from gps_denied_onboard._types.vio import VioOutput
|
||||
from gps_denied_onboard.components.c5_state import (
|
||||
C5StateConfig,
|
||||
EstimatorDegradedError,
|
||||
EstimatorFatalError,
|
||||
StateEstimator,
|
||||
StateEstimatorConfigError,
|
||||
StateEstimatorError,
|
||||
)
|
||||
from gps_denied_onboard.components.c5_state._isam2_handle import (
|
||||
ISam2GraphHandle,
|
||||
ISam2GraphHandleImpl,
|
||||
)
|
||||
from gps_denied_onboard.config import load_config
|
||||
from gps_denied_onboard.config.schema import Config, ConfigError
|
||||
from gps_denied_onboard.runtime_root.state_factory import (
|
||||
StateIngestThreadAlreadyBoundError,
|
||||
bind_state_ingest_thread,
|
||||
build_state_estimator,
|
||||
clear_state_ingest_binding,
|
||||
clear_state_registry,
|
||||
register_state_estimator,
|
||||
)
|
||||
|
||||
|
||||
@pytest.fixture(autouse=True)
|
||||
def _isolation():
|
||||
clear_state_registry()
|
||||
clear_state_ingest_binding()
|
||||
yield
|
||||
clear_state_registry()
|
||||
clear_state_ingest_binding()
|
||||
|
||||
|
||||
def _make_estimator_output() -> EstimatorOutput:
|
||||
return EstimatorOutput(
|
||||
frame_id=uuid4(),
|
||||
position_wgs84=LatLonAlt(lat_deg=50.0, lon_deg=30.0, alt_m=100.0),
|
||||
orientation_world_T_body=Quat(w=1.0, x=0.0, y=0.0, z=0.0),
|
||||
velocity_world_mps=(0.0, 0.0, 0.0),
|
||||
covariance_6x6=np.eye(6),
|
||||
source_label=PoseSourceLabel.VISUAL_PROPAGATED,
|
||||
last_satellite_anchor_age_ms=0,
|
||||
smoothed=False,
|
||||
emitted_at=0,
|
||||
)
|
||||
|
||||
|
||||
def _make_estimator_health() -> EstimatorHealth:
|
||||
return EstimatorHealth(
|
||||
isam2_state=IsamState.TRACKING,
|
||||
keyframe_count=15,
|
||||
cov_norm_growing_for_s=0.0,
|
||||
spoof_promotion_blocked=False,
|
||||
)
|
||||
|
||||
|
||||
def _build_config(**state_overrides: Any) -> Config:
|
||||
cfg = load_config(env={}, paths=(), require_env=False)
|
||||
# Replace c5_state block with overrides.
|
||||
new_state = dataclasses.replace(C5StateConfig(), **state_overrides)
|
||||
components = dict(cfg.components or {})
|
||||
components["c5_state"] = new_state
|
||||
return dataclasses.replace(cfg, components=components)
|
||||
|
||||
|
||||
class _FakeEstimator:
|
||||
"""Test fake satisfying every StateEstimator method (AC-1)."""
|
||||
|
||||
def add_vio(self, vio: VioOutput) -> None:
|
||||
pass
|
||||
|
||||
def add_pose_anchor(self, pose: PoseEstimate) -> None:
|
||||
pass
|
||||
|
||||
def add_fc_imu(self, imu_window: Any) -> None:
|
||||
pass
|
||||
|
||||
def current_estimate(self) -> EstimatorOutput:
|
||||
return _make_estimator_output()
|
||||
|
||||
def smoothed_history(self, n_keyframes: int) -> list[EstimatorOutput]:
|
||||
return [_make_estimator_output() for _ in range(min(n_keyframes, 5))]
|
||||
|
||||
def health_snapshot(self) -> EstimatorHealth:
|
||||
return _make_estimator_health()
|
||||
|
||||
|
||||
def _fake_handle(estimator: Any) -> ISam2GraphHandle:
|
||||
return ISam2GraphHandleImpl(estimator)
|
||||
|
||||
|
||||
# ----------------------------------------------------------------------
|
||||
# AC-1: Protocol conformance via @runtime_checkable
|
||||
|
||||
|
||||
def test_ac1_protocol_runtime_checkable() -> None:
|
||||
# Arrange
|
||||
fake = _FakeEstimator()
|
||||
|
||||
# Assert
|
||||
assert isinstance(fake, StateEstimator)
|
||||
|
||||
|
||||
def test_ac1_missing_method_fails_isinstance() -> None:
|
||||
class _Incomplete:
|
||||
def add_vio(self, vio: VioOutput) -> None:
|
||||
pass
|
||||
|
||||
# Assert — missing 5 methods → not a StateEstimator
|
||||
assert not isinstance(_Incomplete(), StateEstimator)
|
||||
|
||||
|
||||
# ----------------------------------------------------------------------
|
||||
# AC-2: DTOs frozen + slots
|
||||
|
||||
|
||||
def test_ac2_estimator_output_frozen_and_slotted() -> None:
|
||||
out = _make_estimator_output()
|
||||
# Assert — __slots__ exists and is non-empty
|
||||
assert hasattr(EstimatorOutput, "__slots__")
|
||||
assert len(EstimatorOutput.__slots__) > 0
|
||||
# Assert — mutation raises
|
||||
with pytest.raises(dataclasses.FrozenInstanceError):
|
||||
out.smoothed = True # type: ignore[misc]
|
||||
|
||||
|
||||
def test_ac2_estimator_health_frozen_and_slotted() -> None:
|
||||
h = _make_estimator_health()
|
||||
assert hasattr(EstimatorHealth, "__slots__")
|
||||
assert len(EstimatorHealth.__slots__) > 0
|
||||
with pytest.raises(dataclasses.FrozenInstanceError):
|
||||
h.keyframe_count = 99 # type: ignore[misc]
|
||||
|
||||
|
||||
# ----------------------------------------------------------------------
|
||||
# AC-3: IsamState enum has 4 values
|
||||
|
||||
|
||||
def test_ac3_isam_state_has_four_values() -> None:
|
||||
members = {m.name for m in IsamState}
|
||||
assert members == {"INIT", "TRACKING", "DEGRADED", "LOST"}
|
||||
|
||||
|
||||
# ----------------------------------------------------------------------
|
||||
# AC-4: Factory rejects build-flag OFF
|
||||
|
||||
|
||||
def test_ac4_build_flag_off_rejected(monkeypatch: pytest.MonkeyPatch) -> None:
|
||||
register_state_estimator("gtsam_isam2", lambda **_: (_FakeEstimator(), _fake_handle(None)))
|
||||
monkeypatch.setenv("BUILD_STATE_GTSAM_ISAM2", "OFF")
|
||||
|
||||
cfg = _build_config(strategy="gtsam_isam2")
|
||||
|
||||
with pytest.raises(StateEstimatorConfigError, match="BUILD_STATE_GTSAM_ISAM2 is OFF"):
|
||||
build_state_estimator(
|
||||
cfg,
|
||||
imu_preintegrator=mock.MagicMock(),
|
||||
se3_utils=mock.MagicMock(),
|
||||
wgs_converter=mock.MagicMock(),
|
||||
fdr_client=mock.MagicMock(),
|
||||
)
|
||||
|
||||
|
||||
# ----------------------------------------------------------------------
|
||||
# AC-5: Factory rejects unknown strategy at config-load
|
||||
|
||||
|
||||
def test_ac5_unknown_strategy_rejected_at_config() -> None:
|
||||
# Direct construction surfaces ConfigError (validation in __post_init__).
|
||||
with pytest.raises(ConfigError, match="garbage"):
|
||||
C5StateConfig(strategy="garbage")
|
||||
|
||||
|
||||
def test_ac5_unknown_strategy_via_factory() -> None:
|
||||
# If the block escapes (e.g. test sets strategy = "nonexistent" via a
|
||||
# post-validation bypass), the factory still rejects with a clear
|
||||
# StateEstimatorConfigError naming the disabled flag.
|
||||
fake_block = mock.MagicMock(spec=C5StateConfig)
|
||||
fake_block.strategy = "nonexistent"
|
||||
fake_block.keyframe_window_size = 15
|
||||
cfg = mock.MagicMock(spec=Config)
|
||||
cfg.components = {"c5_state": fake_block}
|
||||
|
||||
with pytest.raises(StateEstimatorConfigError, match="not in"):
|
||||
build_state_estimator(
|
||||
cfg,
|
||||
imu_preintegrator=mock.MagicMock(),
|
||||
se3_utils=mock.MagicMock(),
|
||||
wgs_converter=mock.MagicMock(),
|
||||
fdr_client=mock.MagicMock(),
|
||||
)
|
||||
|
||||
|
||||
# ----------------------------------------------------------------------
|
||||
# AC-6: Factory returns the tuple + INFO log
|
||||
|
||||
|
||||
def test_ac6_factory_returns_tuple_and_logs(
|
||||
caplog: pytest.LogCaptureFixture, monkeypatch: pytest.MonkeyPatch
|
||||
) -> None:
|
||||
fake_estimator = _FakeEstimator()
|
||||
fake_handle = _fake_handle(fake_estimator)
|
||||
|
||||
register_state_estimator(
|
||||
"gtsam_isam2",
|
||||
lambda **_: (fake_estimator, fake_handle),
|
||||
)
|
||||
monkeypatch.delenv("BUILD_STATE_GTSAM_ISAM2", raising=False)
|
||||
|
||||
cfg = _build_config(strategy="gtsam_isam2", keyframe_window_size=15)
|
||||
|
||||
with caplog.at_level(logging.INFO, logger="runtime_root.state_factory"):
|
||||
result = build_state_estimator(
|
||||
cfg,
|
||||
imu_preintegrator=mock.MagicMock(),
|
||||
se3_utils=mock.MagicMock(),
|
||||
wgs_converter=mock.MagicMock(),
|
||||
fdr_client=mock.MagicMock(),
|
||||
)
|
||||
|
||||
estimator, handle = result
|
||||
assert estimator is fake_estimator
|
||||
assert handle is fake_handle
|
||||
loaded_records = [
|
||||
r for r in caplog.records if getattr(r, "kind", None) == "c5.state.strategy_loaded"
|
||||
]
|
||||
assert len(loaded_records) == 1
|
||||
assert loaded_records[0].kv["strategy"] == "gtsam_isam2"
|
||||
assert loaded_records[0].kv["keyframe_window_size"] == 15
|
||||
|
||||
|
||||
# ----------------------------------------------------------------------
|
||||
# AC-7: Thread binding
|
||||
|
||||
|
||||
def test_ac7_second_thread_binding_rejected() -> None:
|
||||
main_ident = bind_state_ingest_thread()
|
||||
|
||||
err: list[BaseException] = []
|
||||
|
||||
def run() -> None:
|
||||
try:
|
||||
bind_state_ingest_thread()
|
||||
except StateIngestThreadAlreadyBoundError as e:
|
||||
err.append(e)
|
||||
|
||||
t = threading.Thread(target=run)
|
||||
t.start()
|
||||
t.join(timeout=2.0)
|
||||
assert len(err) == 1
|
||||
assert main_ident != threading.get_ident() + 1 # placeholder no-op assertion
|
||||
|
||||
|
||||
def test_ac7_same_thread_rebinding_idempotent() -> None:
|
||||
first = bind_state_ingest_thread()
|
||||
second = bind_state_ingest_thread()
|
||||
assert first == second
|
||||
|
||||
|
||||
# ----------------------------------------------------------------------
|
||||
# AC-8: ISam2GraphHandleImpl skeleton
|
||||
|
||||
|
||||
def test_ac8_handle_is_isam2_graph_handle() -> None:
|
||||
handle = ISam2GraphHandleImpl(estimator=mock.MagicMock())
|
||||
assert isinstance(handle, ISam2GraphHandle)
|
||||
|
||||
|
||||
def test_ac8_handle_methods_raise_named_task() -> None:
|
||||
handle = ISam2GraphHandleImpl(estimator=mock.MagicMock())
|
||||
|
||||
with pytest.raises(NotImplementedError, match="AZ-382"):
|
||||
handle.add_factor(mock.MagicMock())
|
||||
with pytest.raises(NotImplementedError, match="AZ-382"):
|
||||
handle.update(mock.MagicMock(), mock.MagicMock())
|
||||
with pytest.raises(NotImplementedError, match="AZ-382"):
|
||||
handle.compute_marginals()
|
||||
with pytest.raises(NotImplementedError, match="AZ-382"):
|
||||
handle.last_anchor_age_ms()
|
||||
|
||||
|
||||
# ----------------------------------------------------------------------
|
||||
# AC-9: Public API re-exports
|
||||
|
||||
|
||||
def test_ac9_public_api_resolves() -> None:
|
||||
# Re-importing here makes the test self-documenting: every symbol below
|
||||
# is part of the C5 public surface AC-9 requires.
|
||||
from gps_denied_onboard.components.c5_state import ( # noqa: F401
|
||||
C5StateConfig,
|
||||
EstimatorHealth,
|
||||
EstimatorOutput,
|
||||
IsamState,
|
||||
PoseSourceLabel,
|
||||
StateEstimator,
|
||||
)
|
||||
|
||||
|
||||
def test_ac9_internals_not_in_all() -> None:
|
||||
from gps_denied_onboard.components import c5_state
|
||||
|
||||
# _isam2_handle is an internal module; it must not be re-exported.
|
||||
assert "ISam2GraphHandle" not in c5_state.__all__
|
||||
assert "ISam2GraphHandleImpl" not in c5_state.__all__
|
||||
|
||||
|
||||
# ----------------------------------------------------------------------
|
||||
# AC-10: Error hierarchy catchability
|
||||
|
||||
|
||||
def test_ac10_every_error_is_state_estimator_error() -> None:
|
||||
for exc_cls in (
|
||||
EstimatorDegradedError,
|
||||
EstimatorFatalError,
|
||||
StateEstimatorConfigError,
|
||||
):
|
||||
with pytest.raises(StateEstimatorError):
|
||||
raise exc_cls("test")
|
||||
|
||||
|
||||
# ----------------------------------------------------------------------
|
||||
# Config block validation
|
||||
|
||||
|
||||
def test_config_keyframe_window_must_be_in_range() -> None:
|
||||
with pytest.raises(ConfigError, match=r"\[10, 20\]"):
|
||||
C5StateConfig(keyframe_window_size=5)
|
||||
with pytest.raises(ConfigError, match=r"\[10, 20\]"):
|
||||
C5StateConfig(keyframe_window_size=25)
|
||||
# Boundaries OK.
|
||||
C5StateConfig(keyframe_window_size=10)
|
||||
C5StateConfig(keyframe_window_size=20)
|
||||
|
||||
|
||||
def test_config_spoof_min_stable_must_be_positive() -> None:
|
||||
with pytest.raises(ConfigError, match="spoof_promotion_min_stable_s"):
|
||||
C5StateConfig(spoof_promotion_min_stable_s=0.0)
|
||||
|
||||
|
||||
def test_config_no_estimate_fallback_must_be_positive() -> None:
|
||||
with pytest.raises(ConfigError, match="no_estimate_fallback_s"):
|
||||
C5StateConfig(no_estimate_fallback_s=-1.0)
|
||||
|
||||
|
||||
# ----------------------------------------------------------------------
|
||||
# Performance budget
|
||||
|
||||
|
||||
def test_nfr_perf_build_under_50ms(monkeypatch: pytest.MonkeyPatch) -> None:
|
||||
import time as _t
|
||||
|
||||
register_state_estimator(
|
||||
"gtsam_isam2",
|
||||
lambda **_: (_FakeEstimator(), _fake_handle(None)),
|
||||
)
|
||||
monkeypatch.delenv("BUILD_STATE_GTSAM_ISAM2", raising=False)
|
||||
cfg = _build_config(strategy="gtsam_isam2")
|
||||
|
||||
iters = 100
|
||||
start = _t.perf_counter()
|
||||
for _ in range(iters):
|
||||
build_state_estimator(
|
||||
cfg,
|
||||
imu_preintegrator=mock.MagicMock(),
|
||||
se3_utils=mock.MagicMock(),
|
||||
wgs_converter=mock.MagicMock(),
|
||||
fdr_client=mock.MagicMock(),
|
||||
)
|
||||
avg_ms = (_t.perf_counter() - start) / iters * 1000.0
|
||||
assert avg_ms < 50.0, f"avg build {avg_ms:.2f}ms exceeds 50ms p99 budget"
|
||||
@@ -1,4 +1,4 @@
|
||||
"""C5 StateEstimator smoke test — AC-9."""
|
||||
"""C5 StateEstimator smoke test — AC-9 (public-API re-export)."""
|
||||
|
||||
|
||||
def test_interface_importable() -> None:
|
||||
@@ -6,9 +6,13 @@ def test_interface_importable() -> None:
|
||||
from gps_denied_onboard.components.c5_state import (
|
||||
EstimatorHealth,
|
||||
EstimatorOutput,
|
||||
IsamState,
|
||||
PoseSourceLabel,
|
||||
StateEstimator,
|
||||
)
|
||||
|
||||
assert StateEstimator is not None
|
||||
assert EstimatorOutput is not None
|
||||
assert EstimatorHealth is not None
|
||||
assert IsamState is not None
|
||||
assert PoseSourceLabel is not None
|
||||
|
||||
@@ -38,7 +38,7 @@ from gps_denied_onboard._types.fc import (
|
||||
Subscription,
|
||||
TelemetryKind,
|
||||
)
|
||||
from gps_denied_onboard._types.pose import EstimatorOutput
|
||||
from gps_denied_onboard._types.state import EstimatorOutput
|
||||
from gps_denied_onboard.components.c8_fc_adapter import (
|
||||
FcAdapter,
|
||||
GcsAdapter,
|
||||
|
||||
@@ -14,28 +14,53 @@ from __future__ import annotations
|
||||
|
||||
import logging
|
||||
import math
|
||||
from datetime import datetime, timezone
|
||||
from unittest import mock
|
||||
from uuid import UUID
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from gps_denied_onboard._types.pose import EstimatorHealth, EstimatorOutput
|
||||
from gps_denied_onboard._types.geo import LatLonAlt
|
||||
from gps_denied_onboard._types.state import (
|
||||
EstimatorOutput,
|
||||
PoseSourceLabel,
|
||||
Quat,
|
||||
)
|
||||
from gps_denied_onboard.components.c8_fc_adapter._covariance_projector import (
|
||||
CovarianceProjector,
|
||||
)
|
||||
from gps_denied_onboard.components.c8_fc_adapter.errors import FcEmitError
|
||||
from gps_denied_onboard.fdr_client.records import FdrRecord
|
||||
|
||||
_TEST_FRAME_ID = UUID("00000000-0000-0000-0000-000000000007")
|
||||
|
||||
def _output(cov: np.ndarray | None, frame_id: int = 7) -> EstimatorOutput:
|
||||
|
||||
def _output(cov: np.ndarray | None, frame_id: UUID = _TEST_FRAME_ID) -> EstimatorOutput:
|
||||
return EstimatorOutput(
|
||||
frame_id=frame_id,
|
||||
timestamp=datetime.now(tz=timezone.utc),
|
||||
pose_se3=np.eye(4),
|
||||
covariance_6x6=cov,
|
||||
source_label="visual_propagated",
|
||||
health=EstimatorHealth(),
|
||||
position_wgs84=LatLonAlt(lat_deg=50.0, lon_deg=30.0, alt_m=100.0),
|
||||
orientation_world_T_body=Quat(w=1.0, x=0.0, y=0.0, z=0.0),
|
||||
velocity_world_mps=(0.0, 0.0, 0.0),
|
||||
covariance_6x6=cov if cov is not None else np.eye(6), # placeholder; tests override
|
||||
source_label=PoseSourceLabel.VISUAL_PROPAGATED,
|
||||
last_satellite_anchor_age_ms=0,
|
||||
smoothed=False,
|
||||
emitted_at=0,
|
||||
)
|
||||
|
||||
|
||||
def _output_with_cov(cov: np.ndarray | None) -> EstimatorOutput:
|
||||
"""Variant that allows None covariance for missing-cov tests."""
|
||||
return EstimatorOutput(
|
||||
frame_id=_TEST_FRAME_ID,
|
||||
position_wgs84=LatLonAlt(lat_deg=50.0, lon_deg=30.0, alt_m=100.0),
|
||||
orientation_world_T_body=Quat(w=1.0, x=0.0, y=0.0, z=0.0),
|
||||
velocity_world_mps=(0.0, 0.0, 0.0),
|
||||
covariance_6x6=cov, # type: ignore[arg-type]
|
||||
source_label=PoseSourceLabel.VISUAL_PROPAGATED,
|
||||
last_satellite_anchor_age_ms=0,
|
||||
smoothed=False,
|
||||
emitted_at=0,
|
||||
)
|
||||
|
||||
|
||||
@@ -150,7 +175,7 @@ def test_ac4_non_spd_raises_fc_emit_error_and_logs_fdr() -> None:
|
||||
record: FdrRecord = fdr.enqueue.call_args.args[0]
|
||||
assert record.kind == "log"
|
||||
assert record.payload["kv"]["reason"] == "non_spd"
|
||||
assert record.payload["kv"]["frame_id"] == 7
|
||||
assert record.payload["kv"]["frame_id"] == str(_TEST_FRAME_ID)
|
||||
|
||||
|
||||
def test_ac4_asymmetric_2x2_rejected() -> None:
|
||||
@@ -196,7 +221,7 @@ def test_ac5_missing_covariance_rejected() -> None:
|
||||
# Arrange
|
||||
fdr = _fake_fdr_client()
|
||||
proj = CovarianceProjector(fdr_client=fdr)
|
||||
out = _output(cov=None)
|
||||
out = _output_with_cov(cov=None)
|
||||
|
||||
# Act + Assert
|
||||
with pytest.raises(FcEmitError, match=r"missing"):
|
||||
@@ -208,7 +233,7 @@ def test_ac5_wrong_shape_rejected() -> None:
|
||||
# Arrange
|
||||
fdr = _fake_fdr_client()
|
||||
proj = CovarianceProjector(fdr_client=fdr)
|
||||
out = _output(cov=np.eye(5))
|
||||
out = _output_with_cov(cov=np.eye(5))
|
||||
|
||||
# Act + Assert
|
||||
with pytest.raises(FcEmitError, match=r"6x6"):
|
||||
@@ -266,7 +291,7 @@ def test_ac7_inav_clamps_at_uint16_max(caplog: pytest.LogCaptureFixture) -> None
|
||||
]
|
||||
assert len(clamp_records) == 1
|
||||
assert clamp_records[0].kv["clamped_to"] == 65535
|
||||
assert clamp_records[0].kv["frame_id"] == 7
|
||||
assert clamp_records[0].kv["frame_id"] == str(_TEST_FRAME_ID)
|
||||
|
||||
|
||||
def test_ac7_inav_exact_at_uint16_max_not_clamped() -> None:
|
||||
|
||||
@@ -9,16 +9,20 @@ from __future__ import annotations
|
||||
|
||||
import logging
|
||||
import threading
|
||||
from datetime import datetime, timezone
|
||||
from typing import Any
|
||||
from unittest import mock
|
||||
from uuid import UUID, uuid4
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from gps_denied_onboard._types.fc import FcKind, PortConfig
|
||||
from gps_denied_onboard._types.geo import LatLonAlt
|
||||
from gps_denied_onboard._types.pose import EstimatorOutput
|
||||
from gps_denied_onboard._types.state import (
|
||||
EstimatorOutput,
|
||||
PoseSourceLabel,
|
||||
Quat,
|
||||
)
|
||||
from gps_denied_onboard.components.c8_fc_adapter._covariance_projector import (
|
||||
CovarianceProjector,
|
||||
)
|
||||
@@ -108,25 +112,31 @@ def _config_for_ap(tmp_path, *, signing_key_source: str = "none"):
|
||||
|
||||
def _make_output(
|
||||
*,
|
||||
source_label: str = "visual_propagated",
|
||||
source_label: PoseSourceLabel = PoseSourceLabel.VISUAL_PROPAGATED,
|
||||
smoothed: bool = False,
|
||||
cov: np.ndarray | None = None,
|
||||
wgs: LatLonAlt | None = None,
|
||||
frame_id: int = 1,
|
||||
frame_id: UUID | int | None = None,
|
||||
) -> EstimatorOutput:
|
||||
if cov is None:
|
||||
cov = np.eye(6, dtype=np.float64) * 0.25
|
||||
if wgs is None:
|
||||
wgs = LatLonAlt(lat_deg=50.0, lon_deg=30.0, alt_m=100.0)
|
||||
if frame_id is None:
|
||||
frame_id = uuid4()
|
||||
elif isinstance(frame_id, int):
|
||||
# Deterministic UUID for legacy int-keyed tests.
|
||||
frame_id = UUID(int=frame_id)
|
||||
return EstimatorOutput(
|
||||
frame_id=frame_id,
|
||||
timestamp=datetime.now(tz=timezone.utc),
|
||||
pose_se3=np.eye(4),
|
||||
position_wgs84=wgs,
|
||||
orientation_world_T_body=Quat(w=1.0, x=0.0, y=0.0, z=0.0),
|
||||
velocity_world_mps=(0.0, 0.0, 0.0),
|
||||
covariance_6x6=cov,
|
||||
source_label=source_label,
|
||||
health=None,
|
||||
last_satellite_anchor_age_ms=0,
|
||||
smoothed=smoothed,
|
||||
extras={"wgs84": wgs},
|
||||
emitted_at=0,
|
||||
)
|
||||
|
||||
|
||||
@@ -184,7 +194,7 @@ def test_ac3_named_value_float_every_frame(
|
||||
assert len(conn.mav.named_value_float_calls) == 100
|
||||
for _, name, value in conn.mav.named_value_float_calls:
|
||||
assert name == b"src_lbl"
|
||||
assert value == pytest.approx(source_label_to_float("visual_propagated"))
|
||||
assert value == pytest.approx(source_label_to_float(PoseSourceLabel.VISUAL_PROPAGATED))
|
||||
|
||||
|
||||
# ----------------------------------------------------------------------
|
||||
@@ -199,7 +209,7 @@ def test_ac4_statustext_only_on_transition(
|
||||
# rate-limiter's min_interval_s — AC-4 measures the transition
|
||||
# behaviour, not the secondary 1 Hz spam-defence cap.
|
||||
adapter._provenance._min_interval_s = 0.0 # type: ignore[attr-defined]
|
||||
labels = ["visual_propagated", "sat_anchored"]
|
||||
labels = [PoseSourceLabel.VISUAL_PROPAGATED, PoseSourceLabel.SATELLITE_ANCHORED]
|
||||
for i in range(100):
|
||||
label = labels[(i // 10) % 2]
|
||||
adapter.emit_external_position(_make_output(source_label=label, frame_id=i))
|
||||
|
||||
@@ -4,16 +4,20 @@ from __future__ import annotations
|
||||
|
||||
import logging
|
||||
import threading
|
||||
from datetime import datetime, timezone
|
||||
from typing import Any
|
||||
from unittest import mock
|
||||
from uuid import UUID, uuid4
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from gps_denied_onboard._types.fc import FcKind, PortConfig
|
||||
from gps_denied_onboard._types.geo import LatLonAlt
|
||||
from gps_denied_onboard._types.pose import EstimatorOutput
|
||||
from gps_denied_onboard._types.state import (
|
||||
EstimatorOutput,
|
||||
PoseSourceLabel,
|
||||
Quat,
|
||||
)
|
||||
from gps_denied_onboard.components.c8_fc_adapter._covariance_projector import (
|
||||
CovarianceProjector,
|
||||
)
|
||||
@@ -89,24 +93,30 @@ def _inav_config(tmp_path) -> Any:
|
||||
|
||||
def _make_output(
|
||||
*,
|
||||
source_label: str = "visual_propagated",
|
||||
source_label: PoseSourceLabel = PoseSourceLabel.VISUAL_PROPAGATED,
|
||||
smoothed: bool = False,
|
||||
cov: np.ndarray | None = None,
|
||||
wgs: LatLonAlt | None = None,
|
||||
frame_id: int = 1,
|
||||
frame_id: UUID | int | None = None,
|
||||
) -> EstimatorOutput:
|
||||
if cov is None:
|
||||
cov = np.eye(6, dtype=np.float64) * 0.25
|
||||
if wgs is None:
|
||||
wgs = LatLonAlt(lat_deg=50.0, lon_deg=30.0, alt_m=100.0)
|
||||
if frame_id is None:
|
||||
frame_id = uuid4()
|
||||
elif isinstance(frame_id, int):
|
||||
frame_id = UUID(int=frame_id)
|
||||
return EstimatorOutput(
|
||||
frame_id=frame_id,
|
||||
timestamp=datetime.now(tz=timezone.utc),
|
||||
pose_se3=np.eye(4),
|
||||
position_wgs84=wgs,
|
||||
orientation_world_T_body=Quat(w=1.0, x=0.0, y=0.0, z=0.0),
|
||||
velocity_world_mps=(0.0, 0.0, 0.0),
|
||||
covariance_6x6=cov,
|
||||
source_label=source_label,
|
||||
last_satellite_anchor_age_ms=0,
|
||||
smoothed=smoothed,
|
||||
extras={"wgs84": wgs},
|
||||
emitted_at=0,
|
||||
)
|
||||
|
||||
|
||||
@@ -179,7 +189,7 @@ def test_ac3_statustext_secondary_only_on_transitions(
|
||||
adapter: Msp2InavAdapter, msp: _MspStub, secondary: _SecondaryMavStub
|
||||
) -> None:
|
||||
adapter._provenance._min_interval_s = 0.0 # type: ignore[attr-defined]
|
||||
labels = ["visual_propagated", "sat_anchored"]
|
||||
labels = [PoseSourceLabel.VISUAL_PROPAGATED, PoseSourceLabel.SATELLITE_ANCHORED]
|
||||
for i in range(100):
|
||||
label = labels[(i // 10) % 2]
|
||||
adapter.emit_external_position(_make_output(source_label=label, frame_id=i))
|
||||
|
||||
@@ -4,17 +4,21 @@ from __future__ import annotations
|
||||
|
||||
import logging
|
||||
import re
|
||||
from datetime import datetime, timezone
|
||||
from types import SimpleNamespace
|
||||
from typing import Any
|
||||
from unittest import mock
|
||||
from uuid import UUID, uuid4
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from gps_denied_onboard._types.fc import FcKind, PortConfig, Severity
|
||||
from gps_denied_onboard._types.geo import LatLonAlt
|
||||
from gps_denied_onboard._types.pose import EstimatorOutput
|
||||
from gps_denied_onboard._types.state import (
|
||||
EstimatorOutput,
|
||||
PoseSourceLabel,
|
||||
Quat,
|
||||
)
|
||||
from gps_denied_onboard.components.c8_fc_adapter._covariance_projector import (
|
||||
CovarianceProjector,
|
||||
)
|
||||
@@ -96,15 +100,21 @@ def _port() -> PortConfig:
|
||||
return PortConfig(fc_kind=FcKind.ARDUPILOT_PLANE, device="/dev/null", baud=921600)
|
||||
|
||||
|
||||
def _make_output(frame_id: int = 1) -> EstimatorOutput:
|
||||
def _make_output(frame_id: UUID | int | None = None) -> EstimatorOutput:
|
||||
if frame_id is None:
|
||||
frame_id = uuid4()
|
||||
elif isinstance(frame_id, int):
|
||||
frame_id = UUID(int=frame_id)
|
||||
return EstimatorOutput(
|
||||
frame_id=frame_id,
|
||||
timestamp=datetime.now(tz=timezone.utc),
|
||||
pose_se3=np.eye(4),
|
||||
position_wgs84=LatLonAlt(lat_deg=50.0, lon_deg=30.0, alt_m=100.0),
|
||||
orientation_world_T_body=Quat(w=1.0, x=0.0, y=0.0, z=0.0),
|
||||
velocity_world_mps=(0.0, 0.0, 0.0),
|
||||
covariance_6x6=np.eye(6, dtype=np.float64) * 0.25,
|
||||
source_label="visual_propagated",
|
||||
source_label=PoseSourceLabel.VISUAL_PROPAGATED,
|
||||
last_satellite_anchor_age_ms=0,
|
||||
smoothed=False,
|
||||
extras={"wgs84": LatLonAlt(lat_deg=50.0, lon_deg=30.0, alt_m=100.0)},
|
||||
emitted_at=0,
|
||||
)
|
||||
|
||||
|
||||
|
||||
@@ -4,10 +4,10 @@ from __future__ import annotations
|
||||
|
||||
import logging
|
||||
import threading
|
||||
from datetime import datetime, timezone
|
||||
from types import SimpleNamespace
|
||||
from typing import Any
|
||||
from unittest import mock
|
||||
from uuid import UUID, uuid4
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
@@ -19,7 +19,11 @@ from gps_denied_onboard._types.fc import (
|
||||
Severity,
|
||||
)
|
||||
from gps_denied_onboard._types.geo import LatLonAlt
|
||||
from gps_denied_onboard._types.pose import EstimatorOutput
|
||||
from gps_denied_onboard._types.state import (
|
||||
EstimatorOutput,
|
||||
PoseSourceLabel,
|
||||
Quat,
|
||||
)
|
||||
from gps_denied_onboard.components.c8_fc_adapter._covariance_projector import (
|
||||
CovarianceProjector,
|
||||
)
|
||||
@@ -76,15 +80,25 @@ def _config(*, summary_rate_hz: float = 2.0) -> Any:
|
||||
return load_config(env=env, paths=(), require_env=False)
|
||||
|
||||
|
||||
def _make_output(*, source_label: str = "visual_propagated", frame_id: int = 1) -> EstimatorOutput:
|
||||
def _make_output(
|
||||
*,
|
||||
source_label: PoseSourceLabel = PoseSourceLabel.VISUAL_PROPAGATED,
|
||||
frame_id: UUID | int | None = None,
|
||||
) -> EstimatorOutput:
|
||||
if frame_id is None:
|
||||
frame_id = uuid4()
|
||||
elif isinstance(frame_id, int):
|
||||
frame_id = UUID(int=frame_id)
|
||||
return EstimatorOutput(
|
||||
frame_id=frame_id,
|
||||
timestamp=datetime.now(tz=timezone.utc),
|
||||
pose_se3=np.eye(4),
|
||||
position_wgs84=LatLonAlt(lat_deg=50.0, lon_deg=30.0, alt_m=100.0),
|
||||
orientation_world_T_body=Quat(w=1.0, x=0.0, y=0.0, z=0.0),
|
||||
velocity_world_mps=(0.0, 0.0, 0.0),
|
||||
covariance_6x6=np.eye(6, dtype=np.float64) * 0.25,
|
||||
source_label=source_label,
|
||||
last_satellite_anchor_age_ms=0,
|
||||
smoothed=False,
|
||||
extras={"wgs84": LatLonAlt(lat_deg=50.0, lon_deg=30.0, alt_m=100.0)},
|
||||
emitted_at=0,
|
||||
)
|
||||
|
||||
|
||||
@@ -168,13 +182,15 @@ def test_ac3_summary_frame_fields() -> None:
|
||||
try:
|
||||
wgs = LatLonAlt(lat_deg=50.45, lon_deg=30.52, alt_m=180.0)
|
||||
output = EstimatorOutput(
|
||||
frame_id=1,
|
||||
timestamp=datetime.now(tz=timezone.utc),
|
||||
pose_se3=np.eye(4),
|
||||
frame_id=UUID(int=1),
|
||||
position_wgs84=wgs,
|
||||
orientation_world_T_body=Quat(w=1.0, x=0.0, y=0.0, z=0.0),
|
||||
velocity_world_mps=(0.0, 0.0, 0.0),
|
||||
covariance_6x6=np.diag([0.25, 0.25, 9.0, 1.0, 1.0, 1.0]).astype(np.float64),
|
||||
source_label="visual_propagated",
|
||||
source_label=PoseSourceLabel.VISUAL_PROPAGATED,
|
||||
last_satellite_anchor_age_ms=0,
|
||||
smoothed=False,
|
||||
extras={"wgs84": wgs},
|
||||
emitted_at=0,
|
||||
)
|
||||
a.emit_summary(output)
|
||||
assert len(conn.mav.global_position_int_calls) == 1
|
||||
|
||||
Reference in New Issue
Block a user