支持 HI13/H32 主机 UTC 桥接对齐、多会话联合标定与 CAD 平移先验。
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
@@ -1,12 +1,18 @@
|
||||
"""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 .dobject import discover_records, iter_payloads, open_dlog_source, resolve_dlog_root
|
||||
from .load_session import H32DlogLidarSession, load_h32_dlog_lidar
|
||||
from .payload_v1 import parse_difop_payload, parse_msop_batch_payload
|
||||
from .timeutil import local_wall_to_dotnet_ticks
|
||||
|
||||
__all__ = [
|
||||
"H32DlogLidarSession",
|
||||
"discover_records",
|
||||
"iter_payloads",
|
||||
"load_h32_dlog_lidar",
|
||||
"local_wall_to_dotnet_ticks",
|
||||
"open_dlog_source",
|
||||
"parse_difop_angles",
|
||||
"parse_difop_payload",
|
||||
"parse_msop_batch_payload",
|
||||
|
||||
+283
-81
@@ -1,16 +1,24 @@
|
||||
"""Index and read Medulla DObject recordings (dobject/ + dobject_recording/)."""
|
||||
"""Index and read Medulla DObject recordings.
|
||||
|
||||
Supports:
|
||||
|
||||
- standard layout: ``dobject/**/*.log`` + ``dobject_recording/**/*.dorec``
|
||||
- recovered layout: ``dobject/all/indices.log`` + ``dobject_recording/data.bin``
|
||||
- either as an extracted directory or a zip containing those paths
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import re
|
||||
import struct
|
||||
import zipfile
|
||||
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"^(?:\[(?P<log_time>[^]]+)\])?>?\s*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+)"
|
||||
)
|
||||
@@ -29,37 +37,225 @@ class RecordRef:
|
||||
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")
|
||||
class _ZipStoredMemberIO:
|
||||
"""Random-access reader for a ZIP_STORED member via the underlying zip file.
|
||||
|
||||
``ZipExtFile.seek`` on multi-GB members is far too slow for per-record reads.
|
||||
"""
|
||||
|
||||
def __init__(self, zip_path: Path, member_name: str, data_offset: int, data_size: int):
|
||||
self._path = zip_path
|
||||
self._member_name = member_name
|
||||
self._data_offset = data_offset
|
||||
self._data_size = data_size
|
||||
self._fh = zip_path.open("rb")
|
||||
self._pos = 0
|
||||
|
||||
def seek(self, offset: int, whence: int = 0) -> int:
|
||||
if whence == 0:
|
||||
self._pos = offset
|
||||
elif whence == 1:
|
||||
self._pos += offset
|
||||
elif whence == 2:
|
||||
self._pos = self._data_size + offset
|
||||
else:
|
||||
raise ValueError(f"invalid whence: {whence}")
|
||||
if self._pos < 0:
|
||||
raise ValueError("negative seek")
|
||||
return self._pos
|
||||
|
||||
def read(self, size: int = -1) -> bytes:
|
||||
if size is None or size < 0:
|
||||
size = self._data_size - self._pos
|
||||
if size <= 0 or self._pos >= self._data_size:
|
||||
return b""
|
||||
size = min(size, self._data_size - self._pos)
|
||||
self._fh.seek(self._data_offset + self._pos)
|
||||
data = self._fh.read(size)
|
||||
self._pos += len(data)
|
||||
return data
|
||||
|
||||
def close(self) -> None:
|
||||
self._fh.close()
|
||||
|
||||
|
||||
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"),
|
||||
)
|
||||
def _zip_stored_member_offset(zip_path: Path, info: zipfile.ZipInfo) -> int:
|
||||
if info.compress_type != zipfile.ZIP_STORED:
|
||||
raise RuntimeError(
|
||||
f"member {info.filename!r} is compressed (type={info.compress_type}); "
|
||||
"extract it first or store uncompressed"
|
||||
)
|
||||
with zip_path.open("rb") as handle:
|
||||
handle.seek(info.header_offset)
|
||||
header = handle.read(30)
|
||||
if len(header) != 30 or header[:4] != b"PK\x03\x04":
|
||||
raise RuntimeError(f"bad local zip header for {info.filename!r}")
|
||||
name_len, extra_len = struct.unpack("<HH", header[26:30])
|
||||
return info.header_offset + 30 + name_len + extra_len
|
||||
|
||||
|
||||
@dataclass
|
||||
class DlogSource:
|
||||
"""Opened dlog directory or recovered zip."""
|
||||
|
||||
label: str
|
||||
directory: Path | None = None
|
||||
zip_path: Path | None = None
|
||||
_zip: zipfile.ZipFile | None = None
|
||||
_log_cache: dict[str, str] | None = None
|
||||
_member_offsets: dict[str, tuple[int, int]] | None = None
|
||||
|
||||
def close(self) -> None:
|
||||
if self._zip is not None:
|
||||
self._zip.close()
|
||||
self._zip = None
|
||||
|
||||
def __enter__(self) -> "DlogSource":
|
||||
return self
|
||||
|
||||
def __exit__(self, exc_type, exc, tb) -> None:
|
||||
self.close()
|
||||
|
||||
def iter_log_texts(self) -> Iterator[tuple[str, str]]:
|
||||
if self.zip_path is not None:
|
||||
assert self._zip is not None
|
||||
if self._log_cache is None:
|
||||
self._log_cache = {}
|
||||
names = sorted(
|
||||
name
|
||||
for name in self._zip.namelist()
|
||||
if name.replace("\\", "/").startswith("dobject/")
|
||||
and name.replace("\\", "/").endswith(".log")
|
||||
)
|
||||
for name in names:
|
||||
key = name.replace("\\", "/")
|
||||
self._log_cache[key] = self._zip.read(name).decode("utf-8", errors="replace")
|
||||
for name, text in self._log_cache.items():
|
||||
yield name, text
|
||||
return
|
||||
assert self.directory is not None
|
||||
for log_path in sorted((self.directory / "dobject").rglob("*.log")):
|
||||
relative = log_path.relative_to(self.directory).as_posix()
|
||||
yield relative, log_path.read_text(encoding="utf-8", errors="replace")
|
||||
|
||||
def open_recording(self, name: str) -> tuple[object, BinaryIO]:
|
||||
"""Return (owner, binary stream) supporting seek/read of one recording member."""
|
||||
|
||||
base = Path(name).name
|
||||
if self.zip_path is not None:
|
||||
assert self._zip is not None
|
||||
candidates = [
|
||||
n
|
||||
for n in self._zip.namelist()
|
||||
if Path(n.replace("\\", "/")).name.casefold() == base.casefold()
|
||||
and "dobject_recording/" in n.replace("\\", "/")
|
||||
]
|
||||
if not candidates:
|
||||
alt = name.replace("\\", "/")
|
||||
if alt in self._zip.namelist():
|
||||
candidates = [alt]
|
||||
elif f"dobject_recording/{base}" in self._zip.namelist():
|
||||
candidates = [f"dobject_recording/{base}"]
|
||||
if not candidates:
|
||||
raise FileNotFoundError(f"missing recording in zip: {name}")
|
||||
if len(candidates) > 1:
|
||||
raise RuntimeError(f"ambiguous recording in zip {name}: {candidates}")
|
||||
member = candidates[0].replace("\\", "/")
|
||||
if self._member_offsets is None:
|
||||
self._member_offsets = {}
|
||||
if member not in self._member_offsets:
|
||||
info = self._zip.getinfo(member)
|
||||
self._member_offsets[member] = (
|
||||
_zip_stored_member_offset(self.zip_path, info),
|
||||
info.file_size,
|
||||
)
|
||||
data_offset, data_size = self._member_offsets[member]
|
||||
stream = _ZipStoredMemberIO(self.zip_path, member, data_offset, data_size)
|
||||
return stream, stream
|
||||
|
||||
assert self.directory is not None
|
||||
index = index_dorec_files(self.directory)
|
||||
if base.casefold() == "data.bin":
|
||||
path = self.directory / "dobject_recording" / "data.bin"
|
||||
if not path.is_file():
|
||||
matches = list((self.directory / "dobject_recording").rglob("data.bin"))
|
||||
if not matches:
|
||||
raise FileNotFoundError(f"missing recording file: {name}")
|
||||
path = matches[0]
|
||||
stream = path.open("rb")
|
||||
return stream, stream
|
||||
path = choose_dorec(index, name)
|
||||
stream = path.open("rb")
|
||||
return stream, stream
|
||||
|
||||
|
||||
def open_dlog_source(value: Path | str) -> DlogSource:
|
||||
path = Path(value).expanduser().resolve()
|
||||
if path.is_file() and path.suffix.lower() == ".zip":
|
||||
zf = zipfile.ZipFile(path, "r")
|
||||
names = {n.replace("\\", "/") for n in zf.namelist()}
|
||||
has_log = any(n.startswith("dobject/") and n.endswith(".log") for n in names)
|
||||
has_rec = any(n.startswith("dobject_recording/") for n in names)
|
||||
if not (has_log and has_rec):
|
||||
zf.close()
|
||||
raise FileNotFoundError(f"{path} is not a recovered/standard dlog zip")
|
||||
return DlogSource(label=str(path), zip_path=path, _zip=zf)
|
||||
|
||||
root = path
|
||||
if not ((root / "dobject").is_dir() and (root / "dobject_recording").is_dir()):
|
||||
child = root / "dlog"
|
||||
if (child / "dobject").is_dir() and (child / "dobject_recording").is_dir():
|
||||
root = child
|
||||
else:
|
||||
raise FileNotFoundError(f"{path} does not contain dobject and dobject_recording")
|
||||
return DlogSource(label=str(root), directory=root)
|
||||
|
||||
|
||||
def resolve_dlog_root(value: Path | str) -> Path:
|
||||
"""Backward-compatible helper: directory roots only (not zip)."""
|
||||
|
||||
source = open_dlog_source(value)
|
||||
try:
|
||||
if source.directory is None:
|
||||
raise FileNotFoundError(
|
||||
f"{value} is a zip; use open_dlog_source()/iter_payloads_from_source()"
|
||||
)
|
||||
return source.directory
|
||||
finally:
|
||||
source.close()
|
||||
|
||||
|
||||
def discover_records_from_source(
|
||||
source: DlogSource,
|
||||
object_name: str,
|
||||
*,
|
||||
host_ticks_min: int | None = None,
|
||||
host_ticks_max: int | None = None,
|
||||
) -> list[RecordRef]:
|
||||
pending: list[tuple[str, str, str, int, int, str, int, str]] = []
|
||||
name_key = object_name.casefold()
|
||||
for relative_log, text in source.iter_log_texts():
|
||||
for line in text.splitlines():
|
||||
match = RECORD_RE.search(line.strip())
|
||||
if not match or match.group("name").casefold() != name_key:
|
||||
continue
|
||||
ticks = int(match.group("tic"))
|
||||
if host_ticks_min is not None and ticks < host_ticks_min:
|
||||
continue
|
||||
if host_ticks_max is not None and ticks > host_ticks_max:
|
||||
continue
|
||||
pending.append(
|
||||
(
|
||||
match.group("name"),
|
||||
match.group("log_time") or "",
|
||||
relative_log,
|
||||
int(match.group("offset")),
|
||||
int(match.group("len")),
|
||||
match.group("id").upper(),
|
||||
ticks,
|
||||
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] = []
|
||||
@@ -84,10 +280,19 @@ def discover_records(dlog_root: Path, object_name: str) -> list[RecordRef]:
|
||||
return records
|
||||
|
||||
|
||||
def discover_records(dlog_root: Path, object_name: str) -> list[RecordRef]:
|
||||
with open_dlog_source(dlog_root) as source:
|
||||
return discover_records_from_source(source, object_name)
|
||||
|
||||
|
||||
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)
|
||||
recording = dlog_root / "dobject_recording"
|
||||
if not recording.is_dir():
|
||||
return result
|
||||
for path in recording.rglob("*"):
|
||||
if path.is_file() and path.suffix.lower() in {".dorec", ".bin"}:
|
||||
result.setdefault(path.name.casefold(), []).append(path)
|
||||
return result
|
||||
|
||||
|
||||
@@ -107,17 +312,15 @@ def read_exact(stream: BinaryIO, size: int) -> bytes:
|
||||
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)
|
||||
|
||||
def _read_payload_at(stream: BinaryIO, record: RecordRef) -> bytes:
|
||||
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:
|
||||
@@ -133,47 +336,46 @@ def read_record_payload(path: Path, record: RecordRef) -> bytes:
|
||||
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)
|
||||
def iter_payloads_from_source(
|
||||
source: DlogSource,
|
||||
object_name: str,
|
||||
*,
|
||||
host_ticks_min: int | None = None,
|
||||
host_ticks_max: int | None = None,
|
||||
) -> Iterator[tuple[RecordRef, bytes]]:
|
||||
records = discover_records_from_source(
|
||||
source,
|
||||
object_name,
|
||||
host_ticks_min=host_ticks_min,
|
||||
host_ticks_max=host_ticks_max,
|
||||
)
|
||||
if not records:
|
||||
return
|
||||
dorec_index = index_dorec_files(root)
|
||||
open_files: dict[str, tuple[Path, BinaryIO]] = {}
|
||||
open_files: dict[str, 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
|
||||
key = Path(record.source_dorec).name.casefold()
|
||||
stream = open_files.get(key)
|
||||
if stream is None:
|
||||
_owner, stream = source.open_recording(record.source_dorec)
|
||||
open_files[key] = stream
|
||||
yield record, _read_payload_at(stream, record)
|
||||
finally:
|
||||
for _path, stream in open_files.values():
|
||||
for stream in open_files.values():
|
||||
stream.close()
|
||||
|
||||
|
||||
def iter_payloads(
|
||||
dlog_root: Path | str,
|
||||
object_name: str,
|
||||
*,
|
||||
host_ticks_min: int | None = None,
|
||||
host_ticks_max: int | None = None,
|
||||
) -> Iterator[tuple[RecordRef, bytes]]:
|
||||
with open_dlog_source(dlog_root) as source:
|
||||
yield from iter_payloads_from_source(
|
||||
source,
|
||||
object_name,
|
||||
host_ticks_min=host_ticks_min,
|
||||
host_ticks_max=host_ticks_max,
|
||||
)
|
||||
|
||||
@@ -10,16 +10,21 @@ 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, iter_payloads, resolve_dlog_root
|
||||
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: Path
|
||||
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
|
||||
@@ -27,6 +32,8 @@ class H32DlogLidarSession:
|
||||
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(
|
||||
@@ -35,65 +42,99 @@ def load_h32_dlog_lidar(
|
||||
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:
|
||||
root = resolve_dlog_root(dlog_root)
|
||||
msop_packets: list[bytes] = []
|
||||
batch_count = 0
|
||||
session_id: str | None = None
|
||||
lidar_ip: str | None = None
|
||||
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
|
||||
|
||||
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)
|
||||
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
|
||||
|
||||
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
|
||||
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(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:
|
||||
if not msop_packets:
|
||||
msop_records = discover_records_from_source(source, msop_object)
|
||||
raise RuntimeError(
|
||||
f"no valid DIFOP calibration from DObject {difop_object!r} under {root}"
|
||||
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}])"
|
||||
)
|
||||
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_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,
|
||||
)
|
||||
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,
|
||||
)
|
||||
|
||||
@@ -0,0 +1,56 @@
|
||||
"""Wall-clock helpers for Medulla tick filtering.
|
||||
|
||||
Two tick conventions appear in this dataset:
|
||||
|
||||
- LiDAR DObject ``tic`` / recovered ``indices.log``: ``DateTime.Now.Ticks`` (local)
|
||||
- IMU / MSOP payload host receive fields: UTC ``DateTime.UtcNow.Ticks``
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from datetime import datetime, timedelta, timezone
|
||||
|
||||
TICKS_PER_SECOND = 10_000_000
|
||||
DOTNET_UNIX_EPOCH_TICKS = 621355968000000000
|
||||
|
||||
|
||||
def _parse_local_wall(text: str) -> datetime:
|
||||
normalized = text.strip().replace(" ", "T")
|
||||
if normalized.endswith("Z"):
|
||||
raise ValueError("expected local wall time without Z; got UTC marker")
|
||||
if "+" in normalized[10:]:
|
||||
idx = normalized.find("+", 10)
|
||||
normalized = normalized[:idx]
|
||||
elif normalized.count("-") > 2:
|
||||
# timezone like -08:00 after the date
|
||||
idx = normalized.find("-", 10)
|
||||
if idx > 0 and ":" in normalized[idx + 1 :]:
|
||||
normalized = normalized[:idx]
|
||||
return datetime.fromisoformat(normalized).replace(tzinfo=None)
|
||||
|
||||
|
||||
def local_wall_to_dotnet_ticks(text: str) -> int:
|
||||
"""Local wall time → ``DateTime.Now.Ticks`` (LiDAR DObject tic)."""
|
||||
|
||||
dt = _parse_local_wall(text)
|
||||
delta = dt - datetime(1, 1, 1)
|
||||
return int(delta.total_seconds() * TICKS_PER_SECOND)
|
||||
|
||||
|
||||
def local_wall_to_utc_dotnet_ticks(text: str, *, tz_hours: float = 8.0) -> int:
|
||||
"""Local wall time in ``tz_hours`` → UTC ``DateTime.UtcNow.Ticks`` (IMU host)."""
|
||||
|
||||
dt = _parse_local_wall(text).replace(tzinfo=timezone(timedelta(hours=tz_hours)))
|
||||
unix = dt.timestamp()
|
||||
return int(round(unix * TICKS_PER_SECOND)) + DOTNET_UNIX_EPOCH_TICKS
|
||||
|
||||
|
||||
def dotnet_ticks_to_local_iso(ticks: int) -> str:
|
||||
dt = datetime(1, 1, 1) + timedelta(microseconds=ticks / 10.0)
|
||||
return dt.isoformat(timespec="milliseconds")
|
||||
|
||||
|
||||
def utc_dotnet_ticks_to_unix_s(ticks: int) -> float:
|
||||
"""UTC ``DateTime.UtcNow.Ticks`` → Unix seconds."""
|
||||
|
||||
return (float(ticks) - float(DOTNET_UNIX_EPOCH_TICKS)) / float(TICKS_PER_SECOND)
|
||||
Reference in New Issue
Block a user