NODEDC_MISSION_CORE/tests/test_lidar_contract.py

from __future__ import annotations

from dataclasses import replace

import numpy as np
import pytest

from k1link.compute.lidar_contract import (
    K1_LAB_LIDAR_PACK_V1_PROFILE,
    K1_LIVE_LIDAR_PROFILE,
    LidarContractError,
    LidarCoordinateSpace,
    LidarEvidenceProfile,
    LidarPipelineStage,
    LidarPointField,
    LidarPoseStatus,
    LidarQualityMonitor,
    LidarReadiness,
    LidarRepresentation,
    LidarTimeBasis,
    assess_lidar_profile,
    lidar_readiness_document,
    sensor_frame_xyzi,
)
from k1link.data_plane import (
    ConsumerFrameContext,
    DecodedPointCloudView,
    DecodedPoseView,
)


def _context(sequence: int = 1) -> ConsumerFrameContext:
    return ConsumerFrameContext(
        sequence=sequence,
        captured_at_epoch_ns=10,
        received_monotonic_ns=20,
        processing_started_monotonic_ns=21,
        encoded_size_bytes=100,
        live=True,
    )


def _assessment(stage: LidarPipelineStage, *, lab_pack: bool = False) -> object:
    profile = K1_LAB_LIDAR_PACK_V1_PROFILE if lab_pack else K1_LIVE_LIDAR_PROFILE
    return next(item for item in assess_lidar_profile(profile) if item.stage is stage)


def test_k1_profiles_round_trip_and_do_not_claim_navigation_authority() -> None:
    restored = type(K1_LIVE_LIDAR_PROFILE).from_dict(K1_LIVE_LIDAR_PROFILE.to_dict())

    assert restored == K1_LIVE_LIDAR_PROFILE
    document = lidar_readiness_document(restored)
    assert document["authority"] == {
        "commands_enabled": False,
        "navigation_or_safety_accepted": False,
    }


def test_current_k1_live_source_is_detector_degraded_but_odometry_blocked() -> None:
    detector = _assessment(LidarPipelineStage.LIDAR_3D_DETECTION)
    odometry = _assessment(LidarPipelineStage.LIDAR_ODOMETRY)
    lio = _assessment(LidarPipelineStage.LIDAR_INERTIAL_SLAM)

    assert detector.readiness is LidarReadiness.DEGRADED
    assert "pretrained-domain-expects-sensor-scan" in detector.reasons
    assert odometry.readiness is LidarReadiness.BLOCKED
    assert lio.readiness is LidarReadiness.BLOCKED
    assert "per-point-time-unavailable" in lio.reasons


def test_lidar_pack_v1_is_blocked_for_pointpillars_because_it_dropped_intensity() -> None:
    detector = _assessment(LidarPipelineStage.LIDAR_3D_DETECTION, lab_pack=True)

    assert detector.readiness is LidarReadiness.BLOCKED
    assert "admitted-pointpillars-baseline-requires-intensity" in detector.reasons


def test_nvblox_is_blocked_until_k1_scan_geometry_is_known() -> None:
    assessment = _assessment(LidarPipelineStage.NVIDIA_NVBLOX)

    assert assessment.readiness is LidarReadiness.BLOCKED
    assert "lidar-intrinsics-or-scan-geometry-unknown" in assessment.reasons


def test_complete_sensor_profile_is_ready_for_all_admitted_stages() -> None:
    profile = LidarEvidenceProfile(
        profile_id="reference-raw-lidar-imu/v1",
        representation=LidarRepresentation.SENSOR_SCAN,
        coordinate_space=LidarCoordinateSpace.SENSOR,
        coordinate_frame="lidar",
        frame_time_basis=LidarTimeBasis.SENSOR,
        point_fields=(
            LidarPointField.XYZ,
            LidarPointField.INTENSITY,
            LidarPointField.RING,
            LidarPointField.RELATIVE_TIME,
        ),
        pose_status=LidarPoseStatus.SENSOR_SYNCHRONIZED,
        scan_geometry_known=True,
        imu_samples_available=True,
        lidar_imu_extrinsic_available=True,
        camera_extrinsic_available=True,
    )

    assert {
        assessment.readiness for assessment in assess_lidar_profile(profile)
    } == {LidarReadiness.READY}
    assert len(assess_lidar_profile(profile)) == len(LidarPipelineStage)


def test_sensor_frame_xyzi_inverts_map_pose_and_normalizes_intensity() -> None:
    cloud = DecodedPointCloudView(
        context=_context(),
        frame_id="map",
        positions_xyz=((11.0, 2.0, 3.0), (10.0, 3.0, 3.0)),
        intensities=bytes((0, 255)),
    )
    pose = DecodedPoseView(
        context=_context(2),
        frame_id="map",
        child_frame_id="k1-lidar",
        position_xyz=(10.0, 2.0, 3.0),
        orientation_xyzw=(0.0, 0.0, 0.0, 1.0),
    )

    xyzi = sensor_frame_xyzi(cloud, pose)

    np.testing.assert_allclose(
        xyzi,
        np.asarray(
            [
                [1.0, 0.0, 0.0, 0.0],
                [0.0, 1.0, 0.0, 1.0],
            ],
            dtype=np.float32,
        ),
    )


def test_sensor_frame_xyzi_rejects_missing_intensity_and_unbound_frames() -> None:
    cloud = DecodedPointCloudView(
        context=_context(),
        frame_id="map",
        positions_xyz=((1.0, 2.0, 3.0),),
    )
    with pytest.raises(LidarContractError, match="requires intensity"):
        sensor_frame_xyzi(cloud)

    with pytest.raises(LidarContractError, match="diagnostic-only"):
        replace(K1_LIVE_LIDAR_PROFILE, navigation_or_safety_accepted=True)


def test_quality_monitor_is_bounded_and_reports_sensor_relative_range() -> None:
    monitor = LidarQualityMonitor(
        K1_LIVE_LIDAR_PROFILE,
        frame_sample_capacity=2,
        point_sample_capacity=256,
        points_sampled_per_frame=2,
    )
    pose = DecodedPoseView(
        context=_context(20),
        frame_id="map",
        child_frame_id="k1-lidar",
        position_xyz=(10.0, 2.0, 3.0),
        orientation_xyzw=(0.0, 0.0, 0.0, 1.0),
    )
    for sequence, capture_ns in enumerate((10, 110_000_010, 210_000_010), start=1):
        context = replace(_context(sequence), captured_at_epoch_ns=capture_ns)
        monitor.observe(
            DecodedPointCloudView(
                context=context,
                frame_id="map",
                positions_xyz=((11.0, 2.0, 3.0), (10.0, 4.0, 3.0)),
                intensities=bytes((0, 255)),
            ),
            pose=pose,
        )

    report = monitor.snapshot()

    assert report["frames_observed"] == 3
    assert report["points_observed"] == 6
    assert report["point_count_per_frame"]["sample_count"] == 2
    assert report["sensor_range_m"]["minimum"] == pytest.approx(1.0)
    assert report["sensor_range_m"]["maximum"] == pytest.approx(2.0)
    assert report["intensity_0_1"]["minimum"] == pytest.approx(0.0)
    assert report["intensity_0_1"]["maximum"] == pytest.approx(1.0)
    assert report["authority"]["navigation_or_safety_accepted"] is False