70 lines
1.8 KiB
Python
70 lines
1.8 KiB
Python
from __future__ import annotations
|
|
|
|
import struct
|
|
|
|
from k1link.protocol.streams import (
|
|
LegacyPoint,
|
|
LegacyPointCloudFrame,
|
|
LioPoint,
|
|
LioPointCloudFrame,
|
|
MqttHeader,
|
|
)
|
|
from k1link.viewer.foxglove_bridge import (
|
|
POINT_STRIDE,
|
|
BridgeMetrics,
|
|
pack_legacy_point_cloud,
|
|
pack_lio_point_cloud,
|
|
)
|
|
|
|
|
|
def test_lio_points_are_scaled_and_packed_with_uint8_intensity() -> None:
|
|
frame = LioPointCloudFrame(
|
|
header=MqttHeader(
|
|
seq=1,
|
|
stamp=2,
|
|
scaler=1000,
|
|
device_id="synthetic",
|
|
session_id="synthetic",
|
|
openapi_key=None,
|
|
),
|
|
compression=0,
|
|
compressed_bytes=10,
|
|
decompressed_bytes=20,
|
|
points=(LioPoint(-1000, 2500, 3000, 0xAABBCC7F),),
|
|
)
|
|
|
|
packed = pack_lio_point_cloud(frame)
|
|
|
|
assert packed.point_count == 1
|
|
assert len(packed.data) == POINT_STRIDE
|
|
assert struct.unpack("<fffB3x", packed.data) == (-1.0, 2.5, 3.0, 0x7F)
|
|
|
|
|
|
def test_legacy_points_keep_verified_metric_fields() -> None:
|
|
frame = LegacyPointCloudFrame(
|
|
envelope=b"\x00" * 12,
|
|
stride=16,
|
|
points=(LegacyPoint(1.0, -2.0, 3.5, 1, 2, 3, 240),),
|
|
)
|
|
|
|
packed = pack_legacy_point_cloud(frame)
|
|
|
|
assert struct.unpack("<fffB3x", packed.data) == (1.0, -2.0, 3.5, 240)
|
|
|
|
|
|
def test_metrics_distinguish_preview_drops_and_pipeline_latency() -> None:
|
|
metrics = BridgeMetrics()
|
|
metrics.received(100)
|
|
metrics.preview_dropped()
|
|
metrics.record_latency(12.3456)
|
|
metrics.published_pcl(42, 2_000_000_000, 1.2345)
|
|
|
|
snapshot = metrics.snapshot()
|
|
|
|
assert snapshot["messages_received"] == 1
|
|
assert snapshot["payload_bytes"] == 100
|
|
assert snapshot["preview_dropped"] == 1
|
|
assert snapshot["last_point_count"] == 42
|
|
assert snapshot["mqtt_to_publish_ms"] == 12.346
|
|
assert snapshot["decode_publish_ms"] == 1.234
|