141 lines
4.8 KiB
Python
141 lines
4.8 KiB
Python
"""Load H32 MSOP packets and DIFOP angles from a Medulla dlog session."""
|
|
|
|
from __future__ import annotations
|
|
|
|
from dataclasses import dataclass
|
|
from pathlib import Path
|
|
|
|
import numpy as np
|
|
|
|
from tools.rscap_v2.h32_msop import default_horizontal_deg, default_vertical_deg
|
|
|
|
from .difop import DifopAngles, parse_difop_angles
|
|
from .dobject import (
|
|
discover_records_from_source,
|
|
iter_payloads_from_source,
|
|
open_dlog_source,
|
|
)
|
|
from .payload_v1 import parse_difop_payload, parse_msop_batch_payload
|
|
|
|
|
|
@dataclass
|
|
class H32DlogLidarSession:
|
|
dlog_root: str
|
|
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
|
|
host_ticks_min: int | None = None
|
|
host_ticks_max: int | 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,
|
|
host_ticks_min: int | None = None,
|
|
host_ticks_max: int | None = None,
|
|
) -> H32DlogLidarSession:
|
|
with open_dlog_source(dlog_root) as source:
|
|
# DIFOP angles: prefer packets inside the window, else any in the capture.
|
|
angles: DifopAngles | None = None
|
|
difop_count = 0
|
|
session_id: str | None = None
|
|
lidar_ip: str | None = None
|
|
for _record, payload in iter_payloads_from_source(
|
|
source,
|
|
difop_object,
|
|
host_ticks_min=host_ticks_min,
|
|
host_ticks_max=host_ticks_max,
|
|
):
|
|
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 angles is None:
|
|
for _record, payload in iter_payloads_from_source(source, 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 angles is not None:
|
|
break
|
|
|
|
msop_packets: list[bytes] = []
|
|
msop_host_utc_ticks: list[int] = []
|
|
batch_count = 0
|
|
for record, payload in iter_payloads_from_source(
|
|
source,
|
|
msop_object,
|
|
host_ticks_min=host_ticks_min,
|
|
host_ticks_max=host_ticks_max,
|
|
):
|
|
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)
|
|
# Per-packet UTC host receive from MSOP DLog payload only.
|
|
# Do NOT fall back to DObject tic (DateTime.Now / local).
|
|
msop_host_utc_ticks.append(int(item.host_receive_utc_ticks))
|
|
|
|
if not msop_packets:
|
|
msop_records = discover_records_from_source(source, msop_object)
|
|
raise RuntimeError(
|
|
f"no MSOP packets from DObject {msop_object!r} under {source.label} "
|
|
f"(log records={len(msop_records)}, "
|
|
f"host_ticks=[{host_ticks_min}, {host_ticks_max}])"
|
|
)
|
|
|
|
if angles is None:
|
|
if require_difop:
|
|
raise RuntimeError(
|
|
f"no valid DIFOP calibration from DObject {difop_object!r} under {source.label}"
|
|
)
|
|
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=source.label,
|
|
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,
|
|
host_ticks_min=host_ticks_min,
|
|
host_ticks_max=host_ticks_max,
|
|
)
|