191 lines
6.6 KiB
Python
191 lines
6.6 KiB
Python
"""Unit tests for H32 Medulla raw dlog → station frame export helpers."""
|
|
|
|
from __future__ import annotations
|
|
|
|
import struct
|
|
import sys
|
|
from pathlib import Path
|
|
|
|
import numpy as np
|
|
|
|
ROOT = Path(__file__).resolve().parents[1]
|
|
TOOLS = ROOT / "tools"
|
|
sys.path.insert(0, str(TOOLS))
|
|
sys.path.insert(0, str(TOOLS / "rscap_v2"))
|
|
|
|
from export_h32_rscap_station import export_station_h32_dlog, is_h32_raw_dlog_station # noqa: E402
|
|
from h32_dlog.difop import CHANNELS, HORIZONTAL_START, VERTICAL_START, parse_difop_angles # noqa: E402
|
|
from h32_dlog.dobject import discover_records, iter_payloads, resolve_dlog_root # noqa: E402
|
|
from h32_dlog.load_session import load_h32_dlog_lidar # noqa: E402
|
|
from h32_dlog.payload_v1 import ( # noqa: E402
|
|
MsopPacketItem,
|
|
build_difop_payload,
|
|
build_msop_batch_payload,
|
|
parse_difop_payload,
|
|
parse_msop_batch_payload,
|
|
)
|
|
from h32_msop import PACKET_LENGTH, iter_h32_frames_polar_from_packets # noqa: E402
|
|
|
|
|
|
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,
|
|
) -> int:
|
|
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:
|
|
start = handle.tell()
|
|
handle.write(blob)
|
|
return start
|
|
|
|
|
|
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].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 = parse_difop_payload(build_difop_payload(raw=difop_raw, sequence=3))
|
|
angles = parse_difop_angles(difop.raw)
|
|
assert np.allclose(angles.vertical_deg, vertical, atol=1e-2)
|
|
assert np.allclose(angles.horizontal_deg, 0.05, atol=1e-2)
|
|
|
|
|
|
def test_export_station_h32_dlog_mini(tmp_path: Path):
|
|
station = tmp_path / "001"
|
|
dorec_name = "raw.dorec"
|
|
dorec_path = station / "dobject_recording" / dorec_name
|
|
log_path = station / "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=621355968000000000 + 10_000_000,
|
|
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",
|
|
)
|
|
|
|
assert resolve_dlog_root(station) == station.resolve()
|
|
assert is_h32_raw_dlog_station(station)
|
|
assert len(discover_records(station, "frontlidar-msop-raw")) == 1
|
|
assert len(list(iter_payloads(station, "frontlidar-msop-raw"))) == 1
|
|
|
|
session = load_h32_dlog_lidar(station, require_difop=True)
|
|
assert session.angle_source == "difop_channel_angles"
|
|
frames = iter_h32_frames_polar_from_packets(
|
|
session.msop_packets,
|
|
host_utc_ticks=session.msop_host_utc_ticks,
|
|
min_frame_points=1,
|
|
vertical_deg=session.vertical_deg,
|
|
horizontal_deg=session.horizontal_deg,
|
|
)
|
|
assert len(frames) == 1
|
|
assert frames[0].points_raw.shape[1] == 5
|
|
|
|
out = tmp_path / "export"
|
|
meta = export_station_h32_dlog(station, out, require_difop=True, min_frame_points=1)
|
|
assert meta["kind"] == "h32_dlog_raw"
|
|
assert meta["angle_source"] == "difop_channel_angles"
|
|
assert meta["frames_written"] >= 1
|
|
assert any((out / "frames").glob("*.npz"))
|