支持 H32 DLogCapture（MSOP+DIFOP）站导出到 combined

Co-authored-by: Cursor <cursoragent@cursor.com>
2026-08-05 10:17:23 +08:00
parent b2271d05ba
commit 69bb44bccd
15 changed files with 1168 additions and 105 deletions
@@ -0,0 +1,190 @@
+"""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"))