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