支持 H32 DLogCapture(MSOP+DIFOP)站导出到 combined
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
@@ -50,20 +50,23 @@ X = T_RTK_lidar
|
|||||||
|
|
||||||
## 3. 原始数据目录
|
## 3. 原始数据目录
|
||||||
|
|
||||||
大体积数据不提交Git。**新车默认布局**(H32 / G90 / N300 新插件,不再出 dlog):
|
大体积数据不提交Git。**新车默认布局**(H32 用 `RSLidarH32_3D_DLogCaptureNet48`;G90/N300 仍为 `.rscap`):
|
||||||
|
|
||||||
```text
|
```text
|
||||||
raw_dataset/
|
raw_dataset/
|
||||||
├── stations/ # 每站停稳后单独录一段雷达
|
├── stations/ # 每站停稳后单独录一段雷达 dlog
|
||||||
│ ├── 001/h32.rscap
|
│ ├── 001/ # dobject/ + dobject_recording/
|
||||||
│ ├── 002/h32.rscap
|
│ │ ├── dobject/ # 含 frontlidar-msop-raw / frontlidar-difop-raw
|
||||||
|
│ │ └── dobject_recording/
|
||||||
|
│ ├── 002/
|
||||||
│ └── ...
|
│ └── ...
|
||||||
└── captures/
|
└── captures/
|
||||||
├── rtk.rscap # 进场到收工连续录(G90:#PVTSLNA + #UNIHEADINGA)
|
├── rtk.rscap # 进场到收工连续录(G90:#PVTSLNA + #UNIHEADINGA)
|
||||||
└── imu.rscap # 连续录(N300;仅关联,不参与外参求解)
|
└── 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站,并包含充足的直行、左转、右转和大角度转向姿态变化。
|
每个站点应在车辆完全静止后记录点云;建议不少于30站,并包含充足的直行、左转、右转和大角度转向姿态变化。
|
||||||
|
|
||||||
@@ -79,7 +82,7 @@ python -m pip install -r requirements.txt
|
|||||||
|
|
||||||
## 5. 从原始数据一键复现
|
## 5. 从原始数据一键复现
|
||||||
|
|
||||||
导出与 Lidar-IMU 的 `export_rscap_to_v1` 同级:**一条命令**把原始 rscap 变成标定可直接使用的 `combined/`。
|
导出与 Lidar-IMU 的 `export_rscap_to_v1` 同级:**一条命令**把原始 H32 dlog + G90/N300 rscap 变成标定可直接使用的 `combined/`。
|
||||||
|
|
||||||
仅导出中间包:
|
仅导出中间包:
|
||||||
|
|
||||||
|
|||||||
+2
-2
@@ -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
|
```text
|
||||||
raw_dataset/
|
raw_dataset/
|
||||||
├── stations/<站号>/h32.rscap
|
├── stations/<站号>/ # dobject/ + dobject_recording/(MSOP+DIFOP)
|
||||||
└── captures/rtk.rscap, imu.rscap
|
└── captures/rtk.rscap, imu.rscap
|
||||||
```
|
```
|
||||||
|
|
||||||
|
|||||||
+4
-3
@@ -20,8 +20,9 @@ python tools\export_raw_to_combined.py --stations-root ... --rtk-rscap ... --imu
|
|||||||
|
|
||||||
常用参数:
|
常用参数:
|
||||||
|
|
||||||
- `-LidarCaptureName h32.rscap`:每站雷达文件名(也接受 `lidar.rscap`)
|
- 新车站目录为 H32 raw dlog(自动识别 `frontlidar-msop-raw` / `frontlidar-difop-raw`)
|
||||||
- `-TimeBasis device_gnss`(默认):雷达设备时 ↔ GNSS week/TOW
|
- `-LidarCaptureName h32.rscap`:仅旧站 `.rscap` 文件名(也接受 `lidar.rscap`)
|
||||||
- `-TimeBasis host`:旧 dlog + 主机接收时间关联
|
- `-TimeBasis device_gnss`(默认):雷达设备时 ↔ GNSS week/TOW(新 dlog / rscap)
|
||||||
|
- `-TimeBasis host`:旧「已解码点云」dlog + 主机接收时间关联
|
||||||
|
|
||||||
所有路径均为命令行参数。标定入口要求显式传入RTK/GGA参考点离地高度,避免静默使用与实车不符的默认值;默认生成目录`work/`和`outputs/`不会提交Git。
|
所有路径均为命令行参数。标定入口要求显式传入RTK/GGA参考点离地高度,避免静默使用与实车不符的默认值;默认生成目录`work/`和`outputs/`不会提交Git。
|
||||||
|
|||||||
@@ -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
@@ -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`** | **一步导出**:逐站 H32(dlog MSOP+DIFOP 或旧 `.rscap`)+ 全程 G90/N300 `.rscap` → `combined/` |
|
||||||
| `export_h32_rscap_station.py` | 内部零件:单站 H32 → 雷达帧 NPZ(一般不必单独跑) |
|
| `export_h32_rscap_station.py` | 内部零件:单站 H32 → 雷达帧 NPZ(一般不必单独跑) |
|
||||||
| `frontlidar_dlog_export.py` | **旧数据** LiDAR dlog → 逐帧 NPZ;由一步导出在遇到 dlog 站时自动调用 |
|
| `h32_dlog/` | 新 H32 DLogCapture:dobject 索引、MSOP/DIFOP payload、DIFOP 通道角 |
|
||||||
|
| `frontlidar_dlog_export.py` | **旧数据** 已解码点云 dlog → 逐帧 NPZ;无 raw MSOP 时由一步导出回退调用 |
|
||||||
| `rscap_v2/parse_rtk_imu_v2.py` | 单独解析 RTK/IMU(调试用);一步导出已内嵌同等逻辑 |
|
| `rscap_v2/parse_rtk_imu_v2.py` | 单独解析 RTK/IMU(调试用);一步导出已内嵌同等逻辑 |
|
||||||
| `rscap_v2/h32_msop.py` | H32 MSOP 解码(XYZ / 极坐标 `points_raw`) |
|
| `rscap_v2/h32_msop.py` | H32 MSOP 解码(XYZ / 极坐标 `points_raw`) |
|
||||||
| `rscap_v2/n300_imu.py` | N300 FDILink 采样解码 |
|
| `rscap_v2/n300_imu.py` | N300 FDILink 采样解码 |
|
||||||
|
|||||||
@@ -1,5 +1,10 @@
|
|||||||
#!/usr/bin/env python3
|
#!/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``.
|
This is an **internal** helper used by ``export_raw_to_combined.py``.
|
||||||
For RTK–LiDAR calibration, prefer the one-shot exporter that writes ``combined/``.
|
For RTK–LiDAR 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)
|
- ``unix_time_ns``: H32 MSOP device timestamp (seconds+us → ns)
|
||||||
- ``frame_counter``, ``point_count``, optional host receive stamp
|
- ``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
|
from __future__ import annotations
|
||||||
@@ -25,10 +30,16 @@ from typing import Any
|
|||||||
import numpy as np
|
import numpy as np
|
||||||
|
|
||||||
ROOT = Path(__file__).resolve().parent
|
ROOT = Path(__file__).resolve().parent
|
||||||
|
sys.path.insert(0, str(ROOT))
|
||||||
sys.path.insert(0, str(ROOT / "rscap_v2"))
|
sys.path.insert(0, str(ROOT / "rscap_v2"))
|
||||||
|
|
||||||
from capture_format_v2 import file_summary, read_capture # noqa: E402
|
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:
|
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(
|
def try_resolve_dlog_root(station_dir: Path) -> Path | None:
|
||||||
station: Path,
|
try:
|
||||||
out: Path,
|
return resolve_dlog_root(station_dir)
|
||||||
*,
|
except FileNotFoundError:
|
||||||
capture_name: str = "h32.rscap",
|
return None
|
||||||
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."""
|
|
||||||
|
|
||||||
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 = out / "frames"
|
||||||
frames_dir.mkdir(parents=True, exist_ok=True)
|
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
|
saver = np.savez_compressed if compress else np.savez
|
||||||
manifest_rows: list[dict[str, Any]] = []
|
manifest_rows: list[dict[str, Any]] = []
|
||||||
written = 0
|
written = 0
|
||||||
@@ -93,7 +115,7 @@ def export_station_h32(
|
|||||||
"device_time_s": np.asarray([frame.t_start_s], dtype=np.float64),
|
"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),
|
"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),
|
"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)
|
saver(destination, **payload)
|
||||||
written += 1
|
written += 1
|
||||||
@@ -108,18 +130,17 @@ def export_station_h32(
|
|||||||
)
|
)
|
||||||
|
|
||||||
metadata: dict[str, Any] = {
|
metadata: dict[str, Any] = {
|
||||||
"source_rscap": str(rscap.resolve()),
|
|
||||||
"capture": file_summary(capture),
|
|
||||||
"frames_decoded": len(frames),
|
"frames_decoded": len(frames),
|
||||||
"frames_written": written,
|
"frames_written": written,
|
||||||
"frames_dir": str(frames_dir.resolve()),
|
"frames_dir": str(frames_dir.resolve()),
|
||||||
"time_basis": "H32 MSOP device timestamp (packet seconds+microseconds)",
|
"time_basis": "H32 MSOP device timestamp (packet seconds+microseconds)",
|
||||||
"points_raw_columns": ["d_mm", "azimuth_deg", "altitude_deg", "intensity", "progression"],
|
"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 / "metadata.json").write_text(json.dumps(metadata, ensure_ascii=False, indent=2), encoding="utf-8")
|
||||||
(out / "README.md").write_text(
|
(out / "README.md").write_text(
|
||||||
"# H32 station export (internal)\n\n"
|
"# H32 station export (internal)\n\n"
|
||||||
f"- source: `{rscap}`\n"
|
f"- source: `{source_label}`\n"
|
||||||
f"- frames: `{frames_dir}`\n"
|
f"- frames: `{frames_dir}`\n"
|
||||||
"- Prefer ``tools/export_raw_to_combined.py`` for the full RTK–LiDAR package.\n",
|
"- Prefer ``tools/export_raw_to_combined.py`` for the full RTK–LiDAR package.\n",
|
||||||
encoding="utf-8",
|
encoding="utf-8",
|
||||||
@@ -128,18 +149,133 @@ def export_station_h32(
|
|||||||
reports = out / "reports"
|
reports = out / "reports"
|
||||||
reports.mkdir(parents=True, exist_ok=True)
|
reports.mkdir(parents=True, exist_ok=True)
|
||||||
with (reports / "manifest.csv").open("w", encoding="utf-8", newline="") as stream:
|
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.writeheader()
|
||||||
writer.writerows(manifest_rows)
|
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
|
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:
|
def parse_args() -> argparse.Namespace:
|
||||||
parser = argparse.ArgumentParser(description=__doc__)
|
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("--out", type=Path, required=True)
|
||||||
parser.add_argument("--capture-name", default="h32.rscap")
|
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("--stride", type=int, default=1)
|
||||||
parser.add_argument("--min-frame-points", type=int, default=100)
|
parser.add_argument("--min-frame-points", type=int, default=100)
|
||||||
parser.add_argument("--min-range-m", type=float, default=0.3)
|
parser.add_argument("--min-range-m", type=float, default=0.3)
|
||||||
@@ -152,18 +288,48 @@ def parse_args() -> argparse.Namespace:
|
|||||||
|
|
||||||
def main() -> int:
|
def main() -> int:
|
||||||
args = parse_args()
|
args = parse_args()
|
||||||
metadata = export_station_h32(
|
station = args.station
|
||||||
args.station,
|
if station.is_file() and station.suffix.lower() == ".rscap":
|
||||||
args.out,
|
metadata = export_station_h32(
|
||||||
capture_name=args.capture_name,
|
station,
|
||||||
stride=args.stride,
|
args.out,
|
||||||
min_frame_points=args.min_frame_points,
|
capture_name=args.capture_name,
|
||||||
min_range_m=args.min_range_m,
|
stride=args.stride,
|
||||||
max_range_m=args.max_range_m,
|
min_frame_points=args.min_frame_points,
|
||||||
compress=args.compress,
|
min_range_m=args.min_range_m,
|
||||||
write_reports=args.write_reports,
|
max_range_m=args.max_range_m,
|
||||||
resume=args.resume,
|
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))
|
print(json.dumps(metadata, ensure_ascii=False, indent=2))
|
||||||
return 0
|
return 0
|
||||||
|
|
||||||
|
|||||||
@@ -1,19 +1,22 @@
|
|||||||
#!/usr/bin/env python3
|
#!/usr/bin/env python3
|
||||||
"""One-shot export: raw H32/G90/N300 captures → RTK–LiDAR ``combined/`` package.
|
"""One-shot export: raw H32/G90/N300 captures → RTK–LiDAR ``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/``
|
calibration-ready intermediate out. Downstream prepare/solve consume ``combined/``
|
||||||
only (``manifest.csv`` + associated frame NPZs).
|
only (``manifest.csv`` + associated frame NPZs).
|
||||||
|
|
||||||
Expected raw layout:
|
Expected raw layout (new H32 DLogCapture):
|
||||||
|
|
||||||
stations/
|
stations/
|
||||||
001/h32.rscap
|
001/ # dobject/ + dobject_recording/ (or 001/dlog/...)
|
||||||
002/h32.rscap
|
002/
|
||||||
...
|
...
|
||||||
captures/ (paths passed explicitly)
|
captures/
|
||||||
rtk.rscap # G90: #PVTSLNA + #UNIHEADINGA
|
rtk.rscap # G90: #PVTSLNA + #UNIHEADINGA
|
||||||
imu.rscap # N300 (associated only; not used in AX=XB)
|
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``:
|
Output under ``--out``:
|
||||||
|
|
||||||
@@ -22,9 +25,6 @@ Output under ``--out``:
|
|||||||
combined/frames/*.npz + manifest.csv + dataset_summary.json
|
combined/frames/*.npz + manifest.csv + dataset_summary.json
|
||||||
export_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.
|
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 build_multisensor_npz import build_combined # noqa: E402
|
||||||
from capture_format_v2 import file_summary, read_capture # 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
|
from pipeline_common_corrected import ( # noqa: E402
|
||||||
parse_imu_capture,
|
parse_imu_capture,
|
||||||
parse_rtk_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:
|
def is_h32_station(station: Path, capture_name: str) -> bool:
|
||||||
try:
|
try:
|
||||||
resolve_lidar_rscap(station, capture_name)
|
resolve_lidar_rscap(station, capture_name)
|
||||||
return True
|
|
||||||
except FileNotFoundError:
|
except FileNotFoundError:
|
||||||
return False
|
return False
|
||||||
|
return True
|
||||||
|
|
||||||
|
|
||||||
def is_dlog_station(station: Path) -> bool:
|
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:
|
if names:
|
||||||
stations = [stations_root / name for name in names]
|
stations = [stations_root / name for name in names]
|
||||||
missing = [str(path) for path in stations if not path.is_dir()]
|
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
|
path
|
||||||
for path in stations_root.iterdir()
|
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,
|
key=lambda path: path.name,
|
||||||
)
|
)
|
||||||
if not stations:
|
if not stations:
|
||||||
raise FileNotFoundError(
|
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
|
return stations
|
||||||
|
|
||||||
@@ -101,7 +119,7 @@ def export_legacy_dlog_station(
|
|||||||
sys.executable,
|
sys.executable,
|
||||||
str(exporter),
|
str(exporter),
|
||||||
"--dlog",
|
"--dlog",
|
||||||
str(station),
|
str(try_resolve_dlog_root(station) or station),
|
||||||
"--out",
|
"--out",
|
||||||
str(out),
|
str(out),
|
||||||
"--object",
|
"--object",
|
||||||
@@ -154,6 +172,9 @@ def export_raw_to_combined(
|
|||||||
out: Path,
|
out: Path,
|
||||||
station_names: list[str] | None = None,
|
station_names: list[str] | None = None,
|
||||||
lidar_capture_name: str = "h32.rscap",
|
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",
|
lidar_object: str = "frontlidar",
|
||||||
timezone: str = "+08:00",
|
timezone: str = "+08:00",
|
||||||
stride: int = 1,
|
stride: int = 1,
|
||||||
@@ -174,7 +195,6 @@ def export_raw_to_combined(
|
|||||||
if out.exists() and any(out.iterdir()) and not overwrite:
|
if out.exists() and any(out.iterdir()) and not overwrite:
|
||||||
raise FileExistsError(f"{out} is non-empty; pass --overwrite")
|
raise FileExistsError(f"{out} is non-empty; pass --overwrite")
|
||||||
if overwrite and out.exists():
|
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"):
|
for child in ("export", "parsed", "combined", "export_summary.json", "capture_audit.json"):
|
||||||
target = out / child
|
target = out / child
|
||||||
if target.is_dir():
|
if target.is_dir():
|
||||||
@@ -187,15 +207,29 @@ def export_raw_to_combined(
|
|||||||
parsed_root = out / "parsed"
|
parsed_root = out / "parsed"
|
||||||
combined_root = out / "combined"
|
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)
|
parse_summary = parse_serial(rtk_rscap, imu_rscap, parsed_root)
|
||||||
|
|
||||||
station_meta: list[dict[str, Any]] = []
|
station_meta: list[dict[str, Any]] = []
|
||||||
lidar_segments: list[tuple[str, Path]] = []
|
lidar_segments: list[tuple[str, Path]] = []
|
||||||
saw_dlog = False
|
saw_legacy_dlog = False
|
||||||
for station in stations:
|
for station in stations:
|
||||||
station_out = export_root / station.name
|
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(
|
meta = export_station_h32(
|
||||||
station,
|
station,
|
||||||
station_out,
|
station_out,
|
||||||
@@ -205,8 +239,10 @@ def export_raw_to_combined(
|
|||||||
resume=False,
|
resume=False,
|
||||||
)
|
)
|
||||||
kind = "h32_rscap"
|
kind = "h32_rscap"
|
||||||
elif is_dlog_station(station):
|
elif is_legacy_pointcloud_dlog_station(station, msop_object=msop_object) or is_dlog_station(
|
||||||
saw_dlog = True
|
station
|
||||||
|
):
|
||||||
|
saw_legacy_dlog = True
|
||||||
export_legacy_dlog_station(
|
export_legacy_dlog_station(
|
||||||
station,
|
station,
|
||||||
station_out,
|
station_out,
|
||||||
@@ -217,16 +253,18 @@ def export_raw_to_combined(
|
|||||||
meta = {"source": str(station.resolve()), "kind": "legacy_dlog"}
|
meta = {"source": str(station.resolve()), "kind": "legacy_dlog"}
|
||||||
kind = "legacy_dlog"
|
kind = "legacy_dlog"
|
||||||
else:
|
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"
|
frames_dir = station_out / "frames"
|
||||||
if not frames_dir.is_dir() or not any(frames_dir.glob("*.npz")):
|
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}")
|
raise RuntimeError(f"no exported frames for station {station.name}: {frames_dir}")
|
||||||
lidar_segments.append((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})
|
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(
|
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,
|
file=sys.stderr,
|
||||||
)
|
)
|
||||||
|
|
||||||
@@ -260,7 +298,9 @@ def export_raw_to_combined(
|
|||||||
},
|
},
|
||||||
"timestamp_policy": {
|
"timestamp_policy": {
|
||||||
"default_time_basis": time_basis,
|
"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",
|
"rtk": "GNSS week/TOW when time_basis=device_gnss; else host_receive_utc_ns",
|
||||||
"imu": "associated only; host-anchored device deltas in combined window",
|
"imu": "associated only; host-anchored device deltas in combined window",
|
||||||
"host_utc": "kept for audit; not the default calibration timeline for new captures",
|
"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("--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("--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("--station", action="append", default=[], help="Optional station name filter; repeatable")
|
||||||
parser.add_argument("--lidar-capture-name", default="h32.rscap")
|
parser.add_argument("--lidar-capture-name", default="h32.rscap", help="Legacy H32 .rscap filename")
|
||||||
parser.add_argument("--lidar-object", default="frontlidar", help="Legacy dlog DObject name")
|
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("--timezone", default="+08:00", help="Legacy dlog tick timezone")
|
||||||
parser.add_argument("--stride", type=int, default=1)
|
parser.add_argument("--stride", type=int, default=1)
|
||||||
parser.add_argument("--rtk-max-dt-ms", type=float, default=150.0)
|
parser.add_argument("--rtk-max-dt-ms", type=float, default=150.0)
|
||||||
@@ -304,6 +352,9 @@ def main() -> int:
|
|||||||
out=args.out,
|
out=args.out,
|
||||||
station_names=args.station,
|
station_names=args.station,
|
||||||
lidar_capture_name=args.lidar_capture_name,
|
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,
|
lidar_object=args.lidar_object,
|
||||||
timezone=args.timezone,
|
timezone=args.timezone,
|
||||||
stride=args.stride,
|
stride=args.stride,
|
||||||
|
|||||||
@@ -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",
|
||||||
|
]
|
||||||
@@ -0,0 +1,40 @@
|
|||||||
|
"""Parse RoboSense H32 DIFOP channel calibration angles."""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
from dataclasses import dataclass
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
CHANNELS = 32
|
||||||
|
VERTICAL_START = 468
|
||||||
|
HORIZONTAL_START = 564
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class DifopAngles:
|
||||||
|
vertical_deg: np.ndarray # (32,)
|
||||||
|
horizontal_deg: np.ndarray # (32,)
|
||||||
|
|
||||||
|
|
||||||
|
def _read_u16_be(packet: bytes, index: int) -> int:
|
||||||
|
return (packet[index] << 8) | packet[index + 1]
|
||||||
|
|
||||||
|
|
||||||
|
def signed_angle_deg(packet: bytes, index: int) -> float:
|
||||||
|
"""Match RSLidarH32 plugin SignedAngle: sign byte + BE u16 * 0.01 deg."""
|
||||||
|
|
||||||
|
sign = -1.0 if packet[index] > 0 else 1.0
|
||||||
|
return sign * _read_u16_be(packet, index + 1) * 0.01
|
||||||
|
|
||||||
|
|
||||||
|
def parse_difop_angles(packet: bytes) -> DifopAngles:
|
||||||
|
needed = HORIZONTAL_START + CHANNELS * 3
|
||||||
|
if len(packet) < needed:
|
||||||
|
raise ValueError(f"DIFOP packet too short: {len(packet)} < {needed}")
|
||||||
|
vertical = np.empty(CHANNELS, dtype=np.float64)
|
||||||
|
horizontal = np.empty(CHANNELS, dtype=np.float64)
|
||||||
|
for channel in range(CHANNELS):
|
||||||
|
vertical[channel] = signed_angle_deg(packet, VERTICAL_START + channel * 3)
|
||||||
|
horizontal[channel] = signed_angle_deg(packet, HORIZONTAL_START + channel * 3)
|
||||||
|
return DifopAngles(vertical_deg=vertical, horizontal_deg=horizontal)
|
||||||
@@ -0,0 +1,179 @@
|
|||||||
|
"""Index and read Medulla DObject recordings (dobject/ + dobject_recording/)."""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import re
|
||||||
|
import struct
|
||||||
|
from dataclasses import dataclass
|
||||||
|
from pathlib import Path
|
||||||
|
from typing import BinaryIO, Iterator
|
||||||
|
|
||||||
|
|
||||||
|
RECORD_RE = re.compile(
|
||||||
|
r"^\[(?P<log_time>[^]]+)\].*?DObject `(?P<name>[^`]+)` post "
|
||||||
|
r"len=(?P<len>\d+)B, id:(?P<id>[0-9A-Fa-f]+), tic:(?P<tic>\d+), "
|
||||||
|
r"@(?P<file>[^:]+):(?P<offset>\d+)"
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class RecordRef:
|
||||||
|
sequence: int
|
||||||
|
object_name: str
|
||||||
|
log_time: str
|
||||||
|
source_log: str
|
||||||
|
source_dorec: str
|
||||||
|
source_offset: int
|
||||||
|
payload_length: int
|
||||||
|
log_record_id: str
|
||||||
|
dotnet_ticks: int
|
||||||
|
|
||||||
|
|
||||||
|
def resolve_dlog_root(value: Path | str) -> Path:
|
||||||
|
root = Path(value).expanduser().resolve()
|
||||||
|
if (root / "dobject").is_dir() and (root / "dobject_recording").is_dir():
|
||||||
|
return root
|
||||||
|
child = root / "dlog"
|
||||||
|
if (child / "dobject").is_dir() and (child / "dobject_recording").is_dir():
|
||||||
|
return child
|
||||||
|
raise FileNotFoundError(f"{root} does not contain dobject and dobject_recording")
|
||||||
|
|
||||||
|
|
||||||
|
def discover_records(dlog_root: Path, object_name: str) -> list[RecordRef]:
|
||||||
|
pending: list[tuple[str, str, str, int, int, str, int, str]] = []
|
||||||
|
for log_path in sorted((dlog_root / "dobject").rglob("*.log")):
|
||||||
|
relative_log = log_path.relative_to(dlog_root).as_posix()
|
||||||
|
with log_path.open("r", encoding="utf-8", errors="replace") as stream:
|
||||||
|
for line in stream:
|
||||||
|
match = RECORD_RE.search(line)
|
||||||
|
if not match or match.group("name").casefold() != object_name.casefold():
|
||||||
|
continue
|
||||||
|
pending.append(
|
||||||
|
(
|
||||||
|
match.group("name"),
|
||||||
|
match.group("log_time"),
|
||||||
|
relative_log,
|
||||||
|
int(match.group("offset")),
|
||||||
|
int(match.group("len")),
|
||||||
|
match.group("id").upper(),
|
||||||
|
int(match.group("tic")),
|
||||||
|
match.group("file"),
|
||||||
|
)
|
||||||
|
)
|
||||||
|
pending.sort(key=lambda item: (item[6], item[7].casefold(), item[3]))
|
||||||
|
seen: set[tuple[str, int, int]] = set()
|
||||||
|
records: list[RecordRef] = []
|
||||||
|
for item in pending:
|
||||||
|
key = (item[7].casefold(), item[3], item[6])
|
||||||
|
if key in seen:
|
||||||
|
continue
|
||||||
|
seen.add(key)
|
||||||
|
records.append(
|
||||||
|
RecordRef(
|
||||||
|
sequence=len(records),
|
||||||
|
object_name=item[0],
|
||||||
|
log_time=item[1],
|
||||||
|
source_log=item[2],
|
||||||
|
source_dorec=item[7],
|
||||||
|
source_offset=item[3],
|
||||||
|
payload_length=item[4],
|
||||||
|
log_record_id=item[5],
|
||||||
|
dotnet_ticks=item[6],
|
||||||
|
)
|
||||||
|
)
|
||||||
|
return records
|
||||||
|
|
||||||
|
|
||||||
|
def index_dorec_files(dlog_root: Path) -> dict[str, list[Path]]:
|
||||||
|
result: dict[str, list[Path]] = {}
|
||||||
|
for path in (dlog_root / "dobject_recording").rglob("*.dorec"):
|
||||||
|
result.setdefault(path.name.casefold(), []).append(path)
|
||||||
|
return result
|
||||||
|
|
||||||
|
|
||||||
|
def choose_dorec(index: dict[str, list[Path]], name: str) -> Path:
|
||||||
|
matches = index.get(Path(name).name.casefold(), [])
|
||||||
|
if not matches:
|
||||||
|
raise FileNotFoundError(f"missing recording file: {name}")
|
||||||
|
if len(matches) > 1:
|
||||||
|
raise RuntimeError(f"ambiguous recording file {name}: {matches}")
|
||||||
|
return matches[0]
|
||||||
|
|
||||||
|
|
||||||
|
def read_exact(stream: BinaryIO, size: int) -> bytes:
|
||||||
|
data = stream.read(size)
|
||||||
|
if len(data) != size:
|
||||||
|
raise EOFError(f"expected {size} bytes, got {len(data)}")
|
||||||
|
return data
|
||||||
|
|
||||||
|
|
||||||
|
def read_record_payload(path: Path, record: RecordRef) -> bytes:
|
||||||
|
with path.open("rb") as stream:
|
||||||
|
stream.seek(record.source_offset)
|
||||||
|
name_length = read_exact(stream, 1)[0]
|
||||||
|
name = read_exact(stream, name_length).decode("ascii")
|
||||||
|
ticks = struct.unpack("<q", read_exact(stream, 8))[0]
|
||||||
|
id_length = read_exact(stream, 1)[0]
|
||||||
|
id_bytes = read_exact(stream, id_length)
|
||||||
|
payload_length = struct.unpack("<i", read_exact(stream, 4))[0]
|
||||||
|
payload = read_exact(stream, payload_length)
|
||||||
|
|
||||||
|
try:
|
||||||
|
record_id = id_bytes.decode("ascii")
|
||||||
|
except UnicodeDecodeError:
|
||||||
|
record_id = id_bytes.hex().upper()
|
||||||
|
if name != record.object_name:
|
||||||
|
raise ValueError(f"name mismatch: log={record.object_name}, dorec={name}")
|
||||||
|
if ticks != record.dotnet_ticks:
|
||||||
|
raise ValueError(f"tick mismatch: log={record.dotnet_ticks}, dorec={ticks}")
|
||||||
|
if payload_length != record.payload_length:
|
||||||
|
raise ValueError(f"payload mismatch: log={record.payload_length}, dorec={payload_length}")
|
||||||
|
if record_id.upper() != record.log_record_id.upper():
|
||||||
|
raise ValueError(f"record id mismatch: log={record.log_record_id}, dorec={record_id}")
|
||||||
|
return payload
|
||||||
|
|
||||||
|
|
||||||
|
def iter_payloads(dlog_root: Path, object_name: str) -> Iterator[tuple[RecordRef, bytes]]:
|
||||||
|
root = resolve_dlog_root(dlog_root)
|
||||||
|
records = discover_records(root, object_name)
|
||||||
|
if not records:
|
||||||
|
return
|
||||||
|
dorec_index = index_dorec_files(root)
|
||||||
|
open_files: dict[str, tuple[Path, BinaryIO]] = {}
|
||||||
|
try:
|
||||||
|
for record in records:
|
||||||
|
key = record.source_dorec.casefold()
|
||||||
|
handle = open_files.get(key)
|
||||||
|
if handle is None:
|
||||||
|
path = choose_dorec(dorec_index, record.source_dorec)
|
||||||
|
handle = (path, path.open("rb"))
|
||||||
|
open_files[key] = handle
|
||||||
|
path, stream = handle
|
||||||
|
stream.seek(record.source_offset)
|
||||||
|
name_length = read_exact(stream, 1)[0]
|
||||||
|
name = read_exact(stream, name_length).decode("ascii")
|
||||||
|
ticks = struct.unpack("<q", read_exact(stream, 8))[0]
|
||||||
|
id_length = read_exact(stream, 1)[0]
|
||||||
|
id_bytes = read_exact(stream, id_length)
|
||||||
|
payload_length = struct.unpack("<i", read_exact(stream, 4))[0]
|
||||||
|
payload = read_exact(stream, payload_length)
|
||||||
|
try:
|
||||||
|
record_id = id_bytes.decode("ascii")
|
||||||
|
except UnicodeDecodeError:
|
||||||
|
record_id = id_bytes.hex().upper()
|
||||||
|
if name != record.object_name:
|
||||||
|
raise ValueError(f"name mismatch: log={record.object_name}, dorec={name}")
|
||||||
|
if ticks != record.dotnet_ticks:
|
||||||
|
raise ValueError(f"tick mismatch: log={record.dotnet_ticks}, dorec={ticks}")
|
||||||
|
if payload_length != record.payload_length:
|
||||||
|
raise ValueError(
|
||||||
|
f"payload mismatch: log={record.payload_length}, dorec={payload_length}"
|
||||||
|
)
|
||||||
|
if record_id.upper() != record.log_record_id.upper():
|
||||||
|
raise ValueError(
|
||||||
|
f"record id mismatch: log={record.log_record_id}, dorec={record_id}"
|
||||||
|
)
|
||||||
|
yield record, payload
|
||||||
|
finally:
|
||||||
|
for _path, stream in open_files.values():
|
||||||
|
stream.close()
|
||||||
@@ -0,0 +1,69 @@
|
|||||||
|
"""Little-endian .NET BinaryReader/BinaryWriter helpers."""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import struct
|
||||||
|
from typing import BinaryIO
|
||||||
|
|
||||||
|
|
||||||
|
def read_7bit_int(stream: BinaryIO) -> int:
|
||||||
|
value = 0
|
||||||
|
shift = 0
|
||||||
|
while True:
|
||||||
|
raw = stream.read(1)
|
||||||
|
if not raw:
|
||||||
|
raise EOFError("truncated .NET 7-bit int")
|
||||||
|
value |= (raw[0] & 0x7F) << shift
|
||||||
|
if not raw[0] & 0x80:
|
||||||
|
return value
|
||||||
|
shift += 7
|
||||||
|
if shift > 35:
|
||||||
|
raise ValueError("invalid .NET 7-bit int")
|
||||||
|
|
||||||
|
|
||||||
|
def write_7bit_int(stream: BinaryIO, value: int) -> None:
|
||||||
|
if value < 0:
|
||||||
|
raise ValueError("7-bit int must be non-negative")
|
||||||
|
while value >= 0x80:
|
||||||
|
stream.write(bytes([(value & 0x7F) | 0x80]))
|
||||||
|
value >>= 7
|
||||||
|
stream.write(bytes([value & 0x7F]))
|
||||||
|
|
||||||
|
|
||||||
|
def read_dotnet_string(stream: BinaryIO) -> str:
|
||||||
|
length = read_7bit_int(stream)
|
||||||
|
raw = stream.read(length)
|
||||||
|
if len(raw) != length:
|
||||||
|
raise EOFError("truncated .NET string")
|
||||||
|
return raw.decode("utf-8")
|
||||||
|
|
||||||
|
|
||||||
|
def write_dotnet_string(stream: BinaryIO, text: str) -> None:
|
||||||
|
raw = text.encode("utf-8")
|
||||||
|
write_7bit_int(stream, len(raw))
|
||||||
|
stream.write(raw)
|
||||||
|
|
||||||
|
|
||||||
|
def read_i32(stream: BinaryIO) -> int:
|
||||||
|
raw = stream.read(4)
|
||||||
|
if len(raw) != 4:
|
||||||
|
raise EOFError("truncated int32")
|
||||||
|
return struct.unpack("<i", raw)[0]
|
||||||
|
|
||||||
|
|
||||||
|
def read_i64(stream: BinaryIO) -> int:
|
||||||
|
raw = stream.read(8)
|
||||||
|
if len(raw) != 8:
|
||||||
|
raise EOFError("truncated int64")
|
||||||
|
return struct.unpack("<q", raw)[0]
|
||||||
|
|
||||||
|
|
||||||
|
def read_bool(stream: BinaryIO) -> bool:
|
||||||
|
raw = stream.read(1)
|
||||||
|
if not raw:
|
||||||
|
raise EOFError("truncated bool")
|
||||||
|
return raw[0] != 0
|
||||||
|
|
||||||
|
|
||||||
|
def write_bool(stream: BinaryIO, value: bool) -> None:
|
||||||
|
stream.write(b"\x01" if value else b"\x00")
|
||||||
@@ -0,0 +1,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,
|
||||||
|
)
|
||||||
@@ -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
@@ -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],
|
azimuth = normalize(-(block_az + horizontal[ch])), altitude = vertical[ch],
|
||||||
distance_mm = raw * distance_unit_mm, then:
|
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)
|
y = d_m * cos(alt) * sin(az)
|
||||||
z = d_m * sin(alt)
|
z = d_m * sin(alt)
|
||||||
|
|
||||||
MSOP-only captures do not include DIFOP; vertical angles default to a uniform
|
When DIFOP is unavailable, vertical angles default to a uniform -16°…+16° fan
|
||||||
-16°…+16° fan, horizontal channel offsets default to 0.
|
and horizontal channel offsets default to 0.
|
||||||
"""
|
"""
|
||||||
|
|
||||||
from __future__ import annotations
|
from __future__ import annotations
|
||||||
|
|
||||||
from dataclasses import dataclass
|
from dataclasses import dataclass
|
||||||
|
from typing import Iterable, Sequence
|
||||||
|
|
||||||
import numpy as np
|
import numpy as np
|
||||||
|
|
||||||
@@ -192,9 +193,10 @@ def _block_points_raw(
|
|||||||
return np.asarray(rows, dtype=np.float32)
|
return np.asarray(rows, dtype=np.float32)
|
||||||
|
|
||||||
|
|
||||||
def iter_h32_frames_polar(
|
def iter_h32_frames_polar_from_packets(
|
||||||
capture: CaptureFile,
|
packets: Iterable[bytes],
|
||||||
*,
|
*,
|
||||||
|
host_utc_ticks: Sequence[int] | None = None,
|
||||||
min_frame_points: int = MIN_FRAME_POINTS_DEFAULT,
|
min_frame_points: int = MIN_FRAME_POINTS_DEFAULT,
|
||||||
frame_stride: int = 1,
|
frame_stride: int = 1,
|
||||||
min_range_m: float = 0.3,
|
min_range_m: float = 0.3,
|
||||||
@@ -203,7 +205,7 @@ def iter_h32_frames_polar(
|
|||||||
vertical_deg: np.ndarray | None = None,
|
vertical_deg: np.ndarray | None = None,
|
||||||
horizontal_deg: np.ndarray | None = None,
|
horizontal_deg: np.ndarray | None = None,
|
||||||
) -> list[LidarFramePolarExport]:
|
) -> list[LidarFramePolarExport]:
|
||||||
"""Assemble MSOP packets into polar frames for the RTK–LiDAR 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)
|
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)
|
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
|
prev_az: float | None = None
|
||||||
kept = 0
|
kept = 0
|
||||||
stride = max(1, int(frame_stride))
|
stride = max(1, int(frame_stride))
|
||||||
|
host_list = list(host_utc_ticks) if host_utc_ticks is not None else None
|
||||||
|
|
||||||
def emit() -> None:
|
def emit() -> None:
|
||||||
nonlocal point_chunks, t_start, t_end, host_ns, kept
|
nonlocal point_chunks, t_start, t_end, host_ns, kept
|
||||||
@@ -250,13 +253,15 @@ def iter_h32_frames_polar(
|
|||||||
)
|
)
|
||||||
)
|
)
|
||||||
|
|
||||||
for chunk in capture.chunks:
|
for index, packet in enumerate(packets):
|
||||||
packet = chunk.raw
|
|
||||||
if len(packet) != PACKET_LENGTH:
|
if len(packet) != PACKET_LENGTH:
|
||||||
continue
|
continue
|
||||||
packet_t = device_timestamp_ms(packet) * 1e-3
|
packet_t = device_timestamp_ms(packet) * 1e-3
|
||||||
unit = distance_unit_mm(packet)
|
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
|
idx = DATA_START
|
||||||
for _block in range(BLOCKS):
|
for _block in range(BLOCKS):
|
||||||
if idx + BLOCK_LENGTH > PACKET_LENGTH or packet[idx] != 255 or packet[idx + 1] != 238:
|
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
|
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(
|
def iter_h32_frames(
|
||||||
capture: CaptureFile,
|
capture: CaptureFile,
|
||||||
*,
|
*,
|
||||||
|
|||||||
+4
-4
@@ -45,20 +45,20 @@ python -m pip install -r requirements.txt
|
|||||||
|
|
||||||
### 3.1 目录结构
|
### 3.1 目录结构
|
||||||
|
|
||||||
**新车(默认)**:每站一段 H32 雷达 `.rscap`,RTK/IMU 全程各一条:
|
**新车(默认)**:每站一段 H32 雷达 dlog(`RSLidarH32_3D_DLogCaptureNet48`,含 raw MSOP/DIFOP),RTK/IMU 全程各一条 `.rscap`:
|
||||||
|
|
||||||
```text
|
```text
|
||||||
raw_dataset/
|
raw_dataset/
|
||||||
├── stations/
|
├── stations/
|
||||||
│ ├── 001/h32.rscap
|
│ ├── 001/ # dobject/ + dobject_recording/
|
||||||
│ ├── 002/h32.rscap
|
│ ├── 002/
|
||||||
│ └── ...
|
│ └── ...
|
||||||
└── captures/
|
└── captures/
|
||||||
├── rtk.rscap # G90:#PVTSLNA 位置 + #UNIHEADINGA 航向
|
├── rtk.rscap # G90:#PVTSLNA 位置 + #UNIHEADINGA 航向
|
||||||
└── imu.rscap # N300;仅关联保存,不参与外参求解
|
└── imu.rscap # N300;仅关联保存,不参与外参求解
|
||||||
```
|
```
|
||||||
|
|
||||||
旧车 dlog 布局(`dobject/` + `dobject_recording/`)仍可被导出脚本识别;关联时间请用 `-TimeBasis host`。
|
仍兼容旧站 `h32.rscap`。仅含已解码点云、无 raw MSOP 的旧 dlog 仍可识别,关联时间请用 `-TimeBasis host`。
|
||||||
|
|
||||||
### 3.2 采集要求
|
### 3.2 采集要求
|
||||||
|
|
||||||
|
|||||||
Reference in New Issue
Block a user