支持 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
+9 -6
View File
@@ -50,20 +50,23 @@ X = T_RTK_lidar
## 3. 原始数据目录
大体积数据不提交Git。**新车默认布局**(H32 / G90 / N300 新插件,不再出 dlog):
大体积数据不提交Git。**新车默认布局**(H32 `RSLidarH32_3D_DLogCaptureNet48`G90/N300 仍为 `.rscap`):
```text
raw_dataset/
├── stations/ # 每站停稳后单独录一段雷达
│ ├── 001/h32.rscap
│ ├── 002/h32.rscap
├── stations/ # 每站停稳后单独录一段雷达 dlog
│ ├── 001/ # dobject/ + dobject_recording/
│ ├── dobject/ # 含 frontlidar-msop-raw / frontlidar-difop-raw
│ │ └── dobject_recording/
│ ├── 002/
│ └── ...
└── captures/
├── rtk.rscap # 进场到收工连续录(G90#PVTSLNA + #UNIHEADINGA
└── imu.rscap # 连续录(N300;仅关联,不参与外参求解)
```
一键导出默认:雷达用 **MSOP 设备时间**RTK 用 **GNSS week/TOW** 做最近邻关联(`-TimeBasis device_gnss`)。旧 dlog 数据集可继续放在同结构的 `dobject/` + `dobject_recording/` 下,并用 `-TimeBasis host`
一键导出默认:雷达用 **MSOP 设备时间** + DIFOP 通道角RTK 用 **GNSS week/TOW** 做最近邻关联(`-TimeBasis device_gnss`)。
仍兼容旧站 `h32.rscap`;旧「已解码点云」dlog 需用 `-TimeBasis host`
每个站点应在车辆完全静止后记录点云;建议不少于30站,并包含充足的直行、左转、右转和大角度转向姿态变化。
@@ -79,7 +82,7 @@ python -m pip install -r requirements.txt
## 5. 从原始数据一键复现
导出与 Lidar-IMU 的 `export_rscap_to_v1` 同级:**一条命令**把原始 rscap 变成标定可直接使用的 `combined/`
导出与 Lidar-IMU 的 `export_rscap_to_v1` 同级:**一条命令**把原始 H32 dlog + G90/N300 rscap 变成标定可直接使用的 `combined/`
仅导出中间包:
+2 -2
View File
@@ -1,12 +1,12 @@
# 数据说明
原始 H32/G90/N300 `.rscap`(以及旧版 LiDAR dlog)、逐帧 NPZ 和 prepared 点云体积较大,不进入 Git。请从项目云盘取得数据,并按根 README 中的目录示例放置;实际路径通过命令参数传入。
原始 H32 dlog / G90·N300 `.rscap`(以及旧版 LiDAR dlog / `h32.rscap`)、逐帧 NPZ 和 prepared 点云体积较大,不进入 Git。请从项目云盘取得数据,并按根 README 中的目录示例放置;实际路径通过命令参数传入。
推荐原始布局:
```text
raw_dataset/
├── stations/<站号>/h32.rscap
├── stations/<站号>/ # dobject/ + dobject_recording/MSOP+DIFOP
└── captures/rtk.rscap, imu.rscap
```
+4 -3
View File
@@ -20,8 +20,9 @@ python tools\export_raw_to_combined.py --stations-root ... --rtk-rscap ... --imu
常用参数:
- `-LidarCaptureName h32.rscap`:每站雷达文件名(也接受 `lidar.rscap`
- `-TimeBasis device_gnss`(默认):雷达设备时 ↔ GNSS week/TOW
- `-TimeBasis host`:旧 dlog + 主机接收时间关联
- 新车站目录为 H32 raw dlog(自动识别 `frontlidar-msop-raw` / `frontlidar-difop-raw`
- `-LidarCaptureName h32.rscap`:仅旧站 `.rscap` 文件名(也接受 `lidar.rscap`
- `-TimeBasis device_gnss`(默认):雷达设备时 ↔ GNSS week/TOW(新 dlog / rscap
- `-TimeBasis host`:旧「已解码点云」dlog + 主机接收时间关联
所有路径均为命令行参数。标定入口要求显式传入RTK/GGA参考点离地高度,避免静默使用与实车不符的默认值;默认生成目录`work/``outputs/`不会提交Git。
+190
View File
@@ -0,0 +1,190 @@
"""Unit tests for H32 Medulla raw dlog → station frame export helpers."""
from __future__ import annotations
import struct
import sys
from pathlib import Path
import numpy as np
ROOT = Path(__file__).resolve().parents[1]
TOOLS = ROOT / "tools"
sys.path.insert(0, str(TOOLS))
sys.path.insert(0, str(TOOLS / "rscap_v2"))
from export_h32_rscap_station import export_station_h32_dlog, is_h32_raw_dlog_station # noqa: E402
from h32_dlog.difop import CHANNELS, HORIZONTAL_START, VERTICAL_START, parse_difop_angles # noqa: E402
from h32_dlog.dobject import discover_records, iter_payloads, resolve_dlog_root # noqa: E402
from h32_dlog.load_session import load_h32_dlog_lidar # noqa: E402
from h32_dlog.payload_v1 import ( # noqa: E402
MsopPacketItem,
build_difop_payload,
build_msop_batch_payload,
parse_difop_payload,
parse_msop_batch_payload,
)
from h32_msop import PACKET_LENGTH, iter_h32_frames_polar_from_packets # noqa: E402
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,
) -> int:
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("<q", ticks)
+ bytes([len(id_b)])
+ id_b
+ struct.pack("<i", len(payload))
+ payload
)
with path.open("ab" if path.exists() else "wb") as handle:
start = handle.tell()
handle.write(blob)
return start
def test_parse_msop_and_difop_payload_roundtrip():
packet = _make_msop_packet(seconds=1700000000, microseconds=123456)
item = MsopPacketItem(
sequence=7,
device_timestamp_us=1700000000 * 1_000_000 + 123456,
device_timestamp_valid=True,
host_receive_utc_ticks=111,
host_receive_monotonic_ticks=222,
raw=packet,
)
msop_payload = build_msop_batch_payload(packets=[item], session_id="sess-a")
batch = parse_msop_batch_payload(msop_payload)
assert batch.session_id == "sess-a"
assert len(batch.packets) == 1
assert batch.packets[0].raw == packet
vertical = [-16.0 + i * (32.0 / 31) for i in range(CHANNELS)]
difop_raw = _make_difop_packet(vertical=vertical, horizontal=[0.05] * CHANNELS)
difop = parse_difop_payload(build_difop_payload(raw=difop_raw, sequence=3))
angles = parse_difop_angles(difop.raw)
assert np.allclose(angles.vertical_deg, vertical, atol=1e-2)
assert np.allclose(angles.horizontal_deg, 0.05, atol=1e-2)
def test_export_station_h32_dlog_mini(tmp_path: Path):
station = tmp_path / "001"
dorec_name = "raw.dorec"
dorec_path = station / "dobject_recording" / dorec_name
log_path = station / "dobject" / "rec.log"
vertical = [-16.0 + i * (32.0 / 31) for i in range(CHANNELS)]
difop_payload = build_difop_payload(raw=_make_difop_packet(vertical=vertical), sequence=1)
msop_packet = _make_msop_packet(az_deg=15.0)
msop_payload = build_msop_batch_payload(
packets=[
MsopPacketItem(
sequence=1,
device_timestamp_us=100_000_000,
device_timestamp_valid=True,
host_receive_utc_ticks=621355968000000000 + 10_000_000,
host_receive_monotonic_ticks=2,
raw=msop_packet,
)
]
)
off_difop = _write_dorec_record(
dorec_path,
object_name="frontlidar-difop-raw",
ticks=1000,
record_id="AA",
payload=difop_payload,
)
off_msop = _write_dorec_record(
dorec_path,
object_name="frontlidar-msop-raw",
ticks=1001,
record_id="BB",
payload=msop_payload,
)
log_path.parent.mkdir(parents=True, exist_ok=True)
log_path.write_text(
"\n".join(
[
f"[t] DObject `frontlidar-difop-raw` post len={len(difop_payload)}B, "
f"id:AA, tic:1000, @{dorec_name}:{off_difop}",
f"[t] DObject `frontlidar-msop-raw` post len={len(msop_payload)}B, "
f"id:BB, tic:1001, @{dorec_name}:{off_msop}",
]
)
+ "\n",
encoding="utf-8",
)
assert resolve_dlog_root(station) == station.resolve()
assert is_h32_raw_dlog_station(station)
assert len(discover_records(station, "frontlidar-msop-raw")) == 1
assert len(list(iter_payloads(station, "frontlidar-msop-raw"))) == 1
session = load_h32_dlog_lidar(station, require_difop=True)
assert session.angle_source == "difop_channel_angles"
frames = iter_h32_frames_polar_from_packets(
session.msop_packets,
host_utc_ticks=session.msop_host_utc_ticks,
min_frame_points=1,
vertical_deg=session.vertical_deg,
horizontal_deg=session.horizontal_deg,
)
assert len(frames) == 1
assert frames[0].points_raw.shape[1] == 5
out = tmp_path / "export"
meta = export_station_h32_dlog(station, out, require_difop=True, min_frame_points=1)
assert meta["kind"] == "h32_dlog_raw"
assert meta["angle_source"] == "difop_channel_angles"
assert meta["frames_written"] >= 1
assert any((out / "frames").glob("*.npz"))
+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,
*,
+4 -4
View File
@@ -45,20 +45,20 @@ python -m pip install -r requirements.txt
### 3.1 目录结构
**新车(默认)**:每站一段 H32 雷达 `.rscap`RTK/IMU 全程各一条:
**新车(默认)**:每站一段 H32 雷达 dlog`RSLidarH32_3D_DLogCaptureNet48`,含 raw MSOP/DIFOPRTK/IMU 全程各一条 `.rscap`
```text
raw_dataset/
├── stations/
│ ├── 001/h32.rscap
│ ├── 002/h32.rscap
│ ├── 001/ # dobject/ + dobject_recording/
│ ├── 002/
│ └── ...
└── captures/
├── rtk.rscap # G90#PVTSLNA 位置 + #UNIHEADINGA 航向
└── imu.rscap # N300;仅关联保存,不参与外参求解
```
旧车 dlog 布局(`dobject/` + `dobject_recording/`)仍可被导出脚本识别关联时间请用 `-TimeBasis host`
仍兼容旧站 `h32.rscap`。仅含已解码点云、无 raw MSOP 的旧 dlog 仍可识别关联时间请用 `-TimeBasis host`
### 3.2 采集要求