mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-06-22 23:21:13 +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,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))
|
||||
|
||||
Reference in New Issue
Block a user