from __future__ import annotations import json import struct import numpy as np import pytest from k1link.compute.live_perception import ( LIVE_INGRESS_WIRE_SCHEMA, LatestWinsQueue, LivePerceptionIngress, WorldStateProjector, classify_health, decode_live_perception_result, encode_live_perception_result, ) def test_live_ingress_keeps_modalities_separately_bounded_and_ordered() -> None: ingress = LivePerceptionIngress() ingress.open_consumer("worker-1") ingress.begin_session("session-1") for sequence in range(5): assert ingress.publish( modality="camera-frame", source_id="sensor.camera.right", source_sequence=sequence, captured_at_epoch_ns=100 + sequence, received_monotonic_ns=200 + sequence, payload=f"camera-{sequence}".encode(), ) for sequence in range(3): assert ingress.publish( modality="lidar", source_id="lixel/application/report/lio_pcl", source_sequence=sequence, captured_at_epoch_ns=300 + sequence, received_monotonic_ns=400 + sequence, payload=f"lidar-{sequence}".encode(), ) for sequence in range(20): assert ingress.publish( modality="pose", source_id="lixel/application/report/lio_pose", source_sequence=sequence, captured_at_epoch_ns=500 + sequence, received_monotonic_ns=600 + sequence, payload=f"pose-{sequence}".encode(), ) snapshot = ingress.snapshot() assert snapshot["queues"]["camera-frame"]["depth"] == 2 assert snapshot["queues"]["camera-frame"]["dropped_overflow"] == 3 assert snapshot["queues"]["lidar"]["depth"] == 2 assert snapshot["queues"]["lidar"]["dropped_overflow"] == 1 assert snapshot["queues"]["pose"]["depth"] == 16 assert snapshot["queues"]["pose"]["dropped_overflow"] == 4 events = [] while (event := ingress.take_next("worker-1", timeout=0)) is not None: events.append(event) assert events[0].modality == "control" assert [event.ingress_sequence for event in events] == sorted( event.ingress_sequence for event in events ) assert [event.source_sequence for event in events if event.modality == "camera-frame"] == [ 3, 4, ] assert [event.source_sequence for event in events if event.modality == "lidar"] == [1, 2] assert [event.source_sequence for event in events if event.modality == "pose"] == list( range(4, 20) ) def test_live_ingress_wire_is_self_delimiting_and_explicitly_non_authoritative() -> None: ingress = LivePerceptionIngress() ingress.open_consumer("worker-1") ingress.begin_session("session-1") assert ingress.publish( modality="camera-init", source_id="sensor.camera.right", source_sequence=0, captured_at_epoch_ns=123, received_monotonic_ns=456, payload=b"init", ) ingress.take_next("worker-1", timeout=0) event = ingress.take_next("worker-1", timeout=0) assert event is not None encoded = event.wire_bytes() header_bytes = struct.unpack("!I", encoded[:4])[0] header = json.loads(encoded[4 : 4 + header_bytes]) assert header["schema_version"] == LIVE_INGRESS_WIRE_SCHEMA assert header["payload_bytes"] == 4 assert header["commands_enabled"] is False assert header["navigation_or_safety_accepted"] is False assert encoded[4 + header_bytes :] == b"init" def test_live_ingress_rejects_oversize_without_affecting_other_modalities() -> None: ingress = LivePerceptionIngress() ingress.begin_session("session-1") assert not ingress.publish( modality="camera-frame", source_id="sensor.camera.right", source_sequence=1, captured_at_epoch_ns=1, received_monotonic_ns=1, payload=b"x" * (1024 * 1024 + 1), ) assert ingress.publish( modality="pose", source_id="RealtimePath", source_sequence=2, captured_at_epoch_ns=2, received_monotonic_ns=2, payload=b"pose", ) snapshot = ingress.snapshot() assert snapshot["queues"]["camera-frame"]["rejected_oversize"] == 1 assert snapshot["queues"]["pose"]["published"] == 1 def test_live_ingress_allows_only_one_worker_consumer() -> None: ingress = LivePerceptionIngress() ingress.open_consumer("worker-1") with pytest.raises(RuntimeError, match="already has a consumer"): ingress.open_consumer("worker-2") ingress.close_consumer("worker-1") ingress.open_consumer("worker-2") def test_live_result_round_trip_keeps_video_mask_boxes_and_shadow_authority() -> None: mask = np.zeros((600, 800), dtype=np.uint8) mask[100:120, 200:240] = 4 encoded = encode_live_perception_result( frame_index=12, source_frame_index=44, session_seconds=1.25, captured_at_epoch_ns=123_000_000, image_jpeg=b"\xff\xd8test\xff\xd9", segmentation_mask=mask, objects=[ { "track_id": 7, "label": "car", "score": 0.91, "bbox_xyxy": [10, 20, 110, 80], "cuboid_center_map": [1, 2, 0.5], "cuboid_half_size": [2.25, 0.925, 0.775], "cuboid_quaternion_xyzw": [0, 0, 0, 1], } ], delivery={"health": "healthy", "result_age_ms": 49.0}, ) frame = decode_live_perception_result(encoded) assert frame.frame_index == 12 assert frame.source_frame_index == 44 assert frame.image_jpeg == b"\xff\xd8test\xff\xd9" assert frame.segmentation_mask is not None assert np.array_equal(frame.segmentation_mask, mask) assert frame.objects[0]["bbox_xyxy"] == [10.0, 20.0, 110.0, 80.0] assert frame.objects[0]["cuboid_center_map"] == [1.0, 2.0, 0.5] assert frame.delivery["health"] == "healthy" def test_live_result_rejects_tampering_and_partial_cuboid() -> None: with pytest.raises(ValueError, match="cuboid is incomplete"): encode_live_perception_result( frame_index=0, source_frame_index=0, session_seconds=0.0, captured_at_epoch_ns=1, image_jpeg=b"\xff\xd8x\xff\xd9", segmentation_mask=None, objects=[ { "track_id": 1, "label": "car", "score": 0.5, "bbox_xyxy": [0, 0, 1, 1], "cuboid_center_map": [0, 0, 0], } ], delivery={"health": "degraded"}, ) encoded = encode_live_perception_result( frame_index=0, source_frame_index=0, session_seconds=0.0, captured_at_epoch_ns=1, image_jpeg=b"\xff\xd8x\xff\xd9", segmentation_mask=None, objects=[], delivery={"health": "healthy"}, ) changed = bytearray(encoded) changed[-3] ^= 0x01 with pytest.raises(ValueError, match="contract is invalid"): decode_live_perception_result(bytes(changed)) def test_latest_wins_queue_never_exceeds_capacity() -> None: queue = LatestWinsQueue[int](capacity=2) queue.publish(1) queue.publish(2) queue.publish(3) assert queue.take_next(timeout=0) == 2 assert queue.take_next(timeout=0) == 3 snapshot = queue.snapshot() assert snapshot.maximum_depth == 2 assert snapshot.depth == 0 assert snapshot.dropped_overflow == 1 assert snapshot.dropped_superseded == 0 assert snapshot.dropped_total == 1 def test_closed_empty_queue_returns_none() -> None: queue = LatestWinsQueue[str](capacity=1) queue.close() assert queue.take_next(timeout=0) is None assert queue.snapshot().closed is True def test_health_distinguishes_depth_degradation_from_staleness() -> None: assert classify_health( source_available=True, fusion_state="fused", result_age_ms=25.0, stale_after_ms=300.0, unavailable_after_ms=1000.0, ) == ("healthy", ()) assert classify_health( source_available=True, fusion_state="depth-unavailable-sync-gate", result_age_ms=25.0, stale_after_ms=300.0, unavailable_after_ms=1000.0, ) == ("degraded", ("depth-unavailable-sync-gate",)) assert classify_health( source_available=True, fusion_state="fused", result_age_ms=350.0, stale_after_ms=300.0, unavailable_after_ms=1000.0, ) == ("stale", ("result-age-exceeded",)) assert classify_health( source_available=False, fusion_state="fused", result_age_ms=0.0, stale_after_ms=300.0, unavailable_after_ms=1000.0, ) == ("unavailable", ("source-unavailable",)) def test_world_state_contains_metric_position_size_range_and_velocity() -> None: projector = WorldStateProjector(velocity_history_limit_s=1.0) accepted = { "track_id": 7, "label": "car", "association_group": "vehicle", "score": 0.8, "clustered_points": 12, "distance_smoothed_m": 5.0, "cuboid_status": "accepted-point-supported-oriented-p05-p95", "cuboid_center_map": [1.0, 2.0, 0.5], "cuboid_half_size": [2.0, 1.0, 0.75], "cuboid_quaternion_xyzw": [0.0, 0.0, 0.0, 1.0], } base = { "frame_index": 0, "source_frame_index": 1000, "session_seconds": 10.0, "state": "fused", "objects": [ accepted, {**accepted, "track_id": 9, "cuboid_status": "rejected-distance-innovation"}, ], } first = projector.project( frame=base, lidar_positions={7: [4.0, 0.0, 0.0]}, clearance={"front_m": 4.0}, ) states = [first] for index, elapsed in enumerate((0.2, 0.4, 0.6), start=1): states.append( projector.project( frame={ **base, "frame_index": index, "session_seconds": 10.0 + elapsed, "objects": [ { **accepted, "cuboid_center_map": [1.0 + 2.0 * elapsed, 2.0, 0.5], } ], }, lidar_positions={7: [3.0, 0.0, 0.0]}, clearance={"front_m": 3.0}, ) ) second = states[-1] assert first["object_count"] == 1 assert first["objects"][0]["size_m"] == [4.0, 2.0, 1.5] assert first["objects"][0]["position_lidar_m"] == [4.0, 0.0, 0.0] assert first["objects"][0]["velocity_map_mps"] is None assert second["objects"][0]["velocity_map_mps"] == pytest.approx([2.0, 0.0, 0.0]) assert second["objects"][0]["speed_mps"] == pytest.approx(2.0) assert second["objects"][0]["velocity_status"] == "diagnostic-robust-history" def test_world_state_declares_missing_vehicle_body_transform() -> None: state = WorldStateProjector().project( frame={ "frame_index": 0, "source_frame_index": 1, "session_seconds": 1.0, "state": "depth-unavailable-sync-gate", "objects": [], }, lidar_positions={}, clearance={"state": "unavailable"}, ) assert state["coordinate_frames"]["sensor_relative"] == "k1-lidar" assert state["coordinate_frames"]["vehicle_body"].startswith("unavailable")