338 lines
11 KiB
Python
338 lines
11 KiB
Python
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")
|