213 lines
7.2 KiB
Python
213 lines
7.2 KiB
Python
"""Unit tests for H32 Medulla dlog → V1 export helpers."""
|
|
|
|
from __future__ import annotations
|
|
|
|
import struct
|
|
from pathlib import Path
|
|
|
|
import numpy as np
|
|
|
|
from tools.h32_dlog.difop import CHANNELS, HORIZONTAL_START, VERTICAL_START, parse_difop_angles
|
|
from tools.h32_dlog.dobject import RECORD_RE, discover_records, iter_payloads, resolve_dlog_root
|
|
from tools.h32_dlog.timeutil import local_wall_to_dotnet_ticks
|
|
from tools.h32_dlog.load_session import load_h32_dlog_lidar
|
|
from tools.h32_dlog.payload_v1 import (
|
|
MsopPacketItem,
|
|
build_difop_payload,
|
|
build_msop_batch_payload,
|
|
parse_difop_payload,
|
|
parse_msop_batch_payload,
|
|
)
|
|
from tools.rscap_v2.h32_msop import PACKET_LENGTH, iter_h32_frames_from_packets
|
|
|
|
|
|
def _make_msop_packet(*, seconds: int = 100, microseconds: int = 5000, az_deg: float = 10.0) -> bytes:
|
|
packet = bytearray(PACKET_LENGTH)
|
|
packet[17] = 1
|
|
packet[20:26] = int(seconds).to_bytes(6, "big")
|
|
packet[26:30] = int(microseconds).to_bytes(4, "big")
|
|
az_raw = int(round(az_deg * 100))
|
|
for block in range(12):
|
|
offset = 42 + block * 100
|
|
packet[offset] = 255
|
|
packet[offset + 1] = 238
|
|
packet[offset + 2] = (az_raw >> 8) & 0xFF
|
|
packet[offset + 3] = az_raw & 0xFF
|
|
idx = offset + 4
|
|
for _ch in range(CHANNELS):
|
|
packet[idx] = (1600 >> 8) & 0xFF
|
|
packet[idx + 1] = 1600 & 0xFF
|
|
packet[idx + 2] = 10
|
|
idx += 3
|
|
return bytes(packet)
|
|
|
|
|
|
def _write_signed_angle(buf: bytearray, index: int, degrees: float) -> None:
|
|
sign = 1 if degrees < 0 else 0
|
|
raw = int(round(abs(degrees) * 100))
|
|
buf[index] = sign
|
|
buf[index + 1] = (raw >> 8) & 0xFF
|
|
buf[index + 2] = raw & 0xFF
|
|
|
|
|
|
def _make_difop_packet(*, vertical: list[float], horizontal: list[float] | None = None) -> bytes:
|
|
packet = bytearray(1248)
|
|
horiz = horizontal if horizontal is not None else [0.0] * CHANNELS
|
|
for channel, angle in enumerate(vertical):
|
|
_write_signed_angle(packet, VERTICAL_START + channel * 3, angle)
|
|
for channel, angle in enumerate(horiz):
|
|
_write_signed_angle(packet, HORIZONTAL_START + channel * 3, angle)
|
|
return bytes(packet)
|
|
|
|
|
|
def _write_dorec_record(
|
|
path: Path,
|
|
*,
|
|
object_name: str,
|
|
ticks: int,
|
|
record_id: str,
|
|
payload: bytes,
|
|
offset: int = 0,
|
|
) -> int:
|
|
"""Append one DObject record; return file offset of the record start."""
|
|
|
|
path.parent.mkdir(parents=True, exist_ok=True)
|
|
name_b = object_name.encode("ascii")
|
|
id_b = record_id.encode("ascii")
|
|
blob = (
|
|
bytes([len(name_b)])
|
|
+ name_b
|
|
+ struct.pack("<q", ticks)
|
|
+ bytes([len(id_b)])
|
|
+ id_b
|
|
+ struct.pack("<i", len(payload))
|
|
+ payload
|
|
)
|
|
with path.open("ab" if path.exists() else "wb") as handle:
|
|
if offset:
|
|
handle.seek(offset)
|
|
start = handle.tell()
|
|
handle.write(blob)
|
|
return start
|
|
|
|
|
|
def test_recovered_index_line_and_local_ticks():
|
|
line = (
|
|
">DObject `frontlidar-msop-raw` post len=15532B, id:9CF1, "
|
|
"tic:639218060782100466, @data.bin:0"
|
|
)
|
|
match = RECORD_RE.search(line)
|
|
assert match is not None
|
|
assert match.group("name") == "frontlidar-msop-raw"
|
|
assert match.group("file") == "data.bin"
|
|
assert int(match.group("offset")) == 0
|
|
assert local_wall_to_dotnet_ticks("2026-08-08T17:14:38") == 639218060780000000
|
|
|
|
|
|
def test_parse_msop_and_difop_payload_roundtrip():
|
|
packet = _make_msop_packet(seconds=1700000000, microseconds=123456)
|
|
item = MsopPacketItem(
|
|
sequence=7,
|
|
device_timestamp_us=1700000000 * 1_000_000 + 123456,
|
|
device_timestamp_valid=True,
|
|
host_receive_utc_ticks=111,
|
|
host_receive_monotonic_ticks=222,
|
|
raw=packet,
|
|
)
|
|
msop_payload = build_msop_batch_payload(packets=[item], session_id="sess-a")
|
|
batch = parse_msop_batch_payload(msop_payload)
|
|
assert batch.session_id == "sess-a"
|
|
assert len(batch.packets) == 1
|
|
assert batch.packets[0].sequence == 7
|
|
assert batch.packets[0].device_timestamp_us == item.device_timestamp_us
|
|
assert batch.packets[0].device_timestamp_valid is True
|
|
assert batch.packets[0].raw == packet
|
|
|
|
vertical = [-16.0 + i * (32.0 / 31) for i in range(CHANNELS)]
|
|
difop_raw = _make_difop_packet(vertical=vertical, horizontal=[0.05] * CHANNELS)
|
|
difop_payload = build_difop_payload(raw=difop_raw, sequence=3)
|
|
difop = parse_difop_payload(difop_payload)
|
|
assert difop.sequence == 3
|
|
angles = parse_difop_angles(difop.raw)
|
|
assert angles.vertical_deg.shape == (CHANNELS,)
|
|
assert np.allclose(angles.vertical_deg, vertical, atol=1e-2)
|
|
assert np.allclose(angles.horizontal_deg, 0.05, atol=1e-2)
|
|
|
|
|
|
def test_difop_signed_angle_negative():
|
|
packet = _make_difop_packet(vertical=[-5.25] + [0.0] * 31)
|
|
angles = parse_difop_angles(packet)
|
|
assert abs(angles.vertical_deg[0] + 5.25) < 1e-9
|
|
|
|
|
|
def test_load_h32_dlog_lidar_mini_session(tmp_path: Path):
|
|
dlog = tmp_path / "session" / "dlog"
|
|
dorec_name = "raw.dorec"
|
|
dorec_path = dlog / "dobject_recording" / dorec_name
|
|
log_path = dlog / "dobject" / "rec.log"
|
|
|
|
vertical = [-16.0 + i * (32.0 / 31) for i in range(CHANNELS)]
|
|
difop_payload = build_difop_payload(raw=_make_difop_packet(vertical=vertical), sequence=1)
|
|
msop_packet = _make_msop_packet(az_deg=15.0)
|
|
msop_payload = build_msop_batch_payload(
|
|
packets=[
|
|
MsopPacketItem(
|
|
sequence=1,
|
|
device_timestamp_us=100_000_000,
|
|
device_timestamp_valid=True,
|
|
host_receive_utc_ticks=1,
|
|
host_receive_monotonic_ticks=2,
|
|
raw=msop_packet,
|
|
)
|
|
]
|
|
)
|
|
|
|
off_difop = _write_dorec_record(
|
|
dorec_path,
|
|
object_name="frontlidar-difop-raw",
|
|
ticks=1000,
|
|
record_id="AA",
|
|
payload=difop_payload,
|
|
)
|
|
off_msop = _write_dorec_record(
|
|
dorec_path,
|
|
object_name="frontlidar-msop-raw",
|
|
ticks=1001,
|
|
record_id="BB",
|
|
payload=msop_payload,
|
|
)
|
|
|
|
log_path.parent.mkdir(parents=True, exist_ok=True)
|
|
log_path.write_text(
|
|
"\n".join(
|
|
[
|
|
f"[t] DObject `frontlidar-difop-raw` post len={len(difop_payload)}B, "
|
|
f"id:AA, tic:1000, @{dorec_name}:{off_difop}",
|
|
f"[t] DObject `frontlidar-msop-raw` post len={len(msop_payload)}B, "
|
|
f"id:BB, tic:1001, @{dorec_name}:{off_msop}",
|
|
]
|
|
)
|
|
+ "\n",
|
|
encoding="utf-8",
|
|
)
|
|
|
|
root = resolve_dlog_root(tmp_path / "session")
|
|
assert root == dlog
|
|
assert len(discover_records(root, "frontlidar-msop-raw")) == 1
|
|
payloads = list(iter_payloads(root, "frontlidar-msop-raw"))
|
|
assert len(payloads) == 1
|
|
|
|
session = load_h32_dlog_lidar(tmp_path / "session", require_difop=True)
|
|
assert session.angle_source == "difop_channel_angles"
|
|
assert len(session.msop_packets) == 1
|
|
assert np.allclose(session.vertical_deg, vertical, atol=1e-2)
|
|
|
|
frames = iter_h32_frames_from_packets(
|
|
session.msop_packets,
|
|
min_frame_points=1,
|
|
vertical_deg=session.vertical_deg,
|
|
horizontal_deg=session.horizontal_deg,
|
|
)
|
|
assert len(frames) == 1
|
|
assert frames[0].points_xyz.shape[0] > 0
|