支持 H32 DLogCapture(MSOP+DIFOP)站导出到 combined

Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
lichun.qu
2026-08-05 10:17:23 +08:00
co-authored by Cursor
parent b2271d05ba
commit 69bb44bccd
15 changed files with 1168 additions and 105 deletions
+3 -2
View File
@@ -2,9 +2,10 @@
| 文件 | 输入→输出 |
|---|---|
| **`export_raw_to_combined.py`** | **一步导出**:逐站 H32 + 全程 G90/N300 `.rscap``combined/`(标定直接入口,对标 Lidar-IMU `export_rscap_to_v1` |
| **`export_raw_to_combined.py`** | **一步导出**:逐站 H32dlog MSOP+DIFOP 或旧 `.rscap`+ 全程 G90/N300 `.rscap``combined/` |
| `export_h32_rscap_station.py` | 内部零件:单站 H32 → 雷达帧 NPZ(一般不必单独跑) |
| `frontlidar_dlog_export.py` | **旧数据** LiDAR dlog → 逐帧 NPZ;由一步导出在遇到 dlog 站时自动调用 |
| `h32_dlog/` | 新 H32 DLogCapturedobject 索引、MSOP/DIFOP payload、DIFOP 通道角 |
| `frontlidar_dlog_export.py` | **旧数据** 已解码点云 dlog → 逐帧 NPZ;无 raw MSOP 时由一步导出回退调用 |
| `rscap_v2/parse_rtk_imu_v2.py` | 单独解析 RTK/IMU(调试用);一步导出已内嵌同等逻辑 |
| `rscap_v2/h32_msop.py` | H32 MSOP 解码(XYZ / 极坐标 `points_raw` |
| `rscap_v2/n300_imu.py` | N300 FDILink 采样解码 |
+215 -49
View File
@@ -1,5 +1,10 @@
#!/usr/bin/env python3
"""Export one static-station H32 V2 .rscap into LiDAR frame NPZs.
"""Export one static-station H32 capture into LiDAR frame NPZs.
Supports:
- V2 ``.rscap`` (legacy MSOP-only RawCapture)
- Medulla dlog from ``RSLidarH32_3D_DLogCaptureNet48`` (raw MSOP + DIFOP)
This is an **internal** helper used by ``export_raw_to_combined.py``.
For RTKLiDAR calibration, prefer the one-shot exporter that writes ``combined/``.
@@ -10,7 +15,7 @@ Output frame contract (consumed by ``build_multisensor_npz.py``):
- ``unix_time_ns``: H32 MSOP device timestamp (seconds+us → ns)
- ``frame_counter``, ``point_count``, optional host receive stamp
Raw ``.rscap`` files are never modified.
Raw ``.rscap`` / dlog files are never modified.
"""
from __future__ import annotations
@@ -25,10 +30,16 @@ from typing import Any
import numpy as np
ROOT = Path(__file__).resolve().parent
sys.path.insert(0, str(ROOT))
sys.path.insert(0, str(ROOT / "rscap_v2"))
from capture_format_v2 import file_summary, read_capture # noqa: E402
from h32_msop import iter_h32_frames_polar # noqa: E402
from h32_dlog.dobject import discover_records, resolve_dlog_root # noqa: E402
from h32_dlog.load_session import load_h32_dlog_lidar # noqa: E402
from h32_msop import ( # noqa: E402
iter_h32_frames_polar,
iter_h32_frames_polar_from_packets,
)
def resolve_lidar_rscap(station_dir: Path, capture_name: str = "h32.rscap") -> Path:
@@ -45,36 +56,47 @@ def resolve_lidar_rscap(station_dir: Path, capture_name: str = "h32.rscap") -> P
)
def export_station_h32(
station: Path,
out: Path,
*,
capture_name: str = "h32.rscap",
stride: int = 1,
min_frame_points: int = 100,
min_range_m: float = 0.3,
max_range_m: float = 120.0,
compress: bool = True,
write_reports: bool = False,
resume: bool = False,
) -> dict[str, Any]:
"""Decode one station H32 capture into ``out/frames/*.npz``. Returns metadata."""
def try_resolve_dlog_root(station_dir: Path) -> Path | None:
try:
return resolve_dlog_root(station_dir)
except FileNotFoundError:
return None
rscap = station if station.is_file() and station.suffix.lower() == ".rscap" else resolve_lidar_rscap(station, capture_name)
def is_h32_raw_dlog_station(
station_dir: Path,
*,
msop_object: str = "frontlidar-msop-raw",
) -> bool:
root = try_resolve_dlog_root(station_dir)
if root is None:
return False
return len(discover_records(root, msop_object)) > 0
def is_legacy_pointcloud_dlog_station(
station_dir: Path,
*,
msop_object: str = "frontlidar-msop-raw",
) -> bool:
root = try_resolve_dlog_root(station_dir)
if root is None:
return False
return not is_h32_raw_dlog_station(station_dir, msop_object=msop_object)
def _write_polar_frames(
*,
out: Path,
frames,
source_label: str,
compress: bool,
write_reports: bool,
resume: bool,
metadata_extra: dict[str, Any],
) -> dict[str, Any]:
frames_dir = out / "frames"
frames_dir.mkdir(parents=True, exist_ok=True)
capture = read_capture(rscap)
frames = iter_h32_frames_polar(
capture,
min_frame_points=min_frame_points,
frame_stride=max(1, stride),
min_range_m=min_range_m,
max_range_m=max_range_m,
)
if not frames:
raise RuntimeError(f"no H32 frames decoded from {rscap}")
saver = np.savez_compressed if compress else np.savez
manifest_rows: list[dict[str, Any]] = []
written = 0
@@ -93,7 +115,7 @@ def export_station_h32(
"device_time_s": np.asarray([frame.t_start_s], dtype=np.float64),
"device_time_end_s": np.asarray([frame.t_end_s], dtype=np.float64),
"host_receive_utc_ns": np.asarray([frame.host_receive_utc_ns], dtype=np.int64),
"source_file_utf8": np.frombuffer(str(rscap.resolve()).encode("utf-8"), dtype=np.uint8),
"source_file_utf8": np.frombuffer(source_label.encode("utf-8"), dtype=np.uint8),
}
saver(destination, **payload)
written += 1
@@ -108,18 +130,17 @@ def export_station_h32(
)
metadata: dict[str, Any] = {
"source_rscap": str(rscap.resolve()),
"capture": file_summary(capture),
"frames_decoded": len(frames),
"frames_written": written,
"frames_dir": str(frames_dir.resolve()),
"time_basis": "H32 MSOP device timestamp (packet seconds+microseconds)",
"points_raw_columns": ["d_mm", "azimuth_deg", "altitude_deg", "intensity", "progression"],
**metadata_extra,
}
(out / "metadata.json").write_text(json.dumps(metadata, ensure_ascii=False, indent=2), encoding="utf-8")
(out / "README.md").write_text(
"# H32 station export (internal)\n\n"
f"- source: `{rscap}`\n"
f"- source: `{source_label}`\n"
f"- frames: `{frames_dir}`\n"
"- Prefer ``tools/export_raw_to_combined.py`` for the full RTKLiDAR package.\n",
encoding="utf-8",
@@ -128,18 +149,133 @@ def export_station_h32(
reports = out / "reports"
reports.mkdir(parents=True, exist_ok=True)
with (reports / "manifest.csv").open("w", encoding="utf-8", newline="") as stream:
writer = csv.DictWriter(stream, fieldnames=list(manifest_rows[0].keys()) if manifest_rows else ["index"])
writer = csv.DictWriter(
stream, fieldnames=list(manifest_rows[0].keys()) if manifest_rows else ["index"]
)
writer.writeheader()
writer.writerows(manifest_rows)
(reports / "export_summary.json").write_text(json.dumps(metadata, ensure_ascii=False, indent=2), encoding="utf-8")
(reports / "export_summary.json").write_text(
json.dumps(metadata, ensure_ascii=False, indent=2), encoding="utf-8"
)
return metadata
def export_station_h32(
station: Path,
out: Path,
*,
capture_name: str = "h32.rscap",
stride: int = 1,
min_frame_points: int = 100,
min_range_m: float = 0.3,
max_range_m: float = 120.0,
compress: bool = True,
write_reports: bool = False,
resume: bool = False,
) -> dict[str, Any]:
"""Decode one station H32 ``.rscap`` into ``out/frames/*.npz``."""
rscap = (
station
if station.is_file() and station.suffix.lower() == ".rscap"
else resolve_lidar_rscap(station, capture_name)
)
capture = read_capture(rscap)
frames = iter_h32_frames_polar(
capture,
min_frame_points=min_frame_points,
frame_stride=max(1, stride),
min_range_m=min_range_m,
max_range_m=max_range_m,
)
if not frames:
raise RuntimeError(f"no H32 frames decoded from {rscap}")
return _write_polar_frames(
out=out,
frames=frames,
source_label=str(rscap.resolve()),
compress=compress,
write_reports=write_reports,
resume=resume,
metadata_extra={
"kind": "h32_rscap",
"source_rscap": str(rscap.resolve()),
"capture": file_summary(capture),
"angle_source": "default_msop_only_vertical_-16_to_16_deg",
},
)
def export_station_h32_dlog(
station: Path,
out: Path,
*,
msop_object: str = "frontlidar-msop-raw",
difop_object: str = "frontlidar-difop-raw",
require_difop: bool = True,
stride: int = 1,
min_frame_points: int = 100,
min_range_m: float = 0.3,
max_range_m: float = 120.0,
compress: bool = True,
write_reports: bool = False,
resume: bool = False,
) -> dict[str, Any]:
"""Decode one station H32 raw-MSOP/DIFOP dlog into ``out/frames/*.npz``."""
session = load_h32_dlog_lidar(
station,
msop_object=msop_object,
difop_object=difop_object,
require_difop=require_difop,
)
frames = iter_h32_frames_polar_from_packets(
session.msop_packets,
host_utc_ticks=session.msop_host_utc_ticks,
min_frame_points=min_frame_points,
frame_stride=max(1, stride),
min_range_m=min_range_m,
max_range_m=max_range_m,
vertical_deg=session.vertical_deg,
horizontal_deg=session.horizontal_deg,
)
if not frames:
raise RuntimeError(f"no H32 frames decoded from dlog {session.dlog_root}")
return _write_polar_frames(
out=out,
frames=frames,
source_label=str(session.dlog_root.resolve()),
compress=compress,
write_reports=write_reports,
resume=resume,
metadata_extra={
"kind": "h32_dlog_raw",
"source_dlog": str(session.dlog_root.resolve()),
"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,
},
)
def parse_args() -> argparse.Namespace:
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument("--station", type=Path, required=True, help="Station directory or .rscap file")
parser.add_argument("--station", type=Path, required=True, help="Station directory, .rscap, or dlog root")
parser.add_argument("--out", type=Path, required=True)
parser.add_argument("--capture-name", default="h32.rscap")
parser.add_argument("--msop-object", default="frontlidar-msop-raw")
parser.add_argument("--difop-object", default="frontlidar-difop-raw")
parser.add_argument(
"--require-difop",
action=argparse.BooleanOptionalAction,
default=True,
help="For dlog stations, require valid DIFOP angles (default: true)",
)
parser.add_argument("--stride", type=int, default=1)
parser.add_argument("--min-frame-points", type=int, default=100)
parser.add_argument("--min-range-m", type=float, default=0.3)
@@ -152,18 +288,48 @@ def parse_args() -> argparse.Namespace:
def main() -> int:
args = parse_args()
metadata = export_station_h32(
args.station,
args.out,
capture_name=args.capture_name,
stride=args.stride,
min_frame_points=args.min_frame_points,
min_range_m=args.min_range_m,
max_range_m=args.max_range_m,
compress=args.compress,
write_reports=args.write_reports,
resume=args.resume,
)
station = args.station
if station.is_file() and station.suffix.lower() == ".rscap":
metadata = export_station_h32(
station,
args.out,
capture_name=args.capture_name,
stride=args.stride,
min_frame_points=args.min_frame_points,
min_range_m=args.min_range_m,
max_range_m=args.max_range_m,
compress=args.compress,
write_reports=args.write_reports,
resume=args.resume,
)
elif is_h32_raw_dlog_station(station, msop_object=args.msop_object):
metadata = export_station_h32_dlog(
station,
args.out,
msop_object=args.msop_object,
difop_object=args.difop_object,
require_difop=args.require_difop,
stride=args.stride,
min_frame_points=args.min_frame_points,
min_range_m=args.min_range_m,
max_range_m=args.max_range_m,
compress=args.compress,
write_reports=args.write_reports,
resume=args.resume,
)
else:
metadata = export_station_h32(
station,
args.out,
capture_name=args.capture_name,
stride=args.stride,
min_frame_points=args.min_frame_points,
min_range_m=args.min_range_m,
max_range_m=args.max_range_m,
compress=args.compress,
write_reports=args.write_reports,
resume=args.resume,
)
print(json.dumps(metadata, ensure_ascii=False, indent=2))
return 0
+80 -29
View File
@@ -1,19 +1,22 @@
#!/usr/bin/env python3
"""One-shot export: raw H32/G90/N300 captures → RTKLiDAR ``combined/`` package.
Analogous to Lidar-IMU ``tools/export_rscap_to_v1.py``: raw ``.rscap`` in,
Analogous to Lidar-IMU ``tools/export_rscap_to_v1.py``: raw captures in,
calibration-ready intermediate out. Downstream prepare/solve consume ``combined/``
only (``manifest.csv`` + associated frame NPZs).
Expected raw layout:
Expected raw layout (new H32 DLogCapture):
stations/
001/h32.rscap
002/h32.rscap
001/ # dobject/ + dobject_recording/ (or 001/dlog/...)
002/
...
captures/ (paths passed explicitly)
rtk.rscap # G90: #PVTSLNA + #UNIHEADINGA
imu.rscap # N300 (associated only; not used in AX=XB)
captures/
rtk.rscap # G90: #PVTSLNA + #UNIHEADINGA
imu.rscap # N300 (associated only; not used in AX=XB)
Also accepts legacy per-station ``h32.rscap``, and older decoded-point-cloud dlog
stations (prefer ``--time-basis host`` for those).
Output under ``--out``:
@@ -22,9 +25,6 @@ Output under ``--out``:
combined/frames/*.npz + manifest.csv + dataset_summary.json
export_summary.json
Legacy dlog stations (``dobject`` + ``dobject_recording``) are still accepted;
use ``--time-basis host`` for those datasets.
Raw ``.rscap`` / dlog files are never modified.
"""
@@ -45,7 +45,14 @@ sys.path.insert(0, str(ROOT / "rscap_v2"))
from build_multisensor_npz import build_combined # noqa: E402
from capture_format_v2 import file_summary, read_capture # noqa: E402
from export_h32_rscap_station import export_station_h32, resolve_lidar_rscap # noqa: E402
from export_h32_rscap_station import ( # noqa: E402
export_station_h32,
export_station_h32_dlog,
is_h32_raw_dlog_station,
is_legacy_pointcloud_dlog_station,
resolve_lidar_rscap,
try_resolve_dlog_root,
)
from pipeline_common_corrected import ( # noqa: E402
parse_imu_capture,
parse_rtk_capture,
@@ -57,16 +64,21 @@ from pipeline_common_corrected import ( # noqa: E402
def is_h32_station(station: Path, capture_name: str) -> bool:
try:
resolve_lidar_rscap(station, capture_name)
return True
except FileNotFoundError:
return False
return True
def is_dlog_station(station: Path) -> bool:
return (station / "dobject").is_dir() and (station / "dobject_recording").is_dir()
return try_resolve_dlog_root(station) is not None
def discover_stations(stations_root: Path, names: list[str], capture_name: str) -> list[Path]:
def discover_stations(
stations_root: Path,
names: list[str],
capture_name: str,
msop_object: str,
) -> list[Path]:
if names:
stations = [stations_root / name for name in names]
missing = [str(path) for path in stations if not path.is_dir()]
@@ -77,13 +89,19 @@ def discover_stations(stations_root: Path, names: list[str], capture_name: str)
[
path
for path in stations_root.iterdir()
if path.is_dir() and (is_h32_station(path, capture_name) or is_dlog_station(path))
if path.is_dir()
and (
is_h32_station(path, capture_name)
or is_h32_raw_dlog_station(path, msop_object=msop_object)
or is_dlog_station(path)
)
],
key=lambda path: path.name,
)
if not stations:
raise FileNotFoundError(
f"no station with {capture_name}/lidar.rscap or dobject+dobject_recording under {stations_root}"
f"no station with H32 dlog/MSOP, {capture_name}/lidar.rscap, or "
f"dobject+dobject_recording under {stations_root}"
)
return stations
@@ -101,7 +119,7 @@ def export_legacy_dlog_station(
sys.executable,
str(exporter),
"--dlog",
str(station),
str(try_resolve_dlog_root(station) or station),
"--out",
str(out),
"--object",
@@ -154,6 +172,9 @@ def export_raw_to_combined(
out: Path,
station_names: list[str] | None = None,
lidar_capture_name: str = "h32.rscap",
msop_object: str = "frontlidar-msop-raw",
difop_object: str = "frontlidar-difop-raw",
require_difop: bool = True,
lidar_object: str = "frontlidar",
timezone: str = "+08:00",
stride: int = 1,
@@ -174,7 +195,6 @@ def export_raw_to_combined(
if out.exists() and any(out.iterdir()) and not overwrite:
raise FileExistsError(f"{out} is non-empty; pass --overwrite")
if overwrite and out.exists():
# Keep out root but clear known children so rebuild is deterministic.
for child in ("export", "parsed", "combined", "export_summary.json", "capture_audit.json"):
target = out / child
if target.is_dir():
@@ -187,15 +207,29 @@ def export_raw_to_combined(
parsed_root = out / "parsed"
combined_root = out / "combined"
stations = discover_stations(stations_root, station_names or [], lidar_capture_name)
stations = discover_stations(
stations_root, station_names or [], lidar_capture_name, msop_object
)
parse_summary = parse_serial(rtk_rscap, imu_rscap, parsed_root)
station_meta: list[dict[str, Any]] = []
lidar_segments: list[tuple[str, Path]] = []
saw_dlog = False
saw_legacy_dlog = False
for station in stations:
station_out = export_root / station.name
if is_h32_station(station, lidar_capture_name):
if is_h32_raw_dlog_station(station, msop_object=msop_object):
meta = export_station_h32_dlog(
station,
station_out,
msop_object=msop_object,
difop_object=difop_object,
require_difop=require_difop,
stride=stride,
write_reports=True,
resume=False,
)
kind = "h32_dlog_raw"
elif is_h32_station(station, lidar_capture_name):
meta = export_station_h32(
station,
station_out,
@@ -205,8 +239,10 @@ def export_raw_to_combined(
resume=False,
)
kind = "h32_rscap"
elif is_dlog_station(station):
saw_dlog = True
elif is_legacy_pointcloud_dlog_station(station, msop_object=msop_object) or is_dlog_station(
station
):
saw_legacy_dlog = True
export_legacy_dlog_station(
station,
station_out,
@@ -217,16 +253,18 @@ def export_raw_to_combined(
meta = {"source": str(station.resolve()), "kind": "legacy_dlog"}
kind = "legacy_dlog"
else:
raise RuntimeError(f"station {station.name} has neither H32 .rscap nor dlog layout")
raise RuntimeError(
f"station {station.name} has neither H32 raw dlog, .rscap, nor legacy dlog layout"
)
frames_dir = station_out / "frames"
if not frames_dir.is_dir() or not any(frames_dir.glob("*.npz")):
raise RuntimeError(f"no exported frames for station {station.name}: {frames_dir}")
lidar_segments.append((station.name, frames_dir))
station_meta.append({"station": station.name, "kind": kind, "frames_dir": str(frames_dir), **meta})
if saw_dlog and time_basis == "device_gnss":
if saw_legacy_dlog and time_basis == "device_gnss":
print(
"[warn] legacy dlog stations use host/DObject time; prefer --time-basis host",
"[warn] legacy point-cloud dlog stations use host/DObject time; prefer --time-basis host",
file=sys.stderr,
)
@@ -260,7 +298,9 @@ def export_raw_to_combined(
},
"timestamp_policy": {
"default_time_basis": time_basis,
"lidar_h32": "MSOP device timestamp → unix_time_ns",
"lidar_h32_dlog": "MSOP device timestamp → unix_time_ns; DIFOP channel angles for XYZ",
"lidar_h32_rscap": "MSOP device timestamp → unix_time_ns; default vertical angles",
"lidar_legacy_dlog": "DObject/host time; use time_basis=host",
"rtk": "GNSS week/TOW when time_basis=device_gnss; else host_receive_utc_ns",
"imu": "associated only; host-anchored device deltas in combined window",
"host_utc": "kept for audit; not the default calibration timeline for new captures",
@@ -281,8 +321,16 @@ def parse_args() -> argparse.Namespace:
parser.add_argument("--imu-rscap", type=Path, required=True, help="Continuous N300/IMU V2 .rscap")
parser.add_argument("--out", type=Path, required=True, help="Output package root (contains combined/)")
parser.add_argument("--station", action="append", default=[], help="Optional station name filter; repeatable")
parser.add_argument("--lidar-capture-name", default="h32.rscap")
parser.add_argument("--lidar-object", default="frontlidar", help="Legacy dlog DObject name")
parser.add_argument("--lidar-capture-name", default="h32.rscap", help="Legacy H32 .rscap filename")
parser.add_argument("--msop-object", default="frontlidar-msop-raw", help="Raw MSOP DObject name")
parser.add_argument("--difop-object", default="frontlidar-difop-raw", help="Raw DIFOP DObject name")
parser.add_argument(
"--require-difop",
action=argparse.BooleanOptionalAction,
default=True,
help="Require DIFOP channel angles for H32 raw dlog stations (default: true)",
)
parser.add_argument("--lidar-object", default="frontlidar", help="Legacy decoded point-cloud DObject name")
parser.add_argument("--timezone", default="+08:00", help="Legacy dlog tick timezone")
parser.add_argument("--stride", type=int, default=1)
parser.add_argument("--rtk-max-dt-ms", type=float, default=150.0)
@@ -304,6 +352,9 @@ def main() -> int:
out=args.out,
station_names=args.station,
lidar_capture_name=args.lidar_capture_name,
msop_object=args.msop_object,
difop_object=args.difop_object,
require_difop=args.require_difop,
lidar_object=args.lidar_object,
timezone=args.timezone,
stride=args.stride,
+17
View File
@@ -0,0 +1,17 @@
"""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 .load_session import H32DlogLidarSession, load_h32_dlog_lidar
from .payload_v1 import parse_difop_payload, parse_msop_batch_payload
__all__ = [
"H32DlogLidarSession",
"discover_records",
"iter_payloads",
"load_h32_dlog_lidar",
"parse_difop_angles",
"parse_difop_payload",
"parse_msop_batch_payload",
"resolve_dlog_root",
]
+40
View File
@@ -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)
+179
View File
@@ -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()
+69
View File
@@ -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")
+109
View File
@@ -0,0 +1,109 @@
"""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,
)
+204
View File
@@ -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()
+43 -10
View File
@@ -1,6 +1,6 @@
"""Decode RoboSense H32 MSOP V2 .rscap into Cartesian frames (metres).
"""Decode RoboSense H32 MSOP packets into Cartesian / polar 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, Sequence
import numpy as np
@@ -192,9 +193,10 @@ def _block_points_raw(
return np.asarray(rows, dtype=np.float32)
def iter_h32_frames_polar(
capture: CaptureFile,
def iter_h32_frames_polar_from_packets(
packets: Iterable[bytes],
*,
host_utc_ticks: Sequence[int] | None = None,
min_frame_points: int = MIN_FRAME_POINTS_DEFAULT,
frame_stride: int = 1,
min_range_m: float = 0.3,
@@ -203,7 +205,7 @@ def iter_h32_frames_polar(
vertical_deg: np.ndarray | None = None,
horizontal_deg: np.ndarray | None = None,
) -> list[LidarFramePolarExport]:
"""Assemble MSOP packets into polar frames for the RTKLiDAR combined contract."""
"""Assemble raw MSOP packets into polar frames for the combined contract."""
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)
@@ -218,6 +220,7 @@ def iter_h32_frames_polar(
prev_az: float | None = None
kept = 0
stride = max(1, int(frame_stride))
host_list = list(host_utc_ticks) if host_utc_ticks is not None else None
def emit() -> None:
nonlocal point_chunks, t_start, t_end, host_ns, kept
@@ -250,13 +253,15 @@ def iter_h32_frames_polar(
)
)
for chunk in capture.chunks:
packet = chunk.raw
for index, packet in enumerate(packets):
if len(packet) != PACKET_LENGTH:
continue
packet_t = device_timestamp_ms(packet) * 1e-3
unit = distance_unit_mm(packet)
chunk_host = ticks_to_unix_ns(chunk.receive_utc_ticks)
if host_list is not None and index < len(host_list):
chunk_host = ticks_to_unix_ns(int(host_list[index]))
else:
chunk_host = 0
idx = DATA_START
for _block in range(BLOCKS):
if idx + BLOCK_LENGTH > PACKET_LENGTH or packet[idx] != 255 or packet[idx + 1] != 238:
@@ -287,6 +292,34 @@ def iter_h32_frames_polar(
return frames
def iter_h32_frames_polar(
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[LidarFramePolarExport]:
"""Assemble MSOP packets from a V2 .rscap into polar frames."""
packets = [chunk.raw for chunk in capture.chunks]
host_ticks = [chunk.receive_utc_ticks for chunk in capture.chunks]
return iter_h32_frames_polar_from_packets(
packets,
host_utc_ticks=host_ticks,
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,
)
def iter_h32_frames(
capture: CaptureFile,
*,