From beed43724f6295606e4918881433021f658c5dbb Mon Sep 17 00:00:00 2001 From: Oleksandr Bezdieniezhnykh Date: Mon, 11 May 2026 05:35:20 +0300 Subject: [PATCH] [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_); 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 --- .../c5_state/state_estimator_protocol.md | 4 +- .../composition_root_protocol.md | 3 +- .../AZ-381_c5_state_protocol.md | 0 .../batch_12_cycle1_report.md | 126 ++++++ _docs/_autodev_state.md | 2 +- src/gps_denied_onboard/_types/emitted.py | 7 +- src/gps_denied_onboard/_types/pose.py | 44 +- src/gps_denied_onboard/_types/state.py | 107 +++++ .../components/c4_pose/__init__.py | 4 +- .../components/c4_pose/interface.py | 2 + .../components/c5_state/__init__.py | 53 ++- .../components/c5_state/_isam2_handle.py | 84 ++++ .../components/c5_state/config.py | 75 ++++ .../components/c5_state/errors.py | 55 +++ .../components/c5_state/interface.py | 69 ++- .../c8_fc_adapter/_covariance_projector.py | 10 +- .../c8_fc_adapter/_outbound_provenance.py | 52 ++- .../components/c8_fc_adapter/interface.py | 2 +- .../c8_fc_adapter/mavlink_gcs_adapter.py | 7 +- .../c8_fc_adapter/msp2_inav_adapter.py | 13 +- .../pymavlink_ardupilot_adapter.py | 26 +- .../runtime_root/__init__.py | 16 + .../runtime_root/state_factory.py | 214 ++++++++++ tests/unit/c4_pose/test_smoke.py | 4 +- .../c5_state/test_az381_state_protocol.py | 397 ++++++++++++++++++ tests/unit/c5_state/test_smoke.py | 6 +- .../test_az390_adapter_protocol.py | 2 +- .../test_az392_covariance_projector.py | 49 ++- .../test_az393_ardupilot_outbound.py | 30 +- .../c8_fc_adapter/test_az394_inav_outbound.py | 26 +- .../test_az395_mavlink_signing.py | 24 +- .../c8_fc_adapter/test_az397_qgc_telemetry.py | 38 +- 32 files changed, 1394 insertions(+), 157 deletions(-) rename _docs/02_tasks/{todo => done}/AZ-381_c5_state_protocol.md (100%) create mode 100644 _docs/03_implementation/batch_12_cycle1_report.md create mode 100644 src/gps_denied_onboard/_types/state.py create mode 100644 src/gps_denied_onboard/components/c5_state/_isam2_handle.py create mode 100644 src/gps_denied_onboard/components/c5_state/config.py create mode 100644 src/gps_denied_onboard/components/c5_state/errors.py create mode 100644 src/gps_denied_onboard/runtime_root/state_factory.py create mode 100644 tests/unit/c5_state/test_az381_state_protocol.py diff --git a/_docs/02_document/contracts/c5_state/state_estimator_protocol.md b/_docs/02_document/contracts/c5_state/state_estimator_protocol.md index 363a143..4e4ee50 100644 --- a/_docs/02_document/contracts/c5_state/state_estimator_protocol.md +++ b/_docs/02_document/contracts/c5_state/state_estimator_protocol.md @@ -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 diff --git a/_docs/02_document/contracts/shared_config/composition_root_protocol.md b/_docs/02_document/contracts/shared_config/composition_root_protocol.md index e1f47a8..c7ee005 100644 --- a/_docs/02_document/contracts/shared_config/composition_root_protocol.md +++ b/_docs/02_document/contracts/shared_config/composition_root_protocol.md @@ -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 | diff --git a/_docs/02_tasks/todo/AZ-381_c5_state_protocol.md b/_docs/02_tasks/done/AZ-381_c5_state_protocol.md similarity index 100% rename from _docs/02_tasks/todo/AZ-381_c5_state_protocol.md rename to _docs/02_tasks/done/AZ-381_c5_state_protocol.md diff --git a/_docs/03_implementation/batch_12_cycle1_report.md b/_docs/03_implementation/batch_12_cycle1_report.md new file mode 100644 index 0000000..6211bda --- /dev/null +++ b/_docs/03_implementation/batch_12_cycle1_report.md @@ -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_`). 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_`); 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_ 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`. + diff --git a/_docs/_autodev_state.md b/_docs/_autodev_state.md index 53fd4b8..ce0490c 100644 --- a/_docs/_autodev_state.md +++ b/_docs/_autodev_state.md @@ -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 diff --git a/src/gps_denied_onboard/_types/emitted.py b/src/gps_denied_onboard/_types/emitted.py index edd171b..0c9f7a2 100644 --- a/src/gps_denied_onboard/_types/emitted.py +++ b/src/gps_denied_onboard/_types/emitted.py @@ -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 diff --git a/src/gps_denied_onboard/_types/pose.py b/src/gps_denied_onboard/_types/pose.py index 4ab3649..2044771 100644 --- a/src/gps_denied_onboard/_types/pose.py +++ b/src/gps_denied_onboard/_types/pose.py @@ -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 diff --git a/src/gps_denied_onboard/_types/state.py b/src/gps_denied_onboard/_types/state.py new file mode 100644 index 0000000..3abd45e --- /dev/null +++ b/src/gps_denied_onboard/_types/state.py @@ -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 diff --git a/src/gps_denied_onboard/components/c4_pose/__init__.py b/src/gps_denied_onboard/components/c4_pose/__init__.py index 01c71b7..fd620a1 100644 --- a/src/gps_denied_onboard/components/c4_pose/__init__.py +++ b/src/gps_denied_onboard/components/c4_pose/__init__.py @@ -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"] diff --git a/src/gps_denied_onboard/components/c4_pose/interface.py b/src/gps_denied_onboard/components/c4_pose/interface.py index 9c0c226..ed1891b 100644 --- a/src/gps_denied_onboard/components/c4_pose/interface.py +++ b/src/gps_denied_onboard/components/c4_pose/interface.py @@ -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.""" diff --git a/src/gps_denied_onboard/components/c5_state/__init__.py b/src/gps_denied_onboard/components/c5_state/__init__.py index b0c3616..2279db9 100644 --- a/src/gps_denied_onboard/components/c5_state/__init__.py +++ b/src/gps_denied_onboard/components/c5_state/__init__.py @@ -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) diff --git a/src/gps_denied_onboard/components/c5_state/_isam2_handle.py b/src/gps_denied_onboard/components/c5_state/_isam2_handle.py new file mode 100644 index 0000000..f20a872 --- /dev/null +++ b/src/gps_denied_onboard/components/c5_state/_isam2_handle.py @@ -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." + ) diff --git a/src/gps_denied_onboard/components/c5_state/config.py b/src/gps_denied_onboard/components/c5_state/config.py new file mode 100644 index 0000000..4d2e84d --- /dev/null +++ b/src/gps_denied_onboard/components/c5_state/config.py @@ -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}" + ) diff --git a/src/gps_denied_onboard/components/c5_state/errors.py b/src/gps_denied_onboard/components/c5_state/errors.py new file mode 100644 index 0000000..5f2842b --- /dev/null +++ b/src/gps_denied_onboard/components/c5_state/errors.py @@ -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. + """ diff --git a/src/gps_denied_onboard/components/c5_state/interface.py b/src/gps_denied_onboard/components/c5_state/interface.py index 823650f..bc28a60 100644 --- a/src/gps_denied_onboard/components/c5_state/interface.py +++ b/src/gps_denied_onboard/components/c5_state/interface.py @@ -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_`` 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.""" diff --git a/src/gps_denied_onboard/components/c8_fc_adapter/_covariance_projector.py b/src/gps_denied_onboard/components/c8_fc_adapter/_covariance_projector.py index 84e3639..2761c89 100644 --- a/src/gps_denied_onboard/components/c8_fc_adapter/_covariance_projector.py +++ b/src/gps_denied_onboard/components/c8_fc_adapter/_covariance_projector.py @@ -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 diff --git a/src/gps_denied_onboard/components/c8_fc_adapter/_outbound_provenance.py b/src/gps_denied_onboard/components/c8_fc_adapter/_outbound_provenance.py index 4a107ab..6b023ec 100644 --- a/src/gps_denied_onboard/components/c8_fc_adapter/_outbound_provenance.py +++ b/src/gps_denied_onboard/components/c8_fc_adapter/_outbound_provenance.py @@ -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 diff --git a/src/gps_denied_onboard/components/c8_fc_adapter/interface.py b/src/gps_denied_onboard/components/c8_fc_adapter/interface.py index c2285ed..321be06 100644 --- a/src/gps_denied_onboard/components/c8_fc_adapter/interface.py +++ b/src/gps_denied_onboard/components/c8_fc_adapter/interface.py @@ -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"] diff --git a/src/gps_denied_onboard/components/c8_fc_adapter/mavlink_gcs_adapter.py b/src/gps_denied_onboard/components/c8_fc_adapter/mavlink_gcs_adapter.py index 05e755c..b1b5b29 100644 --- a/src/gps_denied_onboard/components/c8_fc_adapter/mavlink_gcs_adapter.py +++ b/src/gps_denied_onboard/components/c8_fc_adapter/mavlink_gcs_adapter.py @@ -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 diff --git a/src/gps_denied_onboard/components/c8_fc_adapter/msp2_inav_adapter.py b/src/gps_denied_onboard/components/c8_fc_adapter/msp2_inav_adapter.py index f991934..b31c4f3 100644 --- a/src/gps_denied_onboard/components/c8_fc_adapter/msp2_inav_adapter.py +++ b/src/gps_denied_onboard/components/c8_fc_adapter/msp2_inav_adapter.py @@ -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)}, }, ) diff --git a/src/gps_denied_onboard/components/c8_fc_adapter/pymavlink_ardupilot_adapter.py b/src/gps_denied_onboard/components/c8_fc_adapter/pymavlink_ardupilot_adapter.py index 21ee85e..b205a1a 100644 --- a/src/gps_denied_onboard/components/c8_fc_adapter/pymavlink_ardupilot_adapter.py +++ b/src/gps_denied_onboard/components/c8_fc_adapter/pymavlink_ardupilot_adapter.py @@ -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 diff --git a/src/gps_denied_onboard/runtime_root/__init__.py b/src/gps_denied_onboard/runtime_root/__init__.py index 403e1d6..bea45a4 100644 --- a/src/gps_denied_onboard/runtime_root/__init__.py +++ b/src/gps_denied_onboard/runtime_root/__init__.py @@ -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", ] diff --git a/src/gps_denied_onboard/runtime_root/state_factory.py b/src/gps_denied_onboard/runtime_root/state_factory.py new file mode 100644 index 0000000..5fae233 --- /dev/null +++ b/src/gps_denied_onboard/runtime_root/state_factory.py @@ -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_`` 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_`` 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, + }, + }, + ) diff --git a/tests/unit/c4_pose/test_smoke.py b/tests/unit/c4_pose/test_smoke.py index 0fd60c6..62c7041 100644 --- a/tests/unit/c4_pose/test_smoke.py +++ b/tests/unit/c4_pose/test_smoke.py @@ -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 diff --git a/tests/unit/c5_state/test_az381_state_protocol.py b/tests/unit/c5_state/test_az381_state_protocol.py new file mode 100644 index 0000000..3ab484e --- /dev/null +++ b/tests/unit/c5_state/test_az381_state_protocol.py @@ -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" diff --git a/tests/unit/c5_state/test_smoke.py b/tests/unit/c5_state/test_smoke.py index efbf173..5b10531 100644 --- a/tests/unit/c5_state/test_smoke.py +++ b/tests/unit/c5_state/test_smoke.py @@ -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 diff --git a/tests/unit/c8_fc_adapter/test_az390_adapter_protocol.py b/tests/unit/c8_fc_adapter/test_az390_adapter_protocol.py index 9d528f4..d567ae3 100644 --- a/tests/unit/c8_fc_adapter/test_az390_adapter_protocol.py +++ b/tests/unit/c8_fc_adapter/test_az390_adapter_protocol.py @@ -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, diff --git a/tests/unit/c8_fc_adapter/test_az392_covariance_projector.py b/tests/unit/c8_fc_adapter/test_az392_covariance_projector.py index efc8bb7..6c383ba 100644 --- a/tests/unit/c8_fc_adapter/test_az392_covariance_projector.py +++ b/tests/unit/c8_fc_adapter/test_az392_covariance_projector.py @@ -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: diff --git a/tests/unit/c8_fc_adapter/test_az393_ardupilot_outbound.py b/tests/unit/c8_fc_adapter/test_az393_ardupilot_outbound.py index cac7f49..fc5585f 100644 --- a/tests/unit/c8_fc_adapter/test_az393_ardupilot_outbound.py +++ b/tests/unit/c8_fc_adapter/test_az393_ardupilot_outbound.py @@ -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)) diff --git a/tests/unit/c8_fc_adapter/test_az394_inav_outbound.py b/tests/unit/c8_fc_adapter/test_az394_inav_outbound.py index 3217b52..8a66c05 100644 --- a/tests/unit/c8_fc_adapter/test_az394_inav_outbound.py +++ b/tests/unit/c8_fc_adapter/test_az394_inav_outbound.py @@ -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)) diff --git a/tests/unit/c8_fc_adapter/test_az395_mavlink_signing.py b/tests/unit/c8_fc_adapter/test_az395_mavlink_signing.py index d11d26e..52e8580 100644 --- a/tests/unit/c8_fc_adapter/test_az395_mavlink_signing.py +++ b/tests/unit/c8_fc_adapter/test_az395_mavlink_signing.py @@ -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, ) diff --git a/tests/unit/c8_fc_adapter/test_az397_qgc_telemetry.py b/tests/unit/c8_fc_adapter/test_az397_qgc_telemetry.py index ca1c6de..5e06982 100644 --- a/tests/unit/c8_fc_adapter/test_az397_qgc_telemetry.py +++ b/tests/unit/c8_fc_adapter/test_az397_qgc_telemetry.py @@ -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