支持 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,204 @@
+"""Parse RSLidarH32_3D_DLogCaptureNet48 raw MSOP/DIFOP DObject payloads."""
+
+from __future__ import annotations
+
+import io
+import struct
+from dataclasses import dataclass
+
+from .dotnet_bin import read_bool, read_dotnet_string, read_i32, read_i64
+
+MSOP_MAGIC = "RSLIDAR_H32_MSOP_DLOG_V1"
+DIFOP_MAGIC = "RSLIDAR_H32_DIFOP_DLOG_V1"
+
+
+@dataclass(frozen=True)
+class MsopPacketItem:
+    sequence: int
+    device_timestamp_us: int
+    device_timestamp_valid: bool
+    host_receive_utc_ticks: int
+    host_receive_monotonic_ticks: int
+    raw: bytes
+
+
+@dataclass(frozen=True)
+class MsopBatch:
+    version: int
+    session_id: str
+    session_start_utc_ticks: int
+    session_start_monotonic_ticks: int
+    monotonic_frequency: int
+    lidar_ip: str
+    msop_port: int
+    packets: list[MsopPacketItem]
+
+
+@dataclass(frozen=True)
+class DifopRecord:
+    version: int
+    session_id: str
+    session_start_utc_ticks: int
+    session_start_monotonic_ticks: int
+    monotonic_frequency: int
+    lidar_ip: str
+    difop_port: int
+    sequence: int
+    host_receive_utc_ticks: int
+    host_receive_monotonic_ticks: int
+    raw: bytes
+
+
+def _read_bytes(stream: io.BytesIO, length: int) -> bytes:
+    if length < 0 or length > 64 * 1024 * 1024:
+        raise ValueError(f"invalid byte length: {length}")
+    raw = stream.read(length)
+    if len(raw) != length:
+        raise EOFError(f"expected {length} bytes, got {len(raw)}")
+    return raw
+
+
+def parse_msop_batch_payload(payload: bytes) -> MsopBatch:
+    stream = io.BytesIO(payload)
+    magic = read_dotnet_string(stream)
+    if magic != MSOP_MAGIC:
+        raise ValueError(f"unexpected MSOP payload magic: {magic!r}")
+    version = read_i32(stream)
+    session_id = read_dotnet_string(stream)
+    session_start_utc_ticks = read_i64(stream)
+    session_start_monotonic_ticks = read_i64(stream)
+    monotonic_frequency = read_i64(stream)
+    lidar_ip = read_dotnet_string(stream)
+    msop_port = read_i32(stream)
+    packet_count = read_i32(stream)
+    if packet_count < 0 or packet_count > 100_000:
+        raise ValueError(f"invalid MSOP packet count: {packet_count}")
+    packets: list[MsopPacketItem] = []
+    for _ in range(packet_count):
+        packets.append(
+            MsopPacketItem(
+                sequence=read_i64(stream),
+                device_timestamp_us=read_i64(stream),
+                device_timestamp_valid=read_bool(stream),
+                host_receive_utc_ticks=read_i64(stream),
+                host_receive_monotonic_ticks=read_i64(stream),
+                raw=_read_bytes(stream, read_i32(stream)),
+            )
+        )
+    return MsopBatch(
+        version=version,
+        session_id=session_id,
+        session_start_utc_ticks=session_start_utc_ticks,
+        session_start_monotonic_ticks=session_start_monotonic_ticks,
+        monotonic_frequency=monotonic_frequency,
+        lidar_ip=lidar_ip,
+        msop_port=msop_port,
+        packets=packets,
+    )
+
+
+def parse_difop_payload(payload: bytes) -> DifopRecord:
+    stream = io.BytesIO(payload)
+    magic = read_dotnet_string(stream)
+    if magic != DIFOP_MAGIC:
+        raise ValueError(f"unexpected DIFOP payload magic: {magic!r}")
+    version = read_i32(stream)
+    session_id = read_dotnet_string(stream)
+    session_start_utc_ticks = read_i64(stream)
+    session_start_monotonic_ticks = read_i64(stream)
+    monotonic_frequency = read_i64(stream)
+    lidar_ip = read_dotnet_string(stream)
+    difop_port = read_i32(stream)
+    sequence = read_i64(stream)
+    host_receive_utc_ticks = read_i64(stream)
+    host_receive_monotonic_ticks = read_i64(stream)
+    raw = _read_bytes(stream, read_i32(stream))
+    return DifopRecord(
+        version=version,
+        session_id=session_id,
+        session_start_utc_ticks=session_start_utc_ticks,
+        session_start_monotonic_ticks=session_start_monotonic_ticks,
+        monotonic_frequency=monotonic_frequency,
+        lidar_ip=lidar_ip,
+        difop_port=difop_port,
+        sequence=sequence,
+        host_receive_utc_ticks=host_receive_utc_ticks,
+        host_receive_monotonic_ticks=host_receive_monotonic_ticks,
+        raw=raw,
+    )
+
+
+def build_msop_batch_payload(
+    *,
+    version: int = 1,
+    session_id: str = "test",
+    session_start_utc_ticks: int = 0,
+    session_start_monotonic_ticks: int = 0,
+    monotonic_frequency: int = 10_000_000,
+    lidar_ip: str = "192.168.1.200",
+    msop_port: int = 6699,
+    packets: list[MsopPacketItem],
+) -> bytes:
+    """Test helper: write an MSOP batch matching the C# BinaryWriter layout."""
+
+    from .dotnet_bin import write_bool, write_dotnet_string
+
+    stream = io.BytesIO()
+    write_dotnet_string(stream, MSOP_MAGIC)
+    stream.write(struct.pack("<i", version))
+    write_dotnet_string(stream, session_id)
+    stream.write(struct.pack("<qqq", session_start_utc_ticks, session_start_monotonic_ticks, monotonic_frequency))
+    write_dotnet_string(stream, lidar_ip)
+    stream.write(struct.pack("<i", msop_port))
+    stream.write(struct.pack("<i", len(packets)))
+    for item in packets:
+        stream.write(struct.pack("<qq", item.sequence, item.device_timestamp_us))
+        write_bool(stream, item.device_timestamp_valid)
+        stream.write(
+            struct.pack(
+                "<qqi",
+                item.host_receive_utc_ticks,
+                item.host_receive_monotonic_ticks,
+                len(item.raw),
+            )
+        )
+        stream.write(item.raw)
+    return stream.getvalue()
+
+
+def build_difop_payload(
+    *,
+    version: int = 1,
+    session_id: str = "test",
+    session_start_utc_ticks: int = 0,
+    session_start_monotonic_ticks: int = 0,
+    monotonic_frequency: int = 10_000_000,
+    lidar_ip: str = "192.168.1.200",
+    difop_port: int = 7788,
+    sequence: int = 1,
+    host_receive_utc_ticks: int = 0,
+    host_receive_monotonic_ticks: int = 0,
+    raw: bytes,
+) -> bytes:
+    """Test helper: write a DIFOP record matching the C# BinaryWriter layout."""
+
+    from .dotnet_bin import write_dotnet_string
+
+    stream = io.BytesIO()
+    write_dotnet_string(stream, DIFOP_MAGIC)
+    stream.write(struct.pack("<i", version))
+    write_dotnet_string(stream, session_id)
+    stream.write(struct.pack("<qqq", session_start_utc_ticks, session_start_monotonic_ticks, monotonic_frequency))
+    write_dotnet_string(stream, lidar_ip)
+    stream.write(struct.pack("<i", difop_port))
+    stream.write(
+        struct.pack(
+            "<qqqi",
+            sequence,
+            host_receive_utc_ticks,
+            host_receive_monotonic_ticks,
+            len(raw),
+        )
+    )
+    stream.write(raw)
+    return stream.getvalue()