mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-06-22 19:31:15 +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:
@@ -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
|
||||
|
||||
Reference in New Issue
Block a user