374 lines
14 KiB
Python
374 lines
14 KiB
Python
#!/usr/bin/env python3
|
||
"""One-shot export: raw H32/G90/N300 captures → RTK–LiDAR ``combined/`` package.
|
||
|
||
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 (new H32 DLogCapture):
|
||
|
||
stations/
|
||
001/ # dobject/ + dobject_recording/ (or 001/dlog/...)
|
||
002/
|
||
...
|
||
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``:
|
||
|
||
export/<station>/frames/*.npz # internal LiDAR frames
|
||
parsed/rtk.jsonl, imu.jsonl
|
||
combined/frames/*.npz + manifest.csv + dataset_summary.json
|
||
export_summary.json
|
||
|
||
Raw ``.rscap`` / dlog files are never modified.
|
||
"""
|
||
|
||
from __future__ import annotations
|
||
|
||
import argparse
|
||
import json
|
||
import shutil
|
||
import subprocess
|
||
import sys
|
||
from pathlib import Path
|
||
from typing import Any
|
||
|
||
ROOT = Path(__file__).resolve().parent
|
||
REPO = ROOT.parent
|
||
sys.path.insert(0, str(ROOT))
|
||
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 ( # 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,
|
||
write_json,
|
||
write_jsonl,
|
||
)
|
||
|
||
|
||
def is_h32_station(station: Path, capture_name: str) -> bool:
|
||
try:
|
||
resolve_lidar_rscap(station, capture_name)
|
||
except FileNotFoundError:
|
||
return False
|
||
return True
|
||
|
||
|
||
def is_dlog_station(station: Path) -> bool:
|
||
return try_resolve_dlog_root(station) is not None
|
||
|
||
|
||
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()]
|
||
if missing:
|
||
raise FileNotFoundError(f"station directories missing: {missing}")
|
||
return stations
|
||
stations = sorted(
|
||
[
|
||
path
|
||
for path in stations_root.iterdir()
|
||
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 H32 dlog/MSOP, {capture_name}/lidar.rscap, or "
|
||
f"dobject+dobject_recording under {stations_root}"
|
||
)
|
||
return stations
|
||
|
||
|
||
def export_legacy_dlog_station(
|
||
station: Path,
|
||
out: Path,
|
||
*,
|
||
lidar_object: str,
|
||
timezone: str,
|
||
stride: int,
|
||
) -> None:
|
||
exporter = ROOT / "frontlidar_dlog_export.py"
|
||
command = [
|
||
sys.executable,
|
||
str(exporter),
|
||
"--dlog",
|
||
str(try_resolve_dlog_root(station) or station),
|
||
"--out",
|
||
str(out),
|
||
"--object",
|
||
lidar_object,
|
||
"--format",
|
||
"npz",
|
||
"--timezone",
|
||
timezone,
|
||
"--stride",
|
||
str(stride),
|
||
"--compress",
|
||
"--skip-rtk",
|
||
"--write-reports",
|
||
"--resume",
|
||
]
|
||
completed = subprocess.run(command, check=False)
|
||
if completed.returncode != 0:
|
||
raise RuntimeError(f"legacy dlog export failed for {station} (exit {completed.returncode})")
|
||
|
||
|
||
def parse_serial(rtk_rscap: Path, imu_rscap: Path, parsed_root: Path) -> dict[str, Any]:
|
||
parsed_root.mkdir(parents=True, exist_ok=True)
|
||
rtk_capture = read_capture(rtk_rscap)
|
||
imu_capture = read_capture(imu_rscap)
|
||
rtk_rows = parse_rtk_capture(rtk_capture)
|
||
imu_rows = parse_imu_capture(imu_capture)
|
||
write_jsonl(parsed_root / "rtk.jsonl", rtk_rows)
|
||
write_jsonl(parsed_root / "imu.jsonl", imu_rows)
|
||
summary = {
|
||
"rtk_capture": file_summary(rtk_capture),
|
||
"imu_capture": file_summary(imu_capture),
|
||
"rtk_records": len(rtk_rows),
|
||
"rtk_checksum_valid": sum(bool(row.get("checksum_valid")) for row in rtk_rows),
|
||
"rtk_pvtslna": sum(row.get("type") == "PVTSLNA" and row.get("checksum_valid") for row in rtk_rows),
|
||
"rtk_gga": sum(row.get("type") == "GGA" and row.get("checksum_valid") for row in rtk_rows),
|
||
"rtk_heading_valid": sum(row.get("type") == "UNIHEADINGA" and row.get("heading_valid") for row in rtk_rows),
|
||
"imu_frames": len(imu_rows),
|
||
"imu_crc_valid": sum(bool(row.get("crc_valid")) for row in imu_rows),
|
||
"imu_types": sorted({str(row.get("type")) for row in imu_rows}),
|
||
}
|
||
write_json(parsed_root / "parse_summary.json", summary)
|
||
return summary
|
||
|
||
|
||
def export_raw_to_combined(
|
||
*,
|
||
stations_root: Path,
|
||
rtk_rscap: Path,
|
||
imu_rscap: Path,
|
||
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,
|
||
rtk_max_dt_ms: float = 150.0,
|
||
imu_before_ms: float = 100.0,
|
||
imu_after_ms: float = 100.0,
|
||
time_basis: str = "device_gnss",
|
||
overwrite: bool = False,
|
||
) -> dict[str, Any]:
|
||
"""Full raw → combined export. Returns ``export_summary`` dict."""
|
||
|
||
if not stations_root.is_dir():
|
||
raise FileNotFoundError(f"stations root does not exist: {stations_root}")
|
||
if not rtk_rscap.is_file():
|
||
raise FileNotFoundError(f"RTK capture missing: {rtk_rscap}")
|
||
if not imu_rscap.is_file():
|
||
raise FileNotFoundError(f"IMU capture missing: {imu_rscap}")
|
||
if out.exists() and any(out.iterdir()) and not overwrite:
|
||
raise FileExistsError(f"{out} is non-empty; pass --overwrite")
|
||
if overwrite and out.exists():
|
||
for child in ("export", "parsed", "combined", "export_summary.json", "capture_audit.json"):
|
||
target = out / child
|
||
if target.is_dir():
|
||
shutil.rmtree(target)
|
||
elif target.is_file():
|
||
target.unlink()
|
||
|
||
out.mkdir(parents=True, exist_ok=True)
|
||
export_root = out / "export"
|
||
parsed_root = out / "parsed"
|
||
combined_root = out / "combined"
|
||
|
||
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_legacy_dlog = False
|
||
for station in stations:
|
||
station_out = export_root / station.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,
|
||
capture_name=lidar_capture_name,
|
||
stride=stride,
|
||
write_reports=True,
|
||
resume=False,
|
||
)
|
||
kind = "h32_rscap"
|
||
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,
|
||
lidar_object=lidar_object,
|
||
timezone=timezone,
|
||
stride=stride,
|
||
)
|
||
meta = {"source": str(station.resolve()), "kind": "legacy_dlog"}
|
||
kind = "legacy_dlog"
|
||
else:
|
||
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_legacy_dlog and time_basis == "device_gnss":
|
||
print(
|
||
"[warn] legacy point-cloud dlog stations use host/DObject time; prefer --time-basis host",
|
||
file=sys.stderr,
|
||
)
|
||
|
||
combined_summary = build_combined(
|
||
lidar_segments,
|
||
[parsed_root / "rtk.jsonl"],
|
||
[parsed_root / "imu.jsonl"],
|
||
combined_root,
|
||
rtk_max_dt_ms=rtk_max_dt_ms,
|
||
imu_before_ms=imu_before_ms,
|
||
imu_after_ms=imu_after_ms,
|
||
time_basis=time_basis,
|
||
overwrite=True,
|
||
)
|
||
|
||
summary = {
|
||
"role": "RTK-LiDAR one-shot raw export (like Lidar-IMU export_rscap_to_v1)",
|
||
"stations_root": str(stations_root.resolve()),
|
||
"rtk_rscap": str(rtk_rscap.resolve()),
|
||
"imu_rscap": str(imu_rscap.resolve()),
|
||
"out": str(out.resolve()),
|
||
"station_count": len(stations),
|
||
"stations": station_meta,
|
||
"parsed": parse_summary,
|
||
"combined": combined_summary,
|
||
"outputs": {
|
||
"combined": str(combined_root.resolve()),
|
||
"manifest": str((combined_root / "manifest.csv").resolve()),
|
||
"parsed": str(parsed_root.resolve()),
|
||
"export": str(export_root.resolve()),
|
||
},
|
||
"timestamp_policy": {
|
||
"default_time_basis": time_basis,
|
||
"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",
|
||
},
|
||
"next_step": "run/run_direct_rtk_lidar.ps1 -CombinedRoot <out>/combined ...",
|
||
}
|
||
(out / "export_summary.json").write_text(
|
||
json.dumps(summary, ensure_ascii=False, indent=2) + "\n",
|
||
encoding="utf-8",
|
||
)
|
||
return summary
|
||
|
||
|
||
def parse_args() -> argparse.Namespace:
|
||
parser = argparse.ArgumentParser(description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter)
|
||
parser.add_argument("--stations-root", type=Path, required=True, help="Directory of per-station folders")
|
||
parser.add_argument("--rtk-rscap", type=Path, required=True, help="Continuous G90/RTK 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("--station", action="append", default=[], help="Optional station name filter; repeatable")
|
||
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)
|
||
parser.add_argument("--imu-before-ms", type=float, default=100.0)
|
||
parser.add_argument("--imu-after-ms", type=float, default=100.0)
|
||
parser.add_argument("--time-basis", choices=("device_gnss", "host"), default="device_gnss")
|
||
parser.add_argument("--overwrite", action="store_true")
|
||
return parser.parse_args()
|
||
|
||
|
||
def main() -> int:
|
||
args = parse_args()
|
||
if args.stride < 1:
|
||
raise SystemExit("stride must be >= 1")
|
||
summary = export_raw_to_combined(
|
||
stations_root=args.stations_root,
|
||
rtk_rscap=args.rtk_rscap,
|
||
imu_rscap=args.imu_rscap,
|
||
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,
|
||
rtk_max_dt_ms=args.rtk_max_dt_ms,
|
||
imu_before_ms=args.imu_before_ms,
|
||
imu_after_ms=args.imu_after_ms,
|
||
time_basis=args.time_basis,
|
||
overwrite=args.overwrite,
|
||
)
|
||
print(json.dumps(summary, ensure_ascii=False, indent=2))
|
||
print(f"\nCombined package ready: {summary['outputs']['combined']}")
|
||
return 0
|
||
|
||
|
||
if __name__ == "__main__":
|
||
raise SystemExit(main())
|