支持 H32 DLogCapture（MSOP+DIFOP）导出到 V1 中间格式

Co-authored-by: Cursor <cursoragent@cursor.com>
2026-08-05 08:58:31 +08:00
parent 4ff176d184
commit 30f7e66db3
14 changed files with 993 additions and 43 deletions
@@ -0,0 +1,198 @@
+"""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 discover_records, iter_payloads, resolve_dlog_root
+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_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