117 lines
3.7 KiB
Python
117 lines
3.7 KiB
Python
"""Regression tests for G90 GNHPR parsing and host-time LiDAR association."""
|
|
|
|
from __future__ import annotations
|
|
|
|
import json
|
|
import sys
|
|
from pathlib import Path
|
|
|
|
import numpy as np
|
|
|
|
ROOT = Path(__file__).resolve().parents[1]
|
|
TOOLS = ROOT / "tools"
|
|
CODE = ROOT / "code"
|
|
sys.path.insert(0, str(TOOLS))
|
|
sys.path.insert(0, str(TOOLS / "rscap_v2"))
|
|
sys.path.insert(0, str(CODE))
|
|
|
|
from build_multisensor_npz import build_combined # noqa: E402
|
|
from pipeline_common import parse_gnhpr # noqa: E402
|
|
from rigorous_calibration import load_npz_xyz # noqa: E402
|
|
|
|
|
|
def test_parse_gnhpr_fixed_heading():
|
|
row = parse_gnhpr("$GNHPR,070411.40,354.7437,000.2518,000.0000,4,26,0.00,0999*58")
|
|
assert row["type"] == "GNHPR"
|
|
assert row["raw_heading_deg"] == 354.7437
|
|
assert row["pitch_deg"] == 0.2518
|
|
assert row["roll_deg"] == 0.0
|
|
assert row["heading_quality"] == 4
|
|
assert row["satellites"] == 26
|
|
assert row["heading_valid"] is True
|
|
|
|
|
|
def test_host_time_uses_lidar_receive_time_and_preserves_device_time(tmp_path: Path):
|
|
host_ns = 1_786_240_000_000_000_000
|
|
device_ns = 1_500_000_000_000_000_000
|
|
frame_dir = tmp_path / "lidar"
|
|
frame_dir.mkdir()
|
|
np.savez_compressed(
|
|
frame_dir / "frame.npz",
|
|
points=np.zeros((4, 4), dtype=np.float32),
|
|
unix_time_ns=np.asarray([device_ns], dtype=np.int64),
|
|
host_receive_utc_ns=np.asarray([host_ns], dtype=np.int64),
|
|
)
|
|
|
|
rtk = tmp_path / "rtk.jsonl"
|
|
rows = [
|
|
{
|
|
"type": "GGA",
|
|
"checksum_valid": True,
|
|
"host_receive_utc_ns": host_ns + 20_000_000,
|
|
"lat_deg": 31.0,
|
|
"lon_deg": 121.0,
|
|
"altitude_m": 10.0,
|
|
"fix_quality": 4,
|
|
"satellites": 20,
|
|
"raw_line": "$GNGGA,...",
|
|
},
|
|
{
|
|
"type": "GNHPR",
|
|
"checksum_valid": True,
|
|
"host_receive_utc_ns": host_ns - 10_000_000,
|
|
"raw_heading_deg": 90.0,
|
|
"pitch_deg": 1.0,
|
|
"roll_deg": 0.0,
|
|
"heading_quality": 4,
|
|
"heading_solution": "GNHPR_QUALITY_4",
|
|
"heading_valid": True,
|
|
"satellites": 22,
|
|
"raw_line": "$GNHPR,...",
|
|
},
|
|
]
|
|
rtk.write_text("".join(json.dumps(row) + "\n" for row in rows), encoding="utf-8")
|
|
imu = tmp_path / "imu.jsonl"
|
|
imu.write_text("", encoding="utf-8")
|
|
|
|
out = tmp_path / "combined"
|
|
summary = build_combined(
|
|
[("STATION-01", frame_dir)],
|
|
[rtk],
|
|
[imu],
|
|
out,
|
|
time_basis="host",
|
|
rtk_max_dt_ms=100.0,
|
|
)
|
|
|
|
assert summary["frames"] == 1
|
|
assert summary["rtk_valid"] == 1
|
|
assert summary["heading_valid"] == 1
|
|
assert summary["rtk_fixed"] == 1
|
|
with np.load(next((out / "frames").glob("*.npz")), allow_pickle=False) as frame:
|
|
assert int(frame["lidar_association_time_ns"][0]) == host_ns
|
|
assert int(frame["unix_time_ns"][0]) == device_ns
|
|
assert int(frame["rtk_gga_dt_ns"][0]) == 20_000_000
|
|
assert int(frame["rtk_heading_dt_ns"][0]) == -10_000_000
|
|
|
|
|
|
def test_registration_prefers_lidar_association_time(tmp_path: Path):
|
|
host_ns = 1_786_240_000_000_000_000
|
|
device_ns = 1_500_000_000_000_000_000
|
|
source = tmp_path / "frame.npz"
|
|
np.savez_compressed(
|
|
source,
|
|
points_raw=np.asarray(
|
|
[[1000.0, 0.0, 0.0, 1.0], [2000.0, 90.0, 0.0, 1.0]],
|
|
dtype=np.float32,
|
|
),
|
|
unix_time_ns=np.asarray([device_ns], dtype=np.int64),
|
|
lidar_association_time_ns=np.asarray([host_ns], dtype=np.int64),
|
|
frame_counter=np.asarray([7], dtype=np.int64),
|
|
)
|
|
|
|
timestamp, counter, xyz = load_npz_xyz(source)
|
|
assert timestamp == host_ns / 1e9
|
|
assert counter == 7
|
|
assert xyz.shape == (2, 3)
|