支持 H32 DLogCapture（MSOP+DIFOP）站导出到 combined
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
lichun.qu
Cursor
@@ -0,0 +1,17 @@
|
||||
"""Medulla dlog readers for RSLidarH32_3D_DLogCaptureNet48 raw MSOP/DIFOP."""
|
||||
|
||||
from .difop import parse_difop_angles
|
||||
from .dobject import discover_records, iter_payloads, resolve_dlog_root
|
||||
from .load_session import H32DlogLidarSession, load_h32_dlog_lidar
|
||||
from .payload_v1 import parse_difop_payload, parse_msop_batch_payload
|
||||
|
||||
__all__ = [
|
||||
"H32DlogLidarSession",
|
||||
"discover_records",
|
||||
"iter_payloads",
|
||||
"load_h32_dlog_lidar",
|
||||
"parse_difop_angles",
|
||||
"parse_difop_payload",
|
||||
"parse_msop_batch_payload",
|
||||
"resolve_dlog_root",
|
||||
]
|
||||
@@ -0,0 +1,40 @@
|
||||
"""Parse RoboSense H32 DIFOP channel calibration angles."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
|
||||
import numpy as np
|
||||
|
||||
CHANNELS = 32
|
||||
VERTICAL_START = 468
|
||||
HORIZONTAL_START = 564
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class DifopAngles:
|
||||
vertical_deg: np.ndarray # (32,)
|
||||
horizontal_deg: np.ndarray # (32,)
|
||||
|
||||
|
||||
def _read_u16_be(packet: bytes, index: int) -> int:
|
||||
return (packet[index] << 8) | packet[index + 1]
|
||||
|
||||
|
||||
def signed_angle_deg(packet: bytes, index: int) -> float:
|
||||
"""Match RSLidarH32 plugin SignedAngle: sign byte + BE u16 * 0.01 deg."""
|
||||
|
||||
sign = -1.0 if packet[index] > 0 else 1.0
|
||||
return sign * _read_u16_be(packet, index + 1) * 0.01
|
||||
|
||||
|
||||
def parse_difop_angles(packet: bytes) -> DifopAngles:
|
||||
needed = HORIZONTAL_START + CHANNELS * 3
|
||||
if len(packet) < needed:
|
||||
raise ValueError(f"DIFOP packet too short: {len(packet)} < {needed}")
|
||||
vertical = np.empty(CHANNELS, dtype=np.float64)
|
||||
horizontal = np.empty(CHANNELS, dtype=np.float64)
|
||||
for channel in range(CHANNELS):
|
||||
vertical[channel] = signed_angle_deg(packet, VERTICAL_START + channel * 3)
|
||||
horizontal[channel] = signed_angle_deg(packet, HORIZONTAL_START + channel * 3)
|
||||
return DifopAngles(vertical_deg=vertical, horizontal_deg=horizontal)
|
||||
@@ -0,0 +1,179 @@
|
||||
"""Index and read Medulla DObject recordings (dobject/ + dobject_recording/)."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import re
|
||||
import struct
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
from typing import BinaryIO, Iterator
|
||||
|
||||
|
||||
RECORD_RE = re.compile(
|
||||
r"^\[(?P<log_time>[^]]+)\].*?DObject `(?P<name>[^`]+)` post "
|
||||
r"len=(?P<len>\d+)B, id:(?P<id>[0-9A-Fa-f]+), tic:(?P<tic>\d+), "
|
||||
r"@(?P<file>[^:]+):(?P<offset>\d+)"
|
||||
)
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class RecordRef:
|
||||
sequence: int
|
||||
object_name: str
|
||||
log_time: str
|
||||
source_log: str
|
||||
source_dorec: str
|
||||
source_offset: int
|
||||
payload_length: int
|
||||
log_record_id: str
|
||||
dotnet_ticks: int
|
||||
|
||||
|
||||
def resolve_dlog_root(value: Path | str) -> Path:
|
||||
root = Path(value).expanduser().resolve()
|
||||
if (root / "dobject").is_dir() and (root / "dobject_recording").is_dir():
|
||||
return root
|
||||
child = root / "dlog"
|
||||
if (child / "dobject").is_dir() and (child / "dobject_recording").is_dir():
|
||||
return child
|
||||
raise FileNotFoundError(f"{root} does not contain dobject and dobject_recording")
|
||||
|
||||
|
||||
def discover_records(dlog_root: Path, object_name: str) -> list[RecordRef]:
|
||||
pending: list[tuple[str, str, str, int, int, str, int, str]] = []
|
||||
for log_path in sorted((dlog_root / "dobject").rglob("*.log")):
|
||||
relative_log = log_path.relative_to(dlog_root).as_posix()
|
||||
with log_path.open("r", encoding="utf-8", errors="replace") as stream:
|
||||
for line in stream:
|
||||
match = RECORD_RE.search(line)
|
||||
if not match or match.group("name").casefold() != object_name.casefold():
|
||||
continue
|
||||
pending.append(
|
||||
(
|
||||
match.group("name"),
|
||||
match.group("log_time"),
|
||||
relative_log,
|
||||
int(match.group("offset")),
|
||||
int(match.group("len")),
|
||||
match.group("id").upper(),
|
||||
int(match.group("tic")),
|
||||
match.group("file"),
|
||||
)
|
||||
)
|
||||
pending.sort(key=lambda item: (item[6], item[7].casefold(), item[3]))
|
||||
seen: set[tuple[str, int, int]] = set()
|
||||
records: list[RecordRef] = []
|
||||
for item in pending:
|
||||
key = (item[7].casefold(), item[3], item[6])
|
||||
if key in seen:
|
||||
continue
|
||||
seen.add(key)
|
||||
records.append(
|
||||
RecordRef(
|
||||
sequence=len(records),
|
||||
object_name=item[0],
|
||||
log_time=item[1],
|
||||
source_log=item[2],
|
||||
source_dorec=item[7],
|
||||
source_offset=item[3],
|
||||
payload_length=item[4],
|
||||
log_record_id=item[5],
|
||||
dotnet_ticks=item[6],
|
||||
)
|
||||
)
|
||||
return records
|
||||
|
||||
|
||||
def index_dorec_files(dlog_root: Path) -> dict[str, list[Path]]:
|
||||
result: dict[str, list[Path]] = {}
|
||||
for path in (dlog_root / "dobject_recording").rglob("*.dorec"):
|
||||
result.setdefault(path.name.casefold(), []).append(path)
|
||||
return result
|
||||
|
||||
|
||||
def choose_dorec(index: dict[str, list[Path]], name: str) -> Path:
|
||||
matches = index.get(Path(name).name.casefold(), [])
|
||||
if not matches:
|
||||
raise FileNotFoundError(f"missing recording file: {name}")
|
||||
if len(matches) > 1:
|
||||
raise RuntimeError(f"ambiguous recording file {name}: {matches}")
|
||||
return matches[0]
|
||||
|
||||
|
||||
def read_exact(stream: BinaryIO, size: int) -> bytes:
|
||||
data = stream.read(size)
|
||||
if len(data) != size:
|
||||
raise EOFError(f"expected {size} bytes, got {len(data)}")
|
||||
return data
|
||||
|
||||
|
||||
def read_record_payload(path: Path, record: RecordRef) -> bytes:
|
||||
with path.open("rb") as stream:
|
||||
stream.seek(record.source_offset)
|
||||
name_length = read_exact(stream, 1)[0]
|
||||
name = read_exact(stream, name_length).decode("ascii")
|
||||
ticks = struct.unpack("<q", read_exact(stream, 8))[0]
|
||||
id_length = read_exact(stream, 1)[0]
|
||||
id_bytes = read_exact(stream, id_length)
|
||||
payload_length = struct.unpack("<i", read_exact(stream, 4))[0]
|
||||
payload = read_exact(stream, payload_length)
|
||||
|
||||
try:
|
||||
record_id = id_bytes.decode("ascii")
|
||||
except UnicodeDecodeError:
|
||||
record_id = id_bytes.hex().upper()
|
||||
if name != record.object_name:
|
||||
raise ValueError(f"name mismatch: log={record.object_name}, dorec={name}")
|
||||
if ticks != record.dotnet_ticks:
|
||||
raise ValueError(f"tick mismatch: log={record.dotnet_ticks}, dorec={ticks}")
|
||||
if payload_length != record.payload_length:
|
||||
raise ValueError(f"payload mismatch: log={record.payload_length}, dorec={payload_length}")
|
||||
if record_id.upper() != record.log_record_id.upper():
|
||||
raise ValueError(f"record id mismatch: log={record.log_record_id}, dorec={record_id}")
|
||||
return payload
|
||||
|
||||
|
||||
def iter_payloads(dlog_root: Path, object_name: str) -> Iterator[tuple[RecordRef, bytes]]:
|
||||
root = resolve_dlog_root(dlog_root)
|
||||
records = discover_records(root, object_name)
|
||||
if not records:
|
||||
return
|
||||
dorec_index = index_dorec_files(root)
|
||||
open_files: dict[str, tuple[Path, BinaryIO]] = {}
|
||||
try:
|
||||
for record in records:
|
||||
key = record.source_dorec.casefold()
|
||||
handle = open_files.get(key)
|
||||
if handle is None:
|
||||
path = choose_dorec(dorec_index, record.source_dorec)
|
||||
handle = (path, path.open("rb"))
|
||||
open_files[key] = handle
|
||||
path, stream = handle
|
||||
stream.seek(record.source_offset)
|
||||
name_length = read_exact(stream, 1)[0]
|
||||
name = read_exact(stream, name_length).decode("ascii")
|
||||
ticks = struct.unpack("<q", read_exact(stream, 8))[0]
|
||||
id_length = read_exact(stream, 1)[0]
|
||||
id_bytes = read_exact(stream, id_length)
|
||||
payload_length = struct.unpack("<i", read_exact(stream, 4))[0]
|
||||
payload = read_exact(stream, payload_length)
|
||||
try:
|
||||
record_id = id_bytes.decode("ascii")
|
||||
except UnicodeDecodeError:
|
||||
record_id = id_bytes.hex().upper()
|
||||
if name != record.object_name:
|
||||
raise ValueError(f"name mismatch: log={record.object_name}, dorec={name}")
|
||||
if ticks != record.dotnet_ticks:
|
||||
raise ValueError(f"tick mismatch: log={record.dotnet_ticks}, dorec={ticks}")
|
||||
if payload_length != record.payload_length:
|
||||
raise ValueError(
|
||||
f"payload mismatch: log={record.payload_length}, dorec={payload_length}"
|
||||
)
|
||||
if record_id.upper() != record.log_record_id.upper():
|
||||
raise ValueError(
|
||||
f"record id mismatch: log={record.log_record_id}, dorec={record_id}"
|
||||
)
|
||||
yield record, payload
|
||||
finally:
|
||||
for _path, stream in open_files.values():
|
||||
stream.close()
|
||||
@@ -0,0 +1,69 @@
|
||||
"""Little-endian .NET BinaryReader/BinaryWriter helpers."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import struct
|
||||
from typing import BinaryIO
|
||||
|
||||
|
||||
def read_7bit_int(stream: BinaryIO) -> int:
|
||||
value = 0
|
||||
shift = 0
|
||||
while True:
|
||||
raw = stream.read(1)
|
||||
if not raw:
|
||||
raise EOFError("truncated .NET 7-bit int")
|
||||
value |= (raw[0] & 0x7F) << shift
|
||||
if not raw[0] & 0x80:
|
||||
return value
|
||||
shift += 7
|
||||
if shift > 35:
|
||||
raise ValueError("invalid .NET 7-bit int")
|
||||
|
||||
|
||||
def write_7bit_int(stream: BinaryIO, value: int) -> None:
|
||||
if value < 0:
|
||||
raise ValueError("7-bit int must be non-negative")
|
||||
while value >= 0x80:
|
||||
stream.write(bytes([(value & 0x7F) | 0x80]))
|
||||
value >>= 7
|
||||
stream.write(bytes([value & 0x7F]))
|
||||
|
||||
|
||||
def read_dotnet_string(stream: BinaryIO) -> str:
|
||||
length = read_7bit_int(stream)
|
||||
raw = stream.read(length)
|
||||
if len(raw) != length:
|
||||
raise EOFError("truncated .NET string")
|
||||
return raw.decode("utf-8")
|
||||
|
||||
|
||||
def write_dotnet_string(stream: BinaryIO, text: str) -> None:
|
||||
raw = text.encode("utf-8")
|
||||
write_7bit_int(stream, len(raw))
|
||||
stream.write(raw)
|
||||
|
||||
|
||||
def read_i32(stream: BinaryIO) -> int:
|
||||
raw = stream.read(4)
|
||||
if len(raw) != 4:
|
||||
raise EOFError("truncated int32")
|
||||
return struct.unpack("<i", raw)[0]
|
||||
|
||||
|
||||
def read_i64(stream: BinaryIO) -> int:
|
||||
raw = stream.read(8)
|
||||
if len(raw) != 8:
|
||||
raise EOFError("truncated int64")
|
||||
return struct.unpack("<q", raw)[0]
|
||||
|
||||
|
||||
def read_bool(stream: BinaryIO) -> bool:
|
||||
raw = stream.read(1)
|
||||
if not raw:
|
||||
raise EOFError("truncated bool")
|
||||
return raw[0] != 0
|
||||
|
||||
|
||||
def write_bool(stream: BinaryIO, value: bool) -> None:
|
||||
stream.write(b"\x01" if value else b"\x00")
|
||||
@@ -0,0 +1,109 @@
|
||||
"""Load H32 MSOP packets and DIFOP angles from a Medulla dlog session."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import sys
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
|
||||
_TOOLS = Path(__file__).resolve().parents[1]
|
||||
_RSCAP_V2 = _TOOLS / "rscap_v2"
|
||||
if str(_RSCAP_V2) not in sys.path:
|
||||
sys.path.insert(0, str(_RSCAP_V2))
|
||||
|
||||
from h32_msop import default_horizontal_deg, default_vertical_deg # noqa: E402
|
||||
|
||||
from .difop import DifopAngles, parse_difop_angles
|
||||
from .dobject import discover_records, iter_payloads, resolve_dlog_root
|
||||
from .payload_v1 import parse_difop_payload, parse_msop_batch_payload
|
||||
|
||||
|
||||
@dataclass
|
||||
class H32DlogLidarSession:
|
||||
dlog_root: Path
|
||||
msop_object: str
|
||||
difop_object: str
|
||||
msop_packets: list[bytes]
|
||||
msop_host_utc_ticks: list[int]
|
||||
msop_batch_count: int
|
||||
difop_record_count: int
|
||||
angle_source: str
|
||||
vertical_deg: np.ndarray
|
||||
horizontal_deg: np.ndarray
|
||||
session_id: str | None = None
|
||||
lidar_ip: str | None = None
|
||||
|
||||
|
||||
def load_h32_dlog_lidar(
|
||||
dlog_root: Path | str,
|
||||
*,
|
||||
msop_object: str = "frontlidar-msop-raw",
|
||||
difop_object: str = "frontlidar-difop-raw",
|
||||
require_difop: bool = False,
|
||||
) -> H32DlogLidarSession:
|
||||
root = resolve_dlog_root(dlog_root)
|
||||
msop_packets: list[bytes] = []
|
||||
msop_host_utc_ticks: list[int] = []
|
||||
batch_count = 0
|
||||
session_id: str | None = None
|
||||
lidar_ip: str | None = None
|
||||
|
||||
for _record, payload in iter_payloads(root, msop_object):
|
||||
batch = parse_msop_batch_payload(payload)
|
||||
batch_count += 1
|
||||
if session_id is None:
|
||||
session_id = batch.session_id
|
||||
lidar_ip = batch.lidar_ip
|
||||
for item in batch.packets:
|
||||
msop_packets.append(item.raw)
|
||||
msop_host_utc_ticks.append(int(item.host_receive_utc_ticks))
|
||||
|
||||
angles: DifopAngles | None = None
|
||||
difop_count = 0
|
||||
for _record, payload in iter_payloads(root, difop_object):
|
||||
difop = parse_difop_payload(payload)
|
||||
difop_count += 1
|
||||
try:
|
||||
angles = parse_difop_angles(difop.raw)
|
||||
except ValueError:
|
||||
continue
|
||||
if session_id is None:
|
||||
session_id = difop.session_id
|
||||
lidar_ip = difop.lidar_ip
|
||||
|
||||
if not msop_packets:
|
||||
msop_records = discover_records(root, msop_object)
|
||||
raise RuntimeError(
|
||||
f"no MSOP packets from DObject {msop_object!r} under {root} "
|
||||
f"(log records={len(msop_records)})"
|
||||
)
|
||||
|
||||
if angles is None:
|
||||
if require_difop:
|
||||
raise RuntimeError(
|
||||
f"no valid DIFOP calibration from DObject {difop_object!r} under {root}"
|
||||
)
|
||||
vertical = default_vertical_deg()
|
||||
horizontal = default_horizontal_deg()
|
||||
angle_source = "default_msop_only_vertical_-16_to_16_deg"
|
||||
else:
|
||||
vertical = angles.vertical_deg
|
||||
horizontal = angles.horizontal_deg
|
||||
angle_source = "difop_channel_angles"
|
||||
|
||||
return H32DlogLidarSession(
|
||||
dlog_root=root,
|
||||
msop_object=msop_object,
|
||||
difop_object=difop_object,
|
||||
msop_packets=msop_packets,
|
||||
msop_host_utc_ticks=msop_host_utc_ticks,
|
||||
msop_batch_count=batch_count,
|
||||
difop_record_count=difop_count,
|
||||
angle_source=angle_source,
|
||||
vertical_deg=vertical,
|
||||
horizontal_deg=horizontal,
|
||||
session_id=session_id,
|
||||
lidar_ip=lidar_ip,
|
||||
)
|
||||
@@ -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()
|
||||
Reference in New Issue
Block a user