diff --git a/README.md b/README.md index 3c793ee..1f7623e 100644 --- a/README.md +++ b/README.md @@ -6,7 +6,7 @@ p_IMU = T_IMU_lidar · p_lidar ``` -**当前阶段:** 算法与合成自检已闭环;已提供 `tools/export_rscap_to_v1.py`(N300+H32 → 中间格式);**合格实车验收尚未完成**,故正式外参尚未对实车落盘交付。 +**当前阶段:** 算法与合成自检已闭环;已提供 `tools/export_rscap_to_v1.py`(N300 `.rscap` + H32 dlog/MSOP → 中间格式);**合格实车验收尚未完成**,故正式外参尚未对实车落盘交付。 --- @@ -16,7 +16,7 @@ p_IMU = T_IMU_lidar · p_lidar | 顺序 | 文档 | 用途 | | --- | -------------------------------------- | -------------------- | | 1 | **本 README** | 做什么、怎么跑、结果怎么判 | -| 2 | [docs/V1_数据格式.md](docs/V1_数据格式.md) | 中间格式 + `.rscap` 导出命令 | +| 2 | [docs/V1_数据格式.md](docs/V1_数据格式.md) | 中间格式 + 原始数据导出命令 | | 3 | [docs/标定流程与采集清单.md](docs/标定流程与采集清单.md) | 现场怎么采(合格数据要求) | @@ -44,13 +44,14 @@ p_IMU = T_IMU_lidar · p_lidar | `summary.json` | 状态、残差、可观性 | -新车原始数据导出: +新车原始数据导出(H32 dlog + N300 rscap): ```powershell python tools\export_rscap_to_v1.py ` --imu-rscap path\to\n300.rscap ` - --lidar-rscap path\to\h32_msop.rscap ` - --out path\to\session_v1 + --lidar-dlog path\to\session_or_dlog ` + --out path\to\session_v1 ` + --require-difop ``` --- diff --git a/docs/IMU-LiDAR标定.md b/docs/IMU-LiDAR标定.md index ef54514..9f527a6 100644 --- a/docs/IMU-LiDAR标定.md +++ b/docs/IMU-LiDAR标定.md @@ -178,7 +178,7 @@ python -m imu_lidar.cli run ` | 合成数据 pytest / 一键复现 | 已实现(证明链路与已知 yaw/δt,不证明实车精度) | | 旧车主机时间烟测(S2) | 线下可跑;预期 `blocked`,不当交付 | | 设备时间新车数据正式验收 | **待做** | -| 原生存储一键导出为中间格式 | 已提供 `tools/export_rscap_to_v1.py`(N300+H32 MSOP → V1;MSOP-only 用默认垂直角) | +| 原生存储一键导出为中间格式 | 已提供 `tools/export_rscap_to_v1.py`(N300 rscap + H32 dlog/MSOP → V1;dlog 用 DIFOP 通道角) | 细项见 [`imu_lidar/CHANGELOG.md`](../imu_lidar/CHANGELOG.md)。 diff --git a/docs/V1_数据格式.md b/docs/V1_数据格式.md index 5564c8a..2f2da4b 100644 --- a/docs/V1_数据格式.md +++ b/docs/V1_数据格式.md @@ -1,8 +1,25 @@ # V1 标准中间数据格式 -**用途:** 标定程序读入的 CSV/NPZ 约定,以及新车 `.rscap` 如何导出。总览见根目录 [README](../README.md)。 +**用途:** 标定程序读入的 CSV/NPZ 约定,以及新车原始数据如何导出。总览见根目录 [README](../README.md)。 -## 从原始 `.rscap` 导出 +## 从原始数据导出 + +**推荐(新 H32 插件 `RSLidarH32_3D_DLogCaptureNet48`):** N300 `.rscap` + 雷达 Medulla dlog(含 raw MSOP / DIFOP)。 + +```powershell +python tools\export_rscap_to_v1.py ` + --imu-rscap path\to\n300.rscap ` + --lidar-dlog path\to\session_or_dlog ` + --out path\to\session_v1 ` + --frame-stride 1 ` + --require-difop +``` + +`--lidar-dlog` 指向含 `dobject/` + `dobject_recording/` 的目录(或其上级含 `dlog/` 子目录亦可)。 +默认 DObject:`frontlidar-msop-raw`、`frontlidar-difop-raw`(可用 `--msop-object` / `--difop-object` 覆盖)。 +有 DIFOP 时用设备通道角做 XYZ;`--require-difop` 在缺少有效 DIFOP 时直接失败。 + +**兼容旧 MSOP-only `.rscap`:** ```powershell python tools\export_rscap_to_v1.py ` @@ -13,7 +30,7 @@ python tools\export_rscap_to_v1.py ` ``` 产出:`imu.csv`、`lidar/`(含 `frames_index.csv`)、`export_summary.json`。 -时间轴为**设备时间**:N300 `device_timestamp_us`→秒;H32 MSOP `device_timestamp_ms`→秒。主机接收时间不写入标定主轴。 +时间轴为**设备时间**:N300 `device_timestamp_us`→秒;H32 MSOP 设备时间戳→秒。主机接收时间不写入标定主轴。 ## IMU diff --git a/imu_lidar/CHANGELOG.md b/imu_lidar/CHANGELOG.md index 5ed034b..8ee4535 100644 --- a/imu_lidar/CHANGELOG.md +++ b/imu_lidar/CHANGELOG.md @@ -5,6 +5,20 @@ --- +## 2026-08-05 09:00 (UTC+8) + +### 导出:支持 H32 DLogCapture(MSOP+DIFOP)→ V1 + +- **原本**:导出只读 H32 MSOP V2 `.rscap`,无 DIFOP,垂直角用默认 −16°…+16°。 +- **改成**: + - 新增 `tools/h32_dlog/`(dobject 索引、MSOP/DIFOP payload V1、DIFOP 通道角)。 + - `export_rscap_to_v1.py` 增加 `--lidar-dlog`(与 `--lidar-rscap` 二选一);默认用 DIFOP 角做 XYZ。 + - `h32_msop.iter_h32_frames_from_packets` 供 dlog/rscap 共用拼帧。 + - 单测 `tests/test_h32_dlog_export.py`;文档改为推荐 dlog 导出命令。 +- **标定核心**(`imu_lidar/` 读 V1)未改。 + +--- + ## 2026-08-03 17:30 (UTC+8) ### 文档:精简对外阅读路径 diff --git a/imu_lidar/文件职责说明.md b/imu_lidar/文件职责说明.md index 10a1f75..5ff644a 100644 --- a/imu_lidar/文件职责说明.md +++ b/imu_lidar/文件职责说明.md @@ -69,7 +69,7 @@ lidar_session/ frames/frame_XXXXX.npz # points: (N,3) 米 ``` -原始 `.rscap` 用仓库工具导出:`python tools/export_rscap_to_v1.py ...`(见 [`docs/V1_数据格式.md`](../docs/V1_数据格式.md))。 +原始 N300 `.rscap` + H32 dlog(或旧 MSOP `.rscap`)用仓库工具导出:`python tools/export_rscap_to_v1.py ...`(见 [`docs/V1_数据格式.md`](../docs/V1_数据格式.md))。 --- diff --git a/tests/test_h32_dlog_export.py b/tests/test_h32_dlog_export.py new file mode 100644 index 0000000..fd031d1 --- /dev/null +++ b/tests/test_h32_dlog_export.py @@ -0,0 +1,198 @@ +"""Unit tests for H32 Medulla dlog → V1 export helpers.""" + +from __future__ import annotations + +import struct +from pathlib import Path + +import numpy as np + +from tools.h32_dlog.difop import CHANNELS, HORIZONTAL_START, VERTICAL_START, parse_difop_angles +from tools.h32_dlog.dobject import discover_records, iter_payloads, resolve_dlog_root +from tools.h32_dlog.load_session import load_h32_dlog_lidar +from tools.h32_dlog.payload_v1 import ( + MsopPacketItem, + build_difop_payload, + build_msop_batch_payload, + parse_difop_payload, + parse_msop_batch_payload, +) +from tools.rscap_v2.h32_msop import PACKET_LENGTH, iter_h32_frames_from_packets + + +def _make_msop_packet(*, seconds: int = 100, microseconds: int = 5000, az_deg: float = 10.0) -> bytes: + packet = bytearray(PACKET_LENGTH) + packet[17] = 1 + packet[20:26] = int(seconds).to_bytes(6, "big") + packet[26:30] = int(microseconds).to_bytes(4, "big") + az_raw = int(round(az_deg * 100)) + for block in range(12): + offset = 42 + block * 100 + packet[offset] = 255 + packet[offset + 1] = 238 + packet[offset + 2] = (az_raw >> 8) & 0xFF + packet[offset + 3] = az_raw & 0xFF + idx = offset + 4 + for _ch in range(CHANNELS): + packet[idx] = (1600 >> 8) & 0xFF + packet[idx + 1] = 1600 & 0xFF + packet[idx + 2] = 10 + idx += 3 + return bytes(packet) + + +def _write_signed_angle(buf: bytearray, index: int, degrees: float) -> None: + sign = 1 if degrees < 0 else 0 + raw = int(round(abs(degrees) * 100)) + buf[index] = sign + buf[index + 1] = (raw >> 8) & 0xFF + buf[index + 2] = raw & 0xFF + + +def _make_difop_packet(*, vertical: list[float], horizontal: list[float] | None = None) -> bytes: + packet = bytearray(1248) + horiz = horizontal if horizontal is not None else [0.0] * CHANNELS + for channel, angle in enumerate(vertical): + _write_signed_angle(packet, VERTICAL_START + channel * 3, angle) + for channel, angle in enumerate(horiz): + _write_signed_angle(packet, HORIZONTAL_START + channel * 3, angle) + return bytes(packet) + + +def _write_dorec_record( + path: Path, + *, + object_name: str, + ticks: int, + record_id: str, + payload: bytes, + offset: int = 0, +) -> int: + """Append one DObject record; return file offset of the record start.""" + + path.parent.mkdir(parents=True, exist_ok=True) + name_b = object_name.encode("ascii") + id_b = record_id.encode("ascii") + blob = ( + bytes([len(name_b)]) + + name_b + + struct.pack(" 0 diff --git a/tools/export_rscap_to_v1.py b/tools/export_rscap_to_v1.py index 223b7c7..85af4cb 100644 --- a/tools/export_rscap_to_v1.py +++ b/tools/export_rscap_to_v1.py @@ -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: diff --git a/tools/h32_dlog/__init__.py b/tools/h32_dlog/__init__.py new file mode 100644 index 0000000..a743f7e --- /dev/null +++ b/tools/h32_dlog/__init__.py @@ -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", +] diff --git a/tools/h32_dlog/difop.py b/tools/h32_dlog/difop.py new file mode 100644 index 0000000..9f831fb --- /dev/null +++ b/tools/h32_dlog/difop.py @@ -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) diff --git a/tools/h32_dlog/dobject.py b/tools/h32_dlog/dobject.py new file mode 100644 index 0000000..82416a0 --- /dev/null +++ b/tools/h32_dlog/dobject.py @@ -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[^]]+)\].*?DObject `(?P[^`]+)` post " + r"len=(?P\d+)B, id:(?P[0-9A-Fa-f]+), tic:(?P\d+), " + r"@(?P[^:]+):(?P\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(" 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(" 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(" int: + raw = stream.read(8) + if len(raw) != 8: + raise EOFError("truncated int64") + return struct.unpack(" 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") diff --git a/tools/h32_dlog/load_session.py b/tools/h32_dlog/load_session.py new file mode 100644 index 0000000..f4c6610 --- /dev/null +++ b/tools/h32_dlog/load_session.py @@ -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, + ) diff --git a/tools/h32_dlog/payload_v1.py b/tools/h32_dlog/payload_v1.py new file mode 100644 index 0000000..0fdd85d --- /dev/null +++ b/tools/h32_dlog/payload_v1.py @@ -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(" 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(" 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, + )