119 lines
3.9 KiB
Python
119 lines
3.9 KiB
Python
from __future__ import annotations
|
|
|
|
import threading
|
|
import time
|
|
|
|
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
|