gps-denied-onboard/tests/unit/test_vio_adapter.py

from shared.contracts import FramePacket, TelemetrySample
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:
    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_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()