mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-06-21 17:01:13 +00:00
Oleksandr Bezdieniezhnykh
238 lines
7.4 KiB
Python
238 lines
7.4 KiB
Python
import pytest
|
|
from pydantic import ValidationError
|
|
|
|
from shared.contracts import FramePacket, TelemetrySample
|
|
from vio_adapter import (
|
|
LocalVioAdapter,
|
|
NativeVioBackend,
|
|
VioBackendEstimate,
|
|
VioInputPacket,
|
|
VioRuntimeConfig,
|
|
create_vio_adapter,
|
|
)
|
|
|
|
|
|
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:
|
|
payload: dict[str, object] = {
|
|
"frame_id": "frame-1",
|
|
"timestamp_ns": 1_000_000,
|
|
"image_ref": "replay/frame-1.jpg",
|
|
"calibration_id": "calib-1",
|
|
"occlusion": "clear",
|
|
"quality": 0.85,
|
|
}
|
|
payload.update(overrides)
|
|
return FramePacket.model_validate(payload)
|
|
|
|
|
|
def _telemetry(timestamp_ns: int = 1_000_000) -> TelemetrySample:
|
|
return TelemetrySample(
|
|
timestamp_ns=timestamp_ns,
|
|
imu={"accel_x": 0.1, "accel_y": 0.0, "accel_z": 9.8},
|
|
attitude={"roll": 0.0, "pitch": 0.01, "yaw": 0.02},
|
|
altitude_m=120.0,
|
|
airspeed_mps=24.0,
|
|
gps_health="lost",
|
|
)
|
|
|
|
|
|
def test_valid_synchronized_packet_emits_vio_state() -> None:
|
|
# Arrange
|
|
adapter = LocalVioAdapter()
|
|
packet = VioInputPacket(frame=_frame(), telemetry_samples=(_telemetry(),))
|
|
|
|
# Act
|
|
result = adapter.process(packet)
|
|
|
|
# Assert
|
|
assert result.error is None
|
|
assert result.state_packet is not None
|
|
assert result.state_packet.timestamp_ns == 1_000_000
|
|
assert result.state_packet.tracking_quality == 0.85
|
|
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_production_profile_selects_native_runtime_path() -> None:
|
|
# Arrange
|
|
runner = RecordingNativeRunner()
|
|
adapter = create_vio_adapter(
|
|
VioRuntimeConfig(environment="production"),
|
|
native_runner_factory=lambda: runner,
|
|
)
|
|
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.health.backend_name == "basalt"
|
|
|
|
|
|
def test_production_profile_without_installed_native_runtime_fails_closed() -> None:
|
|
# Arrange
|
|
adapter = create_vio_adapter(VioRuntimeConfig(environment="production"))
|
|
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"
|
|
assert "unable to load BASALT runtime" in result.error.message
|
|
|
|
|
|
def test_replay_mode_is_explicit_and_not_valid_for_production() -> None:
|
|
# Arrange
|
|
replay_config = VioRuntimeConfig(environment="development", mode="replay")
|
|
adapter = create_vio_adapter(replay_config)
|
|
packet = VioInputPacket(frame=_frame(), telemetry_samples=(_telemetry(),))
|
|
|
|
# Act
|
|
result = adapter.process(packet)
|
|
|
|
# Assert
|
|
assert result.error is None
|
|
assert result.state_packet is not None
|
|
assert result.health.backend_name == "replay_vio"
|
|
with pytest.raises(ValidationError, match="require native runtime mode"):
|
|
VioRuntimeConfig(environment="production", mode="replay")
|
|
|
|
|
|
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)
|
|
packet = VioInputPacket(frame=_frame(), telemetry_samples=(_telemetry(2_000_000),))
|
|
|
|
# Act
|
|
result = adapter.process(packet)
|
|
|
|
# Assert
|
|
assert result.state_packet is None
|
|
assert result.error is not None
|
|
assert result.error.component == "vio_adapter"
|
|
assert result.error.cause == "gap_exceeded"
|
|
assert result.health.state == "degraded"
|
|
|
|
|
|
def test_tracking_loss_degrades_health_without_emitting_absolute_position() -> None:
|
|
# Arrange
|
|
adapter = LocalVioAdapter(degraded_quality_threshold=0.35)
|
|
packet = VioInputPacket(frame=_frame(quality=0.2), telemetry_samples=(_telemetry(),))
|
|
|
|
# Act
|
|
result = adapter.process(packet)
|
|
|
|
# Assert
|
|
assert result.state_packet is not None
|
|
assert result.health.state == "degraded"
|
|
assert "latitude_deg" not in result.state_packet.model_dump()
|
|
assert "longitude_deg" not in result.state_packet.model_dump()
|