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

[AZ-381] C5 StateEstimator protocol + factory + C8 DTO reshape

Browse Source 
- Add StateEstimator Protocol (6 methods, @runtime_checkable) + DTOs
  (EstimatorOutput, EstimatorHealth, IsamState, PoseSourceLabel, Quat)
  in _types/state.py per state_estimator_protocol.md v1.0.0.
- Add C5 error hierarchy (StateEstimatorError + 3 subclasses) and
  C5StateConfig (strategy, keyframe_window, spoof gates,
  no_estimate_fallback_s) with __post_init__ validation.
- Add ISam2GraphHandle Protocol + ISam2GraphHandleImpl skeleton (all
  4 methods raise NotImplementedError naming AZ-382 as owner).
- Add build_state_estimator factory + bind_state_ingest_thread for
  single-writer enforcement; ADR-002 build-flag gating
  (BUILD_STATE_<variant>); INFO log on success.
- Strict reshape of legacy EstimatorOutput / EstimatorHealth across
  all 6 C8 production files (_outbound_provenance,
  _covariance_projector, pymavlink_ardupilot_adapter,
  msp2_inav_adapter, mavlink_gcs_adapter, interface) + 6 C8 test
  files (UUID frame_id, LatLonAlt position_wgs84, Quat orientation,
  PoseSourceLabel enum source_label). Remove ad-hoc DTOs from
  _types/pose.py and from C4's public __init__ (EstimatorOutput is a
  C5 concept, not a C4 one).
- 20 AZ-381 AC tests (10 ACs + 4 config range + NFR + conformance).
- Full suite: 521 passed, 2 skipped (+20 vs Batch 11).
- Contracts: state_estimator_protocol.md v1.0.0 -> active;
  composition_root_protocol.md v1.2.0 -> v1.3.0 (additive state
  block + factory + ingest-thread binding).
- Impl report: _docs/03_implementation/batch_12_cycle1_report.md.

Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
Oleksandr Bezdieniezhnykh
2026-05-11 05:35:20 +03:00
parent 8a9cf88a46
commit beed43724f
32 changed files with 1394 additions and 157 deletions
Download Patch File Download Diff File Expand all files Collapse all files
_docs/02_tasks/done/AZ-381_c5_state_protocol.md
+105
View File
@@ -0,0 +1,105 @@
# C5 StateEstimator Protocol + Factory + DTOs + Composition + Concrete ISam2GraphHandle
**Task**: AZ-381_c5_state_protocol
**Name**: C5 `StateEstimator` Protocol + Factory + DTOs + Composition + concrete `ISam2GraphHandle`
**Description**: Define the public `StateEstimator` Protocol (PEP 544 `@runtime_checkable`), the C5 DTOs (`EstimatorOutput`, `EstimatorHealth`, `IsamState` enum), the error hierarchy (`StateEstimatorError`, `EstimatorDegradedError`, `EstimatorFatalError`, `StateEstimatorConfigError`), the composition-root factory `build_state_estimator(...) -> tuple[StateEstimator, ISam2GraphHandle]`, AND the CONCRETE `ISam2GraphHandle` implementation extending the AZ-355 Protocol stub with `add_factor`/`update`/`compute_marginals`/`last_anchor_age_ms` methods. The handle is constructed alongside the iSAM2 graph (initially empty here; populated by AZ-382 iSAM2 wiring task) and passed by reference to C4 via the runtime root. Strategy resolution per ADR-002 with `BUILD_STATE_<variant>` gating. Shared helpers (`ImuPreintegrator` AZ-276, `SE3Utils` AZ-277, `WgsConverter` AZ-279) constructor-injected. Config schema extension for `state.{strategy, keyframe_window_size, spoof_promotion_min_stable_s, spoof_promotion_visual_consistency_tol_m, no_estimate_fallback_s}`. No iSAM2 graph internals or factor-add logic in scope here.
**Complexity**: 3 points
**Dependencies**: AZ-263, AZ-269, AZ-270, AZ-276 (`ImuPreintegrator`), AZ-277 (`SE3Utils`), AZ-279 (`WgsConverter`), AZ-273 (`FdrClient`), AZ-355 (C4's `ISam2GraphHandle` Protocol stub — extended here), AZ-266
**Component**: c5_state (epic AZ-260 / E-C5)
**Tracker**: AZ-381
**Epic**: AZ-260 (E-C5)
### Document Dependencies
- `_docs/02_document/contracts/c5_state/state_estimator_protocol.md` — the public contract this task implements.
- `_docs/02_document/components/07_c5_state/description.md` — § 1, § 2, § 5 error handling, § 9 logging.
- `_docs/02_document/architecture.md` — ADR-001, ADR-002, ADR-003, ADR-009.
- `_docs/02_document/contracts/c4_pose/pose_estimator_protocol.md` — `ISam2GraphHandle` Protocol stub source.
- `_docs/02_document/module-layout.md` — `c5_state` Per-Component Mapping.
## Problem
Without this task, C4 has no concrete `ISam2GraphHandle` to inject (only the Protocol stub from AZ-355) — meaning the runtime root cannot wire C4 + C5 together. The DTO surface (`EstimatorOutput`, `EstimatorHealth`) is also consumed by C8, C13, and the orthorectifier — defining it once in `_types/state.py` prevents drift. The eight downstream consumer tasks (iSAM2 wiring, factor adds, marginals, spoof gate, ESKF, smoothed history, AC-5.2, orthorectifier) depend on the Protocol surface + the handle being available.
## Outcome
- `src/gps_denied_onboard/components/c5_state/interface.py` — `StateEstimator` Protocol with all 6 methods.
- `src/gps_denied_onboard/components/c5_state/__init__.py` — re-exports `StateEstimator`, `EstimatorOutput`, `EstimatorHealth`.
- `src/gps_denied_onboard/_types/state.py` — `EstimatorOutput`, `EstimatorHealth`, `IsamState` enum (frozen + slots).
- `src/gps_denied_onboard/components/c5_state/errors.py` — error hierarchy.
- `src/gps_denied_onboard/components/c5_state/_isam2_handle.py` — concrete `ISam2GraphHandleImpl(ISam2GraphHandle)` class with all four methods. Body: empty stubs that raise `NotImplementedError("iSAM2 wiring task owns this body")` until AZ-382 lands. Each method's `NotImplementedError` message names the responsible task ID for traceability.
- `src/gps_denied_onboard/runtime_root/state_factory.py` — `build_state_estimator(...)` returning the tuple. Lazy-import per ADR-002.
- Composition-root extension: invoke `build_state_estimator` AFTER the shared helpers; pass the returned `ISam2GraphHandle` to `build_pose_estimator` (C4); bind C4 + C5 to the SAME ingest thread.
- Config schema extension for the five `state.*` fields.
- INFO log on successful build: `kind="c5.state.strategy_loaded"`.
## Scope
### Included
- `StateEstimator` Protocol with 6 methods.
- DTOs (`EstimatorOutput`, `EstimatorHealth`, `IsamState`) in `_types/state.py`.
- Error hierarchy.
- Concrete `ISam2GraphHandleImpl` skeleton (body owned by AZ-382 iSAM2 wiring task).
- Composition-root factory + thread binding.
- Config schema extension.
- Unit tests: Protocol conformance, DTO immutability + slots, factory rejection on unknown strategy + missing build flag, ISam2GraphHandleImpl methods exist (return `NotImplementedError`), thread binding.
### Excluded
- iSAM2 + `IncrementalFixedLagSmoother` body — owned by AZ-382 (next task).
- Factor adds (VIO + Pose + IMU) — owned by AZ-383.
- Marginals + outputs — owned by AZ-384.
- Source-label state machine + spoof gate — owned by AZ-385.
- ESKF baseline — owned by AZ-386.
- Smoothed-history → FDR — owned by AZ-387.
- AC-5.2 fallback — owned by AZ-388.
- Orthorectifier sub-path — owned by AZ-389.
- Component-internal acceptance tests C5-IT-01..07 + C5-PT-01 + C5-ST-01 — deferred to E-BBT (AZ-262).
## Acceptance Criteria
**AC-1: Protocol conformance** — `runtime_checkable` `isinstance` returns True for a fake with all 6 methods.
**AC-2: DTOs frozen + slots** — `FrozenInstanceError` on mutation; `__slots__` non-empty.
**AC-3: `IsamState` enum has 4 values** — `INIT`, `TRACKING`, `DEGRADED`, `LOST`.
**AC-4: Factory rejects missing build flag** — `config.state.strategy = "nonexistent"` → `StateEstimatorConfigError("BUILD_STATE_NONEXISTENT is OFF...")`.
**AC-5: Factory rejects unknown strategy at config-load** — `config.state.strategy = "garbage"` → `StateEstimatorConfigError` at config load.
**AC-6: Factory returns the tuple** — both `StateEstimator` AND `ISam2GraphHandle` are returned from a successful build; INFO log with `{strategy, keyframe_window_size}`.
**AC-7: Thread binding** — composition root binds C5 to ONE ingest thread (the same as C4); second binding raises `RuntimeError`.
**AC-8: `ISam2GraphHandleImpl` skeleton** — instance is `isinstance(handle, ISam2GraphHandle)`; calling `add_factor`, `update`, `compute_marginals`, `last_anchor_age_ms` each raises `NotImplementedError(f"Body owned by ...")` with the correct task ID in the message.
**AC-9: Public API re-exports** — `from gps_denied_onboard.components.c5_state import StateEstimator, EstimatorOutput, EstimatorHealth` resolves; internals not in `__all__`.
**AC-10: Error hierarchy catchability** — every error caught by `except StateEstimatorError`.
## Non-Functional Requirements
- `build_state_estimator` p99 ≤ 50 ms.
## Constraints
- `@runtime_checkable` on Protocol; DTOs `frozen=True, slots=True`.
- Lazy-import per ADR-002.
- Single-thread binding enforced (AC-7).
- The `ISam2GraphHandleImpl` skeleton's `NotImplementedError` messages MUST name the responsible task ID — AZ-382 iSAM2 wiring is the receiver.
## Risks & Mitigation
- **Risk**: AZ-382 iSAM2 task lands before this task → cycle. *Mitigation*: this task ships first; AZ-382 imports `ISam2GraphHandleImpl` and replaces method bodies.
- **Risk**: AZ-355 stub Protocol may differ slightly from AZ-358's extension. *Mitigation*: this task verifies isinstance against the FINAL Protocol shape (post-AZ-358 extension) — both AZ-358 and this task update the Protocol stub in lockstep.
## Runtime Completeness
- **Named capability**: `StateEstimator` Protocol + DTOs + factory + concrete `ISam2GraphHandle` skeleton.
- **Production code**: real Protocol, real DTOs, real error hierarchy, real factory, real `ISam2GraphHandleImpl` skeleton with `NotImplementedError` bodies, real composition wiring.
- **Allowed external stubs**: test fakes only.
- **Unacceptable substitutes**: hardcoding the C5 strategy class in C4's factory (defeats ADR-009); skipping the concrete `ISam2GraphHandleImpl` (would force AZ-382 iSAM2 wiring to also reshape Protocol).
## Contract
Implements `_docs/02_document/contracts/c5_state/state_estimator_protocol.md`.