mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-06-22 11:41:12 +00:00
[AZ-240] [AZ-241] [AZ-242] Add native retrieval remediation
Implement the product remediation paths required before greenfield code testability revision: native VIO backend selection, local VPR descriptor index retrieval, and computed anchor matching gates. Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
@@ -1,5 +1,52 @@
|
||||
from shared.contracts import FramePacket, TelemetrySample
|
||||
from vio_adapter import LocalVioAdapter, VioInputPacket
|
||||
from vio_adapter import LocalVioAdapter, NativeVioBackend, VioBackendEstimate, VioInputPacket
|
||||
|
||||
|
||||
class RecordingNativeRunner:
|
||||
def __init__(self) -> None:
|
||||
self.initialized = False
|
||||
self.estimate_calls = 0
|
||||
|
||||
def initialize(self) -> None:
|
||||
self.initialized = True
|
||||
|
||||
def estimate(
|
||||
self,
|
||||
frame: FramePacket,
|
||||
telemetry_window: tuple[TelemetrySample, ...],
|
||||
) -> VioBackendEstimate:
|
||||
self.estimate_calls += 1
|
||||
return VioBackendEstimate(
|
||||
timestamp_ns=frame.timestamp_ns,
|
||||
relative_pose={"x_m": 12.0, "y_m": -1.5, "z_m": 0.2, "yaw_rad": 0.3},
|
||||
velocity_mps=(4.0, 0.5, 0.0),
|
||||
tracking_quality=0.77,
|
||||
bias_estimate={"sample_count": float(len(telemetry_window)), "gyro_bias": 0.01},
|
||||
covariance_hint=[[0.4, 0.0, 0.0], [0.0, 0.4, 0.0], [0.0, 0.0, 0.8]],
|
||||
)
|
||||
|
||||
|
||||
class FailingNativeRunner:
|
||||
def __init__(self, fail_on: str) -> None:
|
||||
self._fail_on = fail_on
|
||||
|
||||
def initialize(self) -> None:
|
||||
if self._fail_on == "initialize":
|
||||
raise RuntimeError("engine package missing")
|
||||
|
||||
def estimate(
|
||||
self,
|
||||
frame: FramePacket,
|
||||
telemetry_window: tuple[TelemetrySample, ...],
|
||||
) -> VioBackendEstimate:
|
||||
if self._fail_on == "estimate":
|
||||
raise RuntimeError("engine lost tracking")
|
||||
return VioBackendEstimate(
|
||||
timestamp_ns=frame.timestamp_ns,
|
||||
relative_pose={"x_m": 0.0},
|
||||
velocity_mps=(0.0, 0.0, 0.0),
|
||||
tracking_quality=1.0,
|
||||
)
|
||||
|
||||
|
||||
def _frame(**overrides: object) -> FramePacket:
|
||||
@@ -42,6 +89,60 @@ def test_valid_synchronized_packet_emits_vio_state() -> None:
|
||||
assert result.health.state == "ready"
|
||||
|
||||
|
||||
def test_configured_native_backend_path_emits_vio_state() -> None:
|
||||
# Arrange
|
||||
runner = RecordingNativeRunner()
|
||||
adapter = LocalVioAdapter(backend=NativeVioBackend(runner, backend_name="basalt"))
|
||||
packet = VioInputPacket(frame=_frame(), telemetry_samples=(_telemetry(),))
|
||||
|
||||
# Act
|
||||
result = adapter.process(packet)
|
||||
|
||||
# Assert
|
||||
assert runner.initialized is True
|
||||
assert runner.estimate_calls == 1
|
||||
assert result.error is None
|
||||
assert result.state_packet is not None
|
||||
assert result.state_packet.relative_pose["x_m"] == 12.0
|
||||
assert result.state_packet.velocity_mps == (4.0, 0.5, 0.0)
|
||||
assert result.health.backend_name == "basalt"
|
||||
assert result.processing_latency_ms is not None
|
||||
|
||||
|
||||
def test_native_backend_initialization_failure_sets_failed_health() -> None:
|
||||
# Arrange
|
||||
adapter = LocalVioAdapter(
|
||||
backend=NativeVioBackend(FailingNativeRunner("initialize"), backend_name="basalt")
|
||||
)
|
||||
packet = VioInputPacket(frame=_frame(), telemetry_samples=(_telemetry(),))
|
||||
|
||||
# Act
|
||||
result = adapter.process(packet)
|
||||
|
||||
# Assert
|
||||
assert result.state_packet is None
|
||||
assert result.health.state == "failed"
|
||||
assert result.error is not None
|
||||
assert result.error.cause == "backend_initialization_failed"
|
||||
|
||||
|
||||
def test_native_backend_runtime_failure_sets_failed_health() -> None:
|
||||
# Arrange
|
||||
adapter = LocalVioAdapter(
|
||||
backend=NativeVioBackend(FailingNativeRunner("estimate"), backend_name="basalt")
|
||||
)
|
||||
packet = VioInputPacket(frame=_frame(), telemetry_samples=(_telemetry(),))
|
||||
|
||||
# Act
|
||||
result = adapter.process(packet)
|
||||
|
||||
# Assert
|
||||
assert result.state_packet is None
|
||||
assert result.health.state == "failed"
|
||||
assert result.error is not None
|
||||
assert result.error.cause == "backend_runtime_failed"
|
||||
|
||||
|
||||
def test_timestamp_mismatch_is_explicit_validation_error() -> None:
|
||||
# Arrange
|
||||
adapter = LocalVioAdapter(timestamp_tolerance_ns=1_000)
|
||||
|
||||
Reference in New Issue
Block a user