110 lines
3.3 KiB
Python
110 lines
3.3 KiB
Python
"""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,
|
|
)
|