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

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

Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
Oleksandr Bezdieniezhnykh
2026-05-11 05:35:20 +03:00
parent 8a9cf88a46
commit beed43724f
32 changed files with 1394 additions and 157 deletions
@@ -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))