支持 H32 DLogCapture（MSOP+DIFOP）导出到 V1 中间格式
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
lichun.qu
Cursor
+114
-24
@@ -1,5 +1,11 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Export N300 IMU + H32 MSOP V2 .rscap files to Lidar-IMU V1 intermediate format.
|
||||
"""Export N300 IMU + H32 LiDAR captures to Lidar-IMU V1 intermediate format.
|
||||
|
||||
Supported LiDAR sources (exactly one required):
|
||||
|
||||
- ``--lidar-dlog``: Medulla dlog from ``RSLidarH32_3D_DLogCaptureNet48``
|
||||
(raw MSOP + DIFOP DObjects; preferred for new recordings)
|
||||
- ``--lidar-rscap``: legacy H32 MSOP V2 ``.rscap`` (MSOP-only defaults for angles)
|
||||
|
||||
Output layout under --out:
|
||||
|
||||
@@ -27,8 +33,9 @@ ROOT = Path(__file__).resolve().parents[1]
|
||||
if str(ROOT) not in sys.path:
|
||||
sys.path.insert(0, str(ROOT))
|
||||
|
||||
from tools.h32_dlog.load_session import load_h32_dlog_lidar
|
||||
from tools.rscap_v2.capture_format_v2 import file_summary, read_capture
|
||||
from tools.rscap_v2.h32_msop import iter_h32_frames
|
||||
from tools.rscap_v2.h32_msop import iter_h32_frames, iter_h32_frames_from_packets
|
||||
from tools.rscap_v2.n300_imu import iter_n300_imu_samples, samples_to_arrays
|
||||
|
||||
|
||||
@@ -84,41 +91,88 @@ def write_lidar_session(root: Path, frames) -> dict:
|
||||
def export_session(
|
||||
*,
|
||||
imu_rscap: Path,
|
||||
lidar_rscap: Path,
|
||||
out: Path,
|
||||
lidar_rscap: Path | None = None,
|
||||
lidar_dlog: Path | None = None,
|
||||
msop_object: str = "frontlidar-msop-raw",
|
||||
difop_object: str = "frontlidar-difop-raw",
|
||||
require_difop: bool = False,
|
||||
frame_stride: int = 1,
|
||||
max_points_per_frame: int | None = 80000,
|
||||
min_range_m: float = 0.3,
|
||||
max_range_m: float = 120.0,
|
||||
min_frame_points: int = 100,
|
||||
) -> dict:
|
||||
if (lidar_rscap is None) == (lidar_dlog is None):
|
||||
raise ValueError("provide exactly one of lidar_rscap or lidar_dlog")
|
||||
|
||||
out.mkdir(parents=True, exist_ok=True)
|
||||
imu_capture = read_capture(imu_rscap)
|
||||
lidar_capture = read_capture(lidar_rscap)
|
||||
|
||||
samples = iter_n300_imu_samples(imu_capture)
|
||||
t, gyro, accel = samples_to_arrays(samples)
|
||||
imu_csv = out / "imu.csv"
|
||||
write_imu_csv(imu_csv, t, gyro, accel)
|
||||
|
||||
frames = iter_h32_frames(
|
||||
lidar_capture,
|
||||
min_frame_points=min_frame_points,
|
||||
frame_stride=frame_stride,
|
||||
min_range_m=min_range_m,
|
||||
max_range_m=max_range_m,
|
||||
max_points_per_frame=max_points_per_frame,
|
||||
)
|
||||
lidar_meta: dict
|
||||
if lidar_dlog is not None:
|
||||
session = load_h32_dlog_lidar(
|
||||
lidar_dlog,
|
||||
msop_object=msop_object,
|
||||
difop_object=difop_object,
|
||||
require_difop=require_difop,
|
||||
)
|
||||
frames = iter_h32_frames_from_packets(
|
||||
session.msop_packets,
|
||||
min_frame_points=min_frame_points,
|
||||
frame_stride=frame_stride,
|
||||
min_range_m=min_range_m,
|
||||
max_range_m=max_range_m,
|
||||
max_points_per_frame=max_points_per_frame,
|
||||
vertical_deg=session.vertical_deg,
|
||||
horizontal_deg=session.horizontal_deg,
|
||||
)
|
||||
lidar_meta = {
|
||||
"source": "dlog",
|
||||
"lidar_dlog": str(session.dlog_root),
|
||||
"msop_object": session.msop_object,
|
||||
"difop_object": session.difop_object,
|
||||
"msop_packets": len(session.msop_packets),
|
||||
"msop_batches": session.msop_batch_count,
|
||||
"difop_records": session.difop_record_count,
|
||||
"session_id": session.session_id,
|
||||
"lidar_ip": session.lidar_ip,
|
||||
"angle_source": session.angle_source,
|
||||
"timestamp_note": "h32_msop_device_timestamp -> seconds (from MSOP bytes)",
|
||||
}
|
||||
else:
|
||||
assert lidar_rscap is not None
|
||||
lidar_capture = read_capture(lidar_rscap)
|
||||
frames = iter_h32_frames(
|
||||
lidar_capture,
|
||||
min_frame_points=min_frame_points,
|
||||
frame_stride=frame_stride,
|
||||
min_range_m=min_range_m,
|
||||
max_range_m=max_range_m,
|
||||
max_points_per_frame=max_points_per_frame,
|
||||
)
|
||||
lidar_meta = {
|
||||
"source": "rscap_v2",
|
||||
"lidar_rscap": str(lidar_rscap),
|
||||
"capture": file_summary(lidar_capture),
|
||||
"angle_source": "default_msop_only_vertical_-16_to_16_deg",
|
||||
"timestamp_note": "h32_msop_device_timestamp_ms -> seconds",
|
||||
}
|
||||
|
||||
lidar_dir = out / "lidar"
|
||||
lidar_stats = write_lidar_session(lidar_dir, frames)
|
||||
|
||||
summary = {
|
||||
"imu_rscap": str(imu_rscap),
|
||||
"lidar_rscap": str(lidar_rscap),
|
||||
"out": str(out),
|
||||
"timestamp_policy": {
|
||||
"imu": "n300_device_timestamp_us -> seconds",
|
||||
"lidar": "h32_msop_device_timestamp_ms -> seconds (t_start/t_end per frame)",
|
||||
"lidar": lidar_meta["timestamp_note"],
|
||||
"host_utc": "not used as calibration timeline",
|
||||
},
|
||||
"imu": {
|
||||
@@ -131,8 +185,7 @@ def export_session(
|
||||
**lidar_stats,
|
||||
"frame_stride": int(frame_stride),
|
||||
"max_points_per_frame": max_points_per_frame,
|
||||
"capture": file_summary(lidar_capture),
|
||||
"angle_source": "default_msop_only_vertical_-16_to_16_deg",
|
||||
**lidar_meta,
|
||||
},
|
||||
"outputs": {
|
||||
"imu_csv": str(imu_csv),
|
||||
@@ -149,7 +202,32 @@ def export_session(
|
||||
def main() -> int:
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument("--imu-rscap", type=Path, required=True, help="N300 V2 .rscap")
|
||||
parser.add_argument("--lidar-rscap", type=Path, required=True, help="H32 MSOP V2 .rscap")
|
||||
lidar = parser.add_mutually_exclusive_group(required=True)
|
||||
lidar.add_argument(
|
||||
"--lidar-dlog",
|
||||
type=Path,
|
||||
help="H32 Medulla dlog root (dobject/ + dobject_recording/), preferred",
|
||||
)
|
||||
lidar.add_argument(
|
||||
"--lidar-rscap",
|
||||
type=Path,
|
||||
help="Legacy H32 MSOP V2 .rscap (no DIFOP; default vertical angles)",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--msop-object",
|
||||
default="frontlidar-msop-raw",
|
||||
help="DObject name for raw MSOP batches (dlog path)",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--difop-object",
|
||||
default="frontlidar-difop-raw",
|
||||
help="DObject name for raw DIFOP packets (dlog path)",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--require-difop",
|
||||
action="store_true",
|
||||
help="Fail if dlog has no valid DIFOP channel angles",
|
||||
)
|
||||
parser.add_argument("--out", type=Path, required=True, help="Output session directory")
|
||||
parser.add_argument("--frame-stride", type=int, default=1, help="Keep every N-th LiDAR frame")
|
||||
parser.add_argument(
|
||||
@@ -166,6 +244,10 @@ def main() -> int:
|
||||
summary = export_session(
|
||||
imu_rscap=args.imu_rscap,
|
||||
lidar_rscap=args.lidar_rscap,
|
||||
lidar_dlog=args.lidar_dlog,
|
||||
msop_object=args.msop_object,
|
||||
difop_object=args.difop_object,
|
||||
require_difop=args.require_difop,
|
||||
out=args.out,
|
||||
frame_stride=args.frame_stride,
|
||||
max_points_per_frame=max_points,
|
||||
@@ -173,13 +255,21 @@ def main() -> int:
|
||||
max_range_m=args.max_range_m,
|
||||
min_frame_points=args.min_frame_points,
|
||||
)
|
||||
print(json.dumps({
|
||||
"imu_samples": summary["imu"]["samples"],
|
||||
"lidar_frames": summary["lidar"]["frames"],
|
||||
"imu_csv": summary["outputs"]["imu_csv"],
|
||||
"lidar_session": summary["outputs"]["lidar_session"],
|
||||
"export_summary": str(Path(args.out) / "export_summary.json"),
|
||||
}, ensure_ascii=False, indent=2))
|
||||
print(
|
||||
json.dumps(
|
||||
{
|
||||
"imu_samples": summary["imu"]["samples"],
|
||||
"lidar_frames": summary["lidar"]["frames"],
|
||||
"lidar_source": summary["lidar"]["source"],
|
||||
"angle_source": summary["lidar"]["angle_source"],
|
||||
"imu_csv": summary["outputs"]["imu_csv"],
|
||||
"lidar_session": summary["outputs"]["lidar_session"],
|
||||
"export_summary": str(Path(args.out) / "export_summary.json"),
|
||||
},
|
||||
ensure_ascii=False,
|
||||
indent=2,
|
||||
)
|
||||
)
|
||||
if summary["imu"]["samples"] == 0:
|
||||
raise SystemExit("no valid N300 IMU samples decoded")
|
||||
if summary["lidar"]["frames"] == 0:
|
||||
|
||||
@@ -0,0 +1,14 @@
|
||||
"""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 .payload_v1 import parse_difop_payload, parse_msop_batch_payload
|
||||
|
||||
__all__ = [
|
||||
"discover_records",
|
||||
"iter_payloads",
|
||||
"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,99 @@
|
||||
"""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, 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_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] = []
|
||||
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)
|
||||
|
||||
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_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()
|
||||
@@ -1,6 +1,6 @@
|
||||
"""Decode RoboSense H32 MSOP V2 .rscap into Cartesian frames (metres).
|
||||
"""Decode RoboSense H32 MSOP packets into Cartesian frames (metres).
|
||||
|
||||
Angle / distance conventions follow ``RSLidarH32_3D_RawCaptureNet48``:
|
||||
Angle / distance conventions follow the H32 Medulla plugins:
|
||||
azimuth = normalize(-(block_az + horizontal[ch])), altitude = vertical[ch],
|
||||
distance_mm = raw * distance_unit_mm, then:
|
||||
|
||||
@@ -8,13 +8,14 @@ distance_mm = raw * distance_unit_mm, then:
|
||||
y = d_m * cos(alt) * sin(az)
|
||||
z = d_m * sin(alt)
|
||||
|
||||
MSOP-only captures do not include DIFOP; vertical angles default to a uniform
|
||||
-16°…+16° fan, horizontal channel offsets default to 0.
|
||||
When DIFOP is unavailable, vertical angles default to a uniform -16°…+16° fan
|
||||
and horizontal channel offsets default to 0.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
from typing import Iterable
|
||||
|
||||
import numpy as np
|
||||
|
||||
@@ -140,8 +141,8 @@ def _block_points(
|
||||
return np.column_stack([xs, ys, zs]).astype(np.float64, copy=False)
|
||||
|
||||
|
||||
def iter_h32_frames(
|
||||
capture: CaptureFile,
|
||||
def iter_h32_frames_from_packets(
|
||||
packets: Iterable[bytes],
|
||||
*,
|
||||
min_frame_points: int = MIN_FRAME_POINTS_DEFAULT,
|
||||
frame_stride: int = 1,
|
||||
@@ -151,7 +152,7 @@ def iter_h32_frames(
|
||||
vertical_deg: np.ndarray | None = None,
|
||||
horizontal_deg: np.ndarray | None = None,
|
||||
) -> list[LidarFrameExport]:
|
||||
"""Assemble MSOP packets into frames using the 270°→90° azimuth wrap."""
|
||||
"""Assemble raw MSOP packets into frames using the 270°→90° azimuth wrap."""
|
||||
|
||||
vertical = default_vertical_deg() if vertical_deg is None else np.asarray(vertical_deg, dtype=np.float64)
|
||||
horizontal = default_horizontal_deg() if horizontal_deg is None else np.asarray(horizontal_deg, dtype=np.float64)
|
||||
@@ -189,8 +190,7 @@ def iter_h32_frames(
|
||||
end_s = start_s + 0.1
|
||||
frames.append(LidarFrameExport(t_start_s=start_s, t_end_s=end_s, points_xyz=points))
|
||||
|
||||
for chunk in capture.chunks:
|
||||
packet = chunk.raw
|
||||
for packet in packets:
|
||||
if len(packet) != PACKET_LENGTH:
|
||||
continue
|
||||
packet_t = device_timestamp_ms(packet) * 1e-3
|
||||
@@ -222,3 +222,28 @@ def iter_h32_frames(
|
||||
|
||||
emit()
|
||||
return frames
|
||||
|
||||
|
||||
def iter_h32_frames(
|
||||
capture: CaptureFile,
|
||||
*,
|
||||
min_frame_points: int = MIN_FRAME_POINTS_DEFAULT,
|
||||
frame_stride: int = 1,
|
||||
min_range_m: float = 0.3,
|
||||
max_range_m: float = 120.0,
|
||||
max_points_per_frame: int | None = None,
|
||||
vertical_deg: np.ndarray | None = None,
|
||||
horizontal_deg: np.ndarray | None = None,
|
||||
) -> list[LidarFrameExport]:
|
||||
"""Assemble MSOP packets from a V2 .rscap capture into frames."""
|
||||
|
||||
return iter_h32_frames_from_packets(
|
||||
(chunk.raw for chunk in capture.chunks),
|
||||
min_frame_points=min_frame_points,
|
||||
frame_stride=frame_stride,
|
||||
min_range_m=min_range_m,
|
||||
max_range_m=max_range_m,
|
||||
max_points_per_frame=max_points_per_frame,
|
||||
vertical_deg=vertical_deg,
|
||||
horizontal_deg=horizontal_deg,
|
||||
)
|
||||
|
||||
Reference in New Issue
Block a user