[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
+1 -3
View File
@@ -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
@@ -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"
+5 -1
View File
@@ -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
@@ -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,
@@ -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:
@@ -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))
@@ -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))
@@ -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,
)
@@ -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