199 lines
7.1 KiB
Python
199 lines
7.1 KiB
Python
from __future__ import annotations
|
|
|
|
import threading
|
|
import time
|
|
|
|
import pytest
|
|
|
|
from k1link.compute import (
|
|
K1LocalSurfacePoseBinder,
|
|
K1LocalSurfaceShadowCoordinator,
|
|
)
|
|
from k1link.compute.live_perception import LiveSensorSynchronizer
|
|
from k1link.data_plane import ConsumerFrameContext, DecodedPointCloudView, DecodedPoseView
|
|
|
|
|
|
def _context(sequence: int, epoch_ns: int) -> ConsumerFrameContext:
|
|
return ConsumerFrameContext(
|
|
sequence=sequence,
|
|
captured_at_epoch_ns=epoch_ns,
|
|
received_monotonic_ns=epoch_ns,
|
|
processing_started_monotonic_ns=epoch_ns,
|
|
encoded_size_bytes=1,
|
|
live=True,
|
|
)
|
|
|
|
|
|
def _points(sequence: int, epoch_ns: int) -> DecodedPointCloudView:
|
|
return DecodedPointCloudView(
|
|
context=_context(sequence, epoch_ns),
|
|
frame_id="map",
|
|
positions_xyz=((1.0, 2.0, 3.0),),
|
|
intensities=b"\x01",
|
|
)
|
|
|
|
|
|
def _pose(sequence: int, epoch_ns: int) -> DecodedPoseView:
|
|
return DecodedPoseView(
|
|
context=_context(sequence, epoch_ns),
|
|
frame_id="map",
|
|
child_frame_id="sensor",
|
|
position_xyz=(0.0, 0.0, 0.0),
|
|
orientation_xyzw=(0.0, 0.0, 0.0, 1.0),
|
|
)
|
|
|
|
|
|
def test_live_sensor_synchronizer_selects_nearest_bounded_pair() -> None:
|
|
synchronizer = LiveSensorSynchronizer(
|
|
maximum_lidar_camera_delta_ms=100,
|
|
maximum_pose_point_delta_ms=100,
|
|
)
|
|
synchronizer.publish_point_cloud(_points(1, 1_000_000_000))
|
|
synchronizer.publish_point_cloud(_points(2, 1_090_000_000))
|
|
synchronizer.publish_pose(_pose(1, 1_020_000_000))
|
|
synchronizer.publish_pose(_pose(2, 1_095_000_000))
|
|
|
|
binding = synchronizer.bind_camera(1_080_000_000)
|
|
|
|
assert binding.state == "fused-ready"
|
|
assert binding.point_cloud is not None
|
|
assert binding.point_cloud.context.sequence == 2
|
|
assert binding.pose is not None
|
|
assert binding.pose.context.sequence == 2
|
|
assert binding.lidar_camera_delta_ms == 10.0
|
|
assert binding.pose_point_delta_ms == 5.0
|
|
|
|
|
|
def test_live_sensor_synchronizer_reports_each_fail_closed_boundary() -> None:
|
|
synchronizer = LiveSensorSynchronizer(
|
|
maximum_lidar_camera_delta_ms=20,
|
|
maximum_pose_point_delta_ms=10,
|
|
)
|
|
assert synchronizer.bind_camera(1_000_000_000).state == "lidar-unavailable"
|
|
|
|
synchronizer.publish_point_cloud(_points(1, 900_000_000))
|
|
assert synchronizer.bind_camera(1_000_000_000).state == "lidar-camera-delta-exceeded"
|
|
|
|
synchronizer.publish_point_cloud(_points(2, 1_000_000_000))
|
|
assert synchronizer.bind_camera(1_000_000_000).state == "pose-unavailable"
|
|
|
|
synchronizer.publish_pose(_pose(1, 1_100_000_000))
|
|
assert synchronizer.bind_camera(1_000_000_000).state == "pose-point-delta-exceeded"
|
|
|
|
|
|
def test_live_sensor_synchronizer_waits_only_for_bounded_inflight_pair() -> None:
|
|
synchronizer = LiveSensorSynchronizer(
|
|
maximum_lidar_camera_delta_ms=100,
|
|
maximum_pose_point_delta_ms=100,
|
|
)
|
|
|
|
def publish() -> None:
|
|
time.sleep(0.02)
|
|
synchronizer.publish_point_cloud(_points(1, 1_000_000_000))
|
|
synchronizer.publish_pose(_pose(1, 1_000_000_000))
|
|
|
|
thread = threading.Thread(target=publish)
|
|
thread.start()
|
|
started = time.monotonic()
|
|
binding = synchronizer.bind_camera(1_000_000_000, wait_seconds=0.2)
|
|
elapsed = time.monotonic() - started
|
|
thread.join()
|
|
|
|
assert binding.state == "fused-ready"
|
|
assert 0.01 <= elapsed < 0.15
|
|
|
|
|
|
def test_live_sensor_synchronizer_storage_never_exceeds_capacity() -> None:
|
|
synchronizer = LiveSensorSynchronizer(
|
|
maximum_lidar_camera_delta_ms=100,
|
|
maximum_pose_point_delta_ms=100,
|
|
capacity_per_modality=3,
|
|
retention_seconds=100,
|
|
)
|
|
for sequence in range(1, 8):
|
|
synchronizer.publish_point_cloud(_points(sequence, sequence * 1_000_000))
|
|
synchronizer.publish_pose(_pose(sequence, sequence * 1_000_000))
|
|
|
|
snapshot = synchronizer.snapshot()
|
|
assert snapshot["point_depth"] == 3
|
|
assert snapshot["pose_depth"] == 3
|
|
assert snapshot["evicted_points"] == 4
|
|
assert snapshot["evicted_poses"] == 4
|
|
|
|
|
|
def test_local_surface_pose_binder_accepts_either_event_order() -> None:
|
|
points_first = K1LocalSurfacePoseBinder(maximum_pose_binding_ms=100)
|
|
assert points_first.publish_point_cloud(_points(1, 1_000_000_000)) == ()
|
|
bound = points_first.publish_pose(_pose(1, 1_010_000_000))
|
|
assert len(bound) == 1
|
|
assert bound[0].point_cloud.context.sequence == 1
|
|
assert bound[0].pose.context.sequence == 1
|
|
assert bound[0].pose_binding_age_ms == 10.0
|
|
|
|
pose_first = K1LocalSurfacePoseBinder(maximum_pose_binding_ms=100)
|
|
assert pose_first.publish_pose(_pose(1, 2_000_000_000)) == ()
|
|
bound = pose_first.publish_point_cloud(_points(1, 2_010_000_000))
|
|
assert len(bound) == 1
|
|
assert bound[0].pose_binding_age_ms == 10.0
|
|
|
|
snapshot = pose_first.snapshot()
|
|
assert snapshot["points"]["published"] == 1
|
|
assert snapshot["points"]["bound"] == 1
|
|
assert snapshot["points"]["missed"] == 0
|
|
assert snapshot["authority"]["commands_enabled"] is False
|
|
|
|
|
|
def test_local_surface_pose_binder_is_bounded_and_accounts_for_misses() -> None:
|
|
binder = K1LocalSurfacePoseBinder(
|
|
maximum_pose_binding_ms=100,
|
|
point_capacity=2,
|
|
pose_capacity=3,
|
|
retention_seconds=10,
|
|
)
|
|
for sequence in range(1, 6):
|
|
assert (
|
|
binder.publish_point_cloud(_points(sequence, 1_000_000_000 + sequence * 1_000_000))
|
|
== ()
|
|
)
|
|
assert binder.flush() == ()
|
|
snapshot = binder.snapshot()
|
|
points = snapshot["points"]
|
|
assert points["capacity"] == 2
|
|
assert points["maximum_depth"] == 2
|
|
assert points["published"] == 5
|
|
assert points["bound"] == 0
|
|
assert points["dropped_overflow"] == 3
|
|
assert points["missed"] == 2
|
|
assert (
|
|
points["bound"] + points["dropped_overflow"] + points["missed"] + points["depth"]
|
|
== points["published"]
|
|
)
|
|
|
|
|
|
def test_local_surface_pose_binder_fails_closed_after_pose_deadline() -> None:
|
|
binder = K1LocalSurfacePoseBinder(maximum_pose_binding_ms=100)
|
|
assert binder.publish_point_cloud(_points(1, 1_000_000_000)) == ()
|
|
assert binder.publish_pose(_pose(1, 1_200_000_000)) == ()
|
|
snapshot = binder.snapshot()
|
|
assert snapshot["points"]["missed"] == 1
|
|
assert snapshot["points"]["depth"] == 0
|
|
|
|
with pytest.raises(ValueError, match="sequence"):
|
|
binder.publish_pose(_pose(1, 1_210_000_000))
|
|
|
|
|
|
def test_local_surface_shadow_coordinator_closes_exact_accounting() -> None:
|
|
coordinator = K1LocalSurfaceShadowCoordinator(result_capacity=2)
|
|
coordinator.begin_session("physical-k1-shadow-test")
|
|
assert coordinator.publish_pose(_pose(1, 1_000_000_000)) == 0
|
|
assert coordinator.publish_point_cloud(_points(1, 1_010_000_000)) == 1
|
|
coordinator.close()
|
|
|
|
snapshot = coordinator.snapshot()
|
|
assert snapshot["closed"] is True
|
|
assert snapshot["binder"]["points"]["bound"] == 1
|
|
assert snapshot["runtime"]["queue"]["published"] == 1
|
|
assert snapshot["runtime"]["queue"]["consumed"] == 1
|
|
assert snapshot["runtime"]["results"]["failed"] == 0
|
|
assert snapshot["authority"]["navigation_or_safety_accepted"] is False
|