1024 lines
38 KiB
Python
1024 lines
38 KiB
Python
from __future__ import annotations
|
|
|
|
import hashlib
|
|
import json
|
|
import math
|
|
import os
|
|
import re
|
|
import shutil
|
|
import time
|
|
from dataclasses import dataclass
|
|
from datetime import UTC, datetime
|
|
from pathlib import Path
|
|
from typing import Any, Final
|
|
|
|
import numpy as np
|
|
import numpy.typing as npt
|
|
|
|
from k1link.data_plane import (
|
|
ConsumerFrameContext,
|
|
DecodedPointCloudView,
|
|
DecodedPoseView,
|
|
)
|
|
from k1link.device_plugins.xgrids_k1.protocol.streams import (
|
|
LioPointCloudFrame,
|
|
LioPoseFrame,
|
|
decode_lio_pcl,
|
|
decode_lio_pose,
|
|
)
|
|
from k1link.device_plugins.xgrids_k1.viewer.messages import StreamMessage
|
|
from k1link.device_plugins.xgrids_k1.viewer.replay import iter_replay_messages
|
|
|
|
from .lidar_contract import (
|
|
K1_LIDAR_PACK_V2_PROFILE,
|
|
LidarEvidenceProfile,
|
|
lidar_readiness_document,
|
|
)
|
|
|
|
LIDAR_REPLAY_PACK_SCHEMA: Final = "missioncore.lidar-replay-pack/v2"
|
|
LIDAR_QUALITY_REPORT_SCHEMA: Final = "missioncore.lidar-quality-report/v1"
|
|
LIDAR_EQUIVALENCE_REPORT_SCHEMA: Final = (
|
|
"missioncore.lidar-live-replay-equivalence/v1"
|
|
)
|
|
LIDAR_REPLAY_ARRAYS_NAME: Final = "lidar-replay.npz"
|
|
LIDAR_QUALITY_REPORT_NAME: Final = "quality-report.json"
|
|
LIDAR_EQUIVALENCE_REPORT_NAME: Final = "equivalence-report.json"
|
|
LIDAR_MANIFEST_NAME: Final = "manifest.json"
|
|
DEFAULT_POSE_COVERAGE_THRESHOLD_MS: Final = 100.0
|
|
|
|
_PACK_ID = re.compile(r"^lidar-replay-pack-[a-f0-9]{64}$")
|
|
_SAFE_SESSION_ID = re.compile(r"^[A-Za-z0-9][A-Za-z0-9._-]{0,127}$")
|
|
_POINT_TOPIC_SUFFIX = "/lio_pcl"
|
|
_POSE_TOPIC_SUFFIX = "/lio_pose"
|
|
|
|
_ARRAY_DTYPES: Final[dict[str, np.dtype[Any]]] = {
|
|
"point_capture_sequence": np.dtype("<i8"),
|
|
"point_payload_bytes": np.dtype("<i8"),
|
|
"point_received_at_epoch_ns": np.dtype("<i8"),
|
|
"point_received_monotonic_ns": np.dtype("<i8"),
|
|
"point_header_seq": np.dtype("<u8"),
|
|
"point_header_stamp": np.dtype("<i8"),
|
|
"point_scaler": np.dtype("<i8"),
|
|
"point_offsets": np.dtype("<i8"),
|
|
"point_raw_xyz": np.dtype("<i8"),
|
|
"point_xyz_map": np.dtype("<f8"),
|
|
"point_rgbi": np.dtype("<u4"),
|
|
"point_intensity": np.dtype("u1"),
|
|
"pose_capture_sequence": np.dtype("<i8"),
|
|
"pose_payload_bytes": np.dtype("<i8"),
|
|
"pose_received_at_epoch_ns": np.dtype("<i8"),
|
|
"pose_received_monotonic_ns": np.dtype("<i8"),
|
|
"pose_header_seq": np.dtype("<u8"),
|
|
"pose_header_stamp": np.dtype("<i8"),
|
|
"pose_header_scaler": np.dtype("<i8"),
|
|
"pose_stamp": np.dtype("<i8"),
|
|
"pose_positions_map": np.dtype("<f8"),
|
|
"pose_quaternions_map_from_lidar": np.dtype("<f8"),
|
|
"pose_distance": np.dtype("<f8"),
|
|
"pose_accuracy": np.dtype("<f8"),
|
|
}
|
|
|
|
IntArray = npt.NDArray[np.integer[Any]]
|
|
FloatArray = npt.NDArray[np.floating[Any]]
|
|
|
|
|
|
class LidarReplayError(ValueError):
|
|
"""A replay pack or its source evidence violates the v2 contract."""
|
|
|
|
|
|
@dataclass(frozen=True, slots=True)
|
|
class LidarReplayPointFrame:
|
|
capture_sequence: int
|
|
payload_bytes: int
|
|
received_at_epoch_ns: int
|
|
received_monotonic_ns: int
|
|
header_seq: int
|
|
header_stamp: int
|
|
scaler: int
|
|
raw_xyz: npt.NDArray[np.int64]
|
|
xyz_map: npt.NDArray[np.float64]
|
|
rgbi: npt.NDArray[np.uint32]
|
|
intensity: npt.NDArray[np.uint8]
|
|
|
|
def decoded_view(self) -> DecodedPointCloudView:
|
|
return DecodedPointCloudView(
|
|
context=ConsumerFrameContext(
|
|
sequence=self.capture_sequence,
|
|
captured_at_epoch_ns=self.received_at_epoch_ns,
|
|
received_monotonic_ns=self.received_monotonic_ns,
|
|
processing_started_monotonic_ns=time.monotonic_ns(),
|
|
encoded_size_bytes=self.payload_bytes,
|
|
live=False,
|
|
),
|
|
frame_id="map",
|
|
positions_xyz=tuple(
|
|
(float(point[0]), float(point[1]), float(point[2]))
|
|
for point in self.xyz_map
|
|
),
|
|
intensities=self.intensity.tobytes(),
|
|
)
|
|
|
|
|
|
@dataclass(frozen=True, slots=True)
|
|
class LidarReplayPoseFrame:
|
|
capture_sequence: int
|
|
payload_bytes: int
|
|
received_at_epoch_ns: int
|
|
received_monotonic_ns: int
|
|
header_seq: int
|
|
header_stamp: int
|
|
header_scaler: int
|
|
pose_stamp: int
|
|
position_map: tuple[float, float, float]
|
|
orientation_map_from_lidar: tuple[float, float, float, float]
|
|
distance: float
|
|
pose_accuracy: float
|
|
|
|
def decoded_view(self) -> DecodedPoseView:
|
|
return DecodedPoseView(
|
|
context=ConsumerFrameContext(
|
|
sequence=self.capture_sequence,
|
|
captured_at_epoch_ns=self.received_at_epoch_ns,
|
|
received_monotonic_ns=self.received_monotonic_ns,
|
|
processing_started_monotonic_ns=time.monotonic_ns(),
|
|
encoded_size_bytes=self.payload_bytes,
|
|
live=False,
|
|
),
|
|
frame_id="map",
|
|
child_frame_id="lidar",
|
|
position_xyz=self.position_map,
|
|
orientation_xyzw=self.orientation_map_from_lidar,
|
|
)
|
|
|
|
|
|
class LidarReplayPackV2:
|
|
"""Strict reader for immutable, field-retaining K1 LiDAR replay evidence."""
|
|
|
|
def __init__(self, root: Path) -> None:
|
|
candidate = root.expanduser().absolute()
|
|
if candidate.is_symlink():
|
|
raise LidarReplayError("LiDAR replay pack directory cannot be a symlink")
|
|
self.root = candidate.resolve(strict=True)
|
|
if not self.root.is_dir() or _PACK_ID.fullmatch(self.root.name) is None:
|
|
raise LidarReplayError("LiDAR replay pack directory id is invalid")
|
|
manifest_path = self.root / LIDAR_MANIFEST_NAME
|
|
self.manifest = _read_json_object(manifest_path)
|
|
self.identity = _object(self.manifest.get("identity"), "LiDAR replay identity")
|
|
identity_sha256 = self.manifest.get("identity_sha256")
|
|
if (
|
|
self.manifest.get("schema_version") != LIDAR_REPLAY_PACK_SCHEMA
|
|
or self.identity.get("schema_version") != LIDAR_REPLAY_PACK_SCHEMA
|
|
or not isinstance(identity_sha256, str)
|
|
or hashlib.sha256(_canonical_json(self.identity)).hexdigest()
|
|
!= identity_sha256
|
|
or self.root.name != f"lidar-replay-pack-{identity_sha256}"
|
|
or self.manifest.get("pack_id") != self.root.name
|
|
):
|
|
raise LidarReplayError("LiDAR replay pack identity is invalid")
|
|
self.pack_id = self.root.name
|
|
artifacts = _validate_artifacts(self.root, self.manifest.get("artifacts"))
|
|
self.arrays_path = artifacts["lidar-arrays"]
|
|
self.quality_path = artifacts["lidar-quality"]
|
|
self.equivalence_path = artifacts["live-replay-equivalence"]
|
|
self.arrays = np.load(self.arrays_path, allow_pickle=False)
|
|
if set(self.arrays.files) != set(_ARRAY_DTYPES):
|
|
self.close()
|
|
raise LidarReplayError("LiDAR replay array set is incompatible")
|
|
try:
|
|
_validate_arrays(self.arrays, self.identity)
|
|
logical_sha256 = _logical_content_sha256(self.arrays)
|
|
if logical_sha256 != self.identity.get("logical_content_sha256"):
|
|
raise LidarReplayError("LiDAR replay logical content identity changed")
|
|
self.profile = LidarEvidenceProfile.from_dict(
|
|
self.identity.get("lidar_evidence_profile")
|
|
)
|
|
if self.profile != K1_LIDAR_PACK_V2_PROFILE:
|
|
raise LidarReplayError("LiDAR replay evidence profile is incompatible")
|
|
self.quality = _read_json_object(self.quality_path)
|
|
self.equivalence = _read_json_object(self.equivalence_path)
|
|
_validate_reports(
|
|
self.quality,
|
|
self.equivalence,
|
|
self.pack_id,
|
|
logical_sha256,
|
|
)
|
|
except BaseException:
|
|
self.close()
|
|
raise
|
|
|
|
@property
|
|
def point_frame_count(self) -> int:
|
|
return int(self.arrays["point_capture_sequence"].shape[0])
|
|
|
|
@property
|
|
def pose_frame_count(self) -> int:
|
|
return int(self.arrays["pose_capture_sequence"].shape[0])
|
|
|
|
@property
|
|
def point_count(self) -> int:
|
|
return int(self.arrays["point_raw_xyz"].shape[0])
|
|
|
|
def close(self) -> None:
|
|
self.arrays.close()
|
|
|
|
def point_frame(self, index: int) -> LidarReplayPointFrame:
|
|
if not 0 <= index < self.point_frame_count:
|
|
raise IndexError(index)
|
|
start = int(self.arrays["point_offsets"][index])
|
|
end = int(self.arrays["point_offsets"][index + 1])
|
|
return LidarReplayPointFrame(
|
|
capture_sequence=int(self.arrays["point_capture_sequence"][index]),
|
|
payload_bytes=int(self.arrays["point_payload_bytes"][index]),
|
|
received_at_epoch_ns=int(
|
|
self.arrays["point_received_at_epoch_ns"][index]
|
|
),
|
|
received_monotonic_ns=int(
|
|
self.arrays["point_received_monotonic_ns"][index]
|
|
),
|
|
header_seq=int(self.arrays["point_header_seq"][index]),
|
|
header_stamp=int(self.arrays["point_header_stamp"][index]),
|
|
scaler=int(self.arrays["point_scaler"][index]),
|
|
raw_xyz=np.asarray(self.arrays["point_raw_xyz"][start:end], dtype=np.int64),
|
|
xyz_map=np.asarray(self.arrays["point_xyz_map"][start:end], dtype=np.float64),
|
|
rgbi=np.asarray(self.arrays["point_rgbi"][start:end], dtype=np.uint32),
|
|
intensity=np.asarray(
|
|
self.arrays["point_intensity"][start:end],
|
|
dtype=np.uint8,
|
|
),
|
|
)
|
|
|
|
def pose_frame(self, index: int) -> LidarReplayPoseFrame:
|
|
if not 0 <= index < self.pose_frame_count:
|
|
raise IndexError(index)
|
|
position = self.arrays["pose_positions_map"][index]
|
|
orientation = self.arrays["pose_quaternions_map_from_lidar"][index]
|
|
return LidarReplayPoseFrame(
|
|
capture_sequence=int(self.arrays["pose_capture_sequence"][index]),
|
|
payload_bytes=int(self.arrays["pose_payload_bytes"][index]),
|
|
received_at_epoch_ns=int(
|
|
self.arrays["pose_received_at_epoch_ns"][index]
|
|
),
|
|
received_monotonic_ns=int(
|
|
self.arrays["pose_received_monotonic_ns"][index]
|
|
),
|
|
header_seq=int(self.arrays["pose_header_seq"][index]),
|
|
header_stamp=int(self.arrays["pose_header_stamp"][index]),
|
|
header_scaler=int(self.arrays["pose_header_scaler"][index]),
|
|
pose_stamp=int(self.arrays["pose_stamp"][index]),
|
|
position_map=(
|
|
float(position[0]),
|
|
float(position[1]),
|
|
float(position[2]),
|
|
),
|
|
orientation_map_from_lidar=(
|
|
float(orientation[0]),
|
|
float(orientation[1]),
|
|
float(orientation[2]),
|
|
float(orientation[3]),
|
|
),
|
|
distance=float(self.arrays["pose_distance"][index]),
|
|
pose_accuracy=float(self.arrays["pose_accuracy"][index]),
|
|
)
|
|
|
|
|
|
def build_lidar_replay_pack_v2(
|
|
capture_path: Path,
|
|
output_root: Path,
|
|
*,
|
|
session_id: str | None = None,
|
|
pose_coverage_threshold_ms: float = DEFAULT_POSE_COVERAGE_THRESHOLD_MS,
|
|
) -> Path:
|
|
"""Materialize a content-bound replay without mutating the source capture."""
|
|
|
|
source = capture_path.expanduser().resolve(strict=True)
|
|
if source.name != "mqtt.raw.k1mqtt" or not source.is_file():
|
|
raise LidarReplayError("LiDAR replay source must be mqtt.raw.k1mqtt")
|
|
metadata = source.with_name("mqtt.metadata.jsonl")
|
|
if not metadata.is_file():
|
|
raise LidarReplayError("exact host timing requires mqtt.metadata.jsonl")
|
|
if (
|
|
not math.isfinite(pose_coverage_threshold_ms)
|
|
or not 0 < pose_coverage_threshold_ms <= 10_000
|
|
):
|
|
raise LidarReplayError("pose coverage threshold is invalid")
|
|
resolved_session_id = session_id or _infer_session_id(source)
|
|
if _SAFE_SESSION_ID.fullmatch(resolved_session_id) is None:
|
|
raise LidarReplayError("LiDAR replay session id is unsafe")
|
|
|
|
arrays = _capture_arrays(source)
|
|
logical_sha256 = _logical_content_sha256(arrays)
|
|
source_evidence = {
|
|
"raw": _artifact_identity(source),
|
|
"metadata": _artifact_identity(metadata),
|
|
}
|
|
clock_origin = source.with_name("mqtt.timeline.origin.json")
|
|
if clock_origin.is_file():
|
|
source_evidence["clock_origin"] = _artifact_identity(clock_origin)
|
|
identity = {
|
|
"schema_version": LIDAR_REPLAY_PACK_SCHEMA,
|
|
"session_id": resolved_session_id,
|
|
"source_evidence": source_evidence,
|
|
"lidar_evidence_profile": K1_LIDAR_PACK_V2_PROFILE.to_dict(),
|
|
"field_retention": {
|
|
"point": [
|
|
"capture-sequence",
|
|
"payload-bytes",
|
|
"received-at-epoch-ns",
|
|
"received-monotonic-ns",
|
|
"header-seq",
|
|
"header-stamp",
|
|
"header-scaler",
|
|
"raw-xyz",
|
|
"xyz-map",
|
|
"raw-rgbi",
|
|
"intensity-low-byte",
|
|
],
|
|
"pose": [
|
|
"capture-sequence",
|
|
"payload-bytes",
|
|
"received-at-epoch-ns",
|
|
"received-monotonic-ns",
|
|
"header-seq",
|
|
"header-stamp",
|
|
"header-scaler",
|
|
"pose-stamp",
|
|
"position-map",
|
|
"orientation-map-from-lidar",
|
|
"distance",
|
|
"pose-accuracy",
|
|
],
|
|
},
|
|
"point_frame_count": int(arrays["point_capture_sequence"].shape[0]),
|
|
"pose_frame_count": int(arrays["pose_capture_sequence"].shape[0]),
|
|
"point_count": int(arrays["point_raw_xyz"].shape[0]),
|
|
"logical_content_sha256": logical_sha256,
|
|
"producer_sha256": _sha256(Path(__file__).resolve(strict=True)),
|
|
"authority": {
|
|
"commands_enabled": False,
|
|
"navigation_or_safety_accepted": False,
|
|
},
|
|
}
|
|
identity_sha256 = hashlib.sha256(_canonical_json(identity)).hexdigest()
|
|
pack_id = f"lidar-replay-pack-{identity_sha256}"
|
|
parent = output_root.expanduser().resolve()
|
|
parent.mkdir(mode=0o700, parents=True, exist_ok=True)
|
|
output = parent / pack_id
|
|
if output.exists():
|
|
existing = LidarReplayPackV2(output)
|
|
existing.close()
|
|
return output
|
|
|
|
staging = parent / f".{pack_id}.{os.getpid()}.incomplete"
|
|
staging.mkdir(mode=0o700, exist_ok=False)
|
|
try:
|
|
arrays_path = staging / LIDAR_REPLAY_ARRAYS_NAME
|
|
np.savez_compressed(arrays_path, **arrays) # type: ignore[arg-type]
|
|
quality = _quality_report(
|
|
pack_id,
|
|
logical_sha256,
|
|
arrays,
|
|
pose_coverage_threshold_ms=pose_coverage_threshold_ms,
|
|
)
|
|
_write_json(staging / LIDAR_QUALITY_REPORT_NAME, quality)
|
|
equivalence = _equivalence_report(
|
|
pack_id,
|
|
logical_sha256,
|
|
source,
|
|
arrays,
|
|
)
|
|
_write_json(staging / LIDAR_EQUIVALENCE_REPORT_NAME, equivalence)
|
|
artifacts = [
|
|
_artifact_descriptor("lidar-arrays", arrays_path, "application/x-npz"),
|
|
_artifact_descriptor(
|
|
"lidar-quality",
|
|
staging / LIDAR_QUALITY_REPORT_NAME,
|
|
"application/json",
|
|
),
|
|
_artifact_descriptor(
|
|
"live-replay-equivalence",
|
|
staging / LIDAR_EQUIVALENCE_REPORT_NAME,
|
|
"application/json",
|
|
),
|
|
]
|
|
manifest = {
|
|
"schema_version": LIDAR_REPLAY_PACK_SCHEMA,
|
|
"pack_id": pack_id,
|
|
"identity_sha256": identity_sha256,
|
|
"identity": identity,
|
|
"created_at_utc": datetime.now(UTC)
|
|
.isoformat(timespec="milliseconds")
|
|
.replace("+00:00", "Z"),
|
|
"classification": "private-recorded-sensor-replay-input",
|
|
"ground_truth": False,
|
|
"artifacts": artifacts,
|
|
}
|
|
_write_json(staging / LIDAR_MANIFEST_NAME, manifest)
|
|
os.replace(staging, output)
|
|
try:
|
|
validation = LidarReplayPackV2(output)
|
|
validation.close()
|
|
except BaseException:
|
|
shutil.rmtree(output, ignore_errors=True)
|
|
raise
|
|
except BaseException:
|
|
shutil.rmtree(staging, ignore_errors=True)
|
|
raise
|
|
return output
|
|
|
|
|
|
def verify_lidar_replay_equivalence(
|
|
capture_path: Path,
|
|
pack: LidarReplayPackV2,
|
|
) -> dict[str, object]:
|
|
"""Re-run the exact source-to-pack comparison for an accepted pack."""
|
|
|
|
source = capture_path.expanduser().resolve(strict=True)
|
|
source_identity = _object(
|
|
pack.identity.get("source_evidence"),
|
|
"LiDAR source evidence",
|
|
)
|
|
raw_identity = _object(source_identity.get("raw"), "LiDAR raw evidence")
|
|
if (
|
|
raw_identity.get("sha256") != _sha256(source)
|
|
or raw_identity.get("byte_length") != source.stat().st_size
|
|
):
|
|
raise LidarReplayError("LiDAR equivalence source differs from pack identity")
|
|
return _equivalence_report(
|
|
pack.pack_id,
|
|
str(pack.identity["logical_content_sha256"]),
|
|
source,
|
|
pack.arrays,
|
|
)
|
|
|
|
|
|
def lidar_pack_catalog_item(pack: LidarReplayPackV2) -> dict[str, object]:
|
|
point_count = _object(pack.quality.get("point_count_per_frame"), "point count")
|
|
cadence = _object(pack.quality.get("point_frame_interval_ms"), "point cadence")
|
|
pose = _object(pack.quality.get("pose_binding"), "pose binding")
|
|
return {
|
|
"pack_id": pack.pack_id,
|
|
"session_id": pack.identity["session_id"],
|
|
"profile_id": pack.profile.profile_id,
|
|
"point_frames": pack.point_frame_count,
|
|
"pose_frames": pack.pose_frame_count,
|
|
"points": pack.point_count,
|
|
"mean_points_per_frame": point_count.get("mean"),
|
|
"p95_frame_interval_ms": cadence.get("p95"),
|
|
"pose_coverage_fraction": pose.get("coverage_fraction"),
|
|
"equivalence_status": pack.equivalence.get("status"),
|
|
"logical_content_sha256": pack.identity["logical_content_sha256"],
|
|
"authority": {
|
|
"commands_enabled": False,
|
|
"navigation_or_safety_accepted": False,
|
|
},
|
|
}
|
|
|
|
|
|
def lidar_pack_detail(pack: LidarReplayPackV2) -> dict[str, object]:
|
|
return {
|
|
"schema_version": "missioncore.lidar-replay-pack-detail/v1",
|
|
"pack": lidar_pack_catalog_item(pack),
|
|
"quality": pack.quality,
|
|
"equivalence": pack.equivalence,
|
|
"readiness": lidar_readiness_document(pack.profile),
|
|
"access": "read-only",
|
|
}
|
|
|
|
|
|
def _capture_arrays(path: Path) -> dict[str, npt.NDArray[Any]]:
|
|
point_messages: list[tuple[StreamMessage, LioPointCloudFrame]] = []
|
|
pose_messages: list[tuple[StreamMessage, LioPoseFrame]] = []
|
|
for message in iter_replay_messages(path):
|
|
if message.source != "k1mqtt" or message.received_monotonic_ns is None:
|
|
raise LidarReplayError("LiDAR v2 requires native capture with exact host time")
|
|
if message.topic.endswith(_POINT_TOPIC_SUFFIX):
|
|
point_messages.append((message, decode_lio_pcl(message.payload)))
|
|
elif message.topic.endswith(_POSE_TOPIC_SUFFIX):
|
|
pose_messages.append((message, decode_lio_pose(message.payload)))
|
|
if not point_messages:
|
|
raise LidarReplayError("LiDAR replay source contains no lio_pcl frames")
|
|
|
|
point_offsets = [0]
|
|
point_raw: list[npt.NDArray[np.int64]] = []
|
|
point_xyz: list[npt.NDArray[np.float64]] = []
|
|
point_rgbi: list[npt.NDArray[np.uint32]] = []
|
|
point_intensity: list[npt.NDArray[np.uint8]] = []
|
|
for _, frame in point_messages:
|
|
raw = np.asarray(
|
|
[(point.x_raw, point.y_raw, point.z_raw) for point in frame.points],
|
|
dtype="<i8",
|
|
).reshape((-1, 3))
|
|
rgbi = np.asarray([point.rgbi for point in frame.points], dtype="<u4")
|
|
intensity = (rgbi & np.uint32(0xFF)).astype(np.uint8)
|
|
xyz = raw.astype(np.float64) / float(frame.header.scaler)
|
|
point_raw.append(raw)
|
|
point_xyz.append(xyz)
|
|
point_rgbi.append(rgbi)
|
|
point_intensity.append(intensity)
|
|
point_offsets.append(point_offsets[-1] + raw.shape[0])
|
|
|
|
arrays: dict[str, npt.NDArray[Any]] = {
|
|
"point_capture_sequence": _message_int_array(point_messages, "sequence"),
|
|
"point_payload_bytes": np.asarray(
|
|
[len(message.payload) for message, _ in point_messages],
|
|
dtype="<i8",
|
|
),
|
|
"point_received_at_epoch_ns": _message_int_array(
|
|
point_messages,
|
|
"received_at_epoch_ns",
|
|
),
|
|
"point_received_monotonic_ns": np.asarray(
|
|
[message.received_monotonic_ns for message, _ in point_messages],
|
|
dtype="<i8",
|
|
),
|
|
"point_header_seq": np.asarray(
|
|
[frame.header.seq for _, frame in point_messages],
|
|
dtype="<u8",
|
|
),
|
|
"point_header_stamp": np.asarray(
|
|
[frame.header.stamp for _, frame in point_messages],
|
|
dtype="<i8",
|
|
),
|
|
"point_scaler": np.asarray(
|
|
[frame.header.scaler for _, frame in point_messages],
|
|
dtype="<i8",
|
|
),
|
|
"point_offsets": np.asarray(point_offsets, dtype="<i8"),
|
|
"point_raw_xyz": np.concatenate(point_raw),
|
|
"point_xyz_map": np.concatenate(point_xyz),
|
|
"point_rgbi": np.concatenate(point_rgbi),
|
|
"point_intensity": np.concatenate(point_intensity),
|
|
"pose_capture_sequence": _message_int_array(pose_messages, "sequence"),
|
|
"pose_payload_bytes": np.asarray(
|
|
[len(message.payload) for message, _ in pose_messages],
|
|
dtype="<i8",
|
|
),
|
|
"pose_received_at_epoch_ns": _message_int_array(
|
|
pose_messages,
|
|
"received_at_epoch_ns",
|
|
),
|
|
"pose_received_monotonic_ns": np.asarray(
|
|
[message.received_monotonic_ns for message, _ in pose_messages],
|
|
dtype="<i8",
|
|
),
|
|
"pose_header_seq": np.asarray(
|
|
[frame.header.seq for _, frame in pose_messages],
|
|
dtype="<u8",
|
|
),
|
|
"pose_header_stamp": np.asarray(
|
|
[frame.header.stamp for _, frame in pose_messages],
|
|
dtype="<i8",
|
|
),
|
|
"pose_header_scaler": np.asarray(
|
|
[frame.header.scaler for _, frame in pose_messages],
|
|
dtype="<i8",
|
|
),
|
|
"pose_stamp": np.asarray(
|
|
[frame.pose_stamp for _, frame in pose_messages],
|
|
dtype="<i8",
|
|
),
|
|
"pose_positions_map": np.asarray(
|
|
[frame.position_xyz for _, frame in pose_messages],
|
|
dtype="<f8",
|
|
).reshape((-1, 3)),
|
|
"pose_quaternions_map_from_lidar": np.asarray(
|
|
[frame.orientation_xyzw for _, frame in pose_messages],
|
|
dtype="<f8",
|
|
).reshape((-1, 4)),
|
|
"pose_distance": np.asarray(
|
|
[frame.distance for _, frame in pose_messages],
|
|
dtype="<f8",
|
|
),
|
|
"pose_accuracy": np.asarray(
|
|
[frame.pose_accuracy for _, frame in pose_messages],
|
|
dtype="<f8",
|
|
),
|
|
}
|
|
return arrays
|
|
|
|
|
|
def _message_int_array(
|
|
messages: list[tuple[StreamMessage, Any]],
|
|
attribute: str,
|
|
) -> npt.NDArray[np.int64]:
|
|
return np.asarray(
|
|
[getattr(message, attribute) for message, _ in messages],
|
|
dtype="<i8",
|
|
)
|
|
|
|
|
|
def _validate_arrays(arrays: Any, identity: dict[str, Any]) -> None:
|
|
for name, dtype in _ARRAY_DTYPES.items():
|
|
if arrays[name].dtype != dtype:
|
|
raise LidarReplayError(f"LiDAR replay array {name} dtype changed")
|
|
point_frames = int(arrays["point_capture_sequence"].shape[0])
|
|
pose_frames = int(arrays["pose_capture_sequence"].shape[0])
|
|
points = int(arrays["point_raw_xyz"].shape[0])
|
|
point_vector_names = {
|
|
"point_payload_bytes",
|
|
"point_received_at_epoch_ns",
|
|
"point_received_monotonic_ns",
|
|
"point_header_seq",
|
|
"point_header_stamp",
|
|
"point_scaler",
|
|
}
|
|
pose_vector_names = {
|
|
"pose_payload_bytes",
|
|
"pose_received_at_epoch_ns",
|
|
"pose_received_monotonic_ns",
|
|
"pose_header_seq",
|
|
"pose_header_stamp",
|
|
"pose_header_scaler",
|
|
"pose_stamp",
|
|
"pose_distance",
|
|
"pose_accuracy",
|
|
}
|
|
if (
|
|
point_frames < 1
|
|
or any(arrays[name].shape != (point_frames,) for name in point_vector_names)
|
|
or arrays["point_offsets"].shape != (point_frames + 1,)
|
|
or arrays["point_raw_xyz"].shape != (points, 3)
|
|
or arrays["point_xyz_map"].shape != (points, 3)
|
|
or arrays["point_rgbi"].shape != (points,)
|
|
or arrays["point_intensity"].shape != (points,)
|
|
or any(arrays[name].shape != (pose_frames,) for name in pose_vector_names)
|
|
or arrays["pose_positions_map"].shape != (pose_frames, 3)
|
|
or arrays["pose_quaternions_map_from_lidar"].shape != (pose_frames, 4)
|
|
or int(arrays["point_offsets"][0]) != 0
|
|
or int(arrays["point_offsets"][-1]) != points
|
|
or np.any(np.diff(arrays["point_offsets"]) <= 0)
|
|
or np.any(np.diff(arrays["point_capture_sequence"]) <= 0)
|
|
or np.any(np.diff(arrays["point_received_monotonic_ns"]) < 0)
|
|
or np.any(arrays["point_payload_bytes"] <= 0)
|
|
or np.any(arrays["point_scaler"] == 0)
|
|
or (
|
|
pose_frames > 0
|
|
and (
|
|
np.any(np.diff(arrays["pose_capture_sequence"]) <= 0)
|
|
or np.any(np.diff(arrays["pose_received_monotonic_ns"]) < 0)
|
|
or np.any(arrays["pose_payload_bytes"] <= 0)
|
|
)
|
|
)
|
|
or identity.get("point_frame_count") != point_frames
|
|
or identity.get("pose_frame_count") != pose_frames
|
|
or identity.get("point_count") != points
|
|
):
|
|
raise LidarReplayError("LiDAR replay array shapes or counts are invalid")
|
|
expected_intensity = (arrays["point_rgbi"] & np.uint32(0xFF)).astype(np.uint8)
|
|
if not np.array_equal(arrays["point_intensity"], expected_intensity):
|
|
raise LidarReplayError("LiDAR intensity is not the exact rgbi low byte")
|
|
expected_xyz = np.empty_like(arrays["point_xyz_map"])
|
|
for index in range(point_frames):
|
|
start = int(arrays["point_offsets"][index])
|
|
end = int(arrays["point_offsets"][index + 1])
|
|
expected_xyz[start:end] = (
|
|
arrays["point_raw_xyz"][start:end].astype(np.float64)
|
|
/ float(arrays["point_scaler"][index])
|
|
)
|
|
if not np.array_equal(arrays["point_xyz_map"], expected_xyz):
|
|
raise LidarReplayError("LiDAR XYZ does not reproduce raw coordinates and scaler")
|
|
finite_names = {
|
|
"point_xyz_map",
|
|
"pose_positions_map",
|
|
"pose_quaternions_map_from_lidar",
|
|
"pose_distance",
|
|
"pose_accuracy",
|
|
}
|
|
if any(not np.isfinite(arrays[name]).all() for name in finite_names):
|
|
raise LidarReplayError("LiDAR replay contains non-finite geometry")
|
|
|
|
|
|
def _quality_report(
|
|
pack_id: str,
|
|
logical_sha256: str,
|
|
arrays: dict[str, npt.NDArray[Any]],
|
|
*,
|
|
pose_coverage_threshold_ms: float,
|
|
) -> dict[str, object]:
|
|
offsets = arrays["point_offsets"]
|
|
point_counts = np.diff(offsets).astype(np.float64)
|
|
point_times = arrays["point_received_monotonic_ns"]
|
|
point_intervals_ms = np.diff(point_times).astype(np.float64) / 1_000_000
|
|
pose_times = arrays["pose_received_monotonic_ns"]
|
|
pose_delta_ms = _nearest_time_delta_ms(point_times, pose_times)
|
|
covered = pose_delta_ms <= pose_coverage_threshold_ms
|
|
point_sequence = _sequence_report(arrays["point_header_seq"])
|
|
pose_sequence = _sequence_report(arrays["pose_header_seq"])
|
|
return {
|
|
"schema_version": LIDAR_QUALITY_REPORT_SCHEMA,
|
|
"pack_id": pack_id,
|
|
"profile_id": K1_LIDAR_PACK_V2_PROFILE.profile_id,
|
|
"logical_content_sha256": logical_sha256,
|
|
"field_retention": {
|
|
"xyz_map": True,
|
|
"raw_xyz": True,
|
|
"raw_rgbi": True,
|
|
"intensity_low_byte": True,
|
|
"source_sequence": True,
|
|
"header_seq_stamp_scaler": True,
|
|
"host_epoch_ns": True,
|
|
"host_monotonic_ns": True,
|
|
},
|
|
"frames": {
|
|
"point": int(point_times.shape[0]),
|
|
"pose": int(pose_times.shape[0]),
|
|
"points": int(arrays["point_raw_xyz"].shape[0]),
|
|
},
|
|
"point_count_per_frame": _distribution(point_counts),
|
|
"point_frame_interval_ms": _distribution(point_intervals_ms),
|
|
"intensity_0_255": _distribution(
|
|
arrays["point_intensity"].astype(np.float64)
|
|
),
|
|
"sensor_range_m": _distribution(np.empty((0,), dtype=np.float64)),
|
|
"sensor_range_status": (
|
|
"unavailable-vendor-map-increment-with-independent-best-effort-pose"
|
|
),
|
|
"scaler": _distribution(arrays["point_scaler"].astype(np.float64)),
|
|
"point_header_sequence": point_sequence,
|
|
"pose_header_sequence": pose_sequence,
|
|
"point_header_stamp": _timestamp_report(arrays["point_header_stamp"]),
|
|
"pose_header_stamp": _timestamp_report(arrays["pose_header_stamp"]),
|
|
"pose_binding": {
|
|
"basis": "nearest-recorded-host-monotonic-arrival",
|
|
"threshold_ms": pose_coverage_threshold_ms,
|
|
"covered_point_frames": int(np.count_nonzero(covered)),
|
|
"coverage_fraction": (
|
|
float(np.mean(covered)) if covered.size else 0.0
|
|
),
|
|
"nearest_delta_ms": _distribution(pose_delta_ms),
|
|
},
|
|
"limitations": [
|
|
"vendor-mapped increment, not an admitted raw sensor sweep",
|
|
"no admitted ring, per-point firing time, scan geometry or IMU samples",
|
|
"host-arrival time is exact evidence but not a shared sensor hardware clock",
|
|
],
|
|
"authority": {
|
|
"commands_enabled": False,
|
|
"navigation_or_safety_accepted": False,
|
|
},
|
|
}
|
|
|
|
|
|
def _equivalence_report(
|
|
pack_id: str,
|
|
logical_sha256: str,
|
|
capture_path: Path,
|
|
replay_arrays: Any,
|
|
) -> dict[str, object]:
|
|
source_arrays = _capture_arrays(capture_path)
|
|
comparisons: dict[str, dict[str, object]] = {}
|
|
mismatch_count = 0
|
|
for name in sorted(_ARRAY_DTYPES):
|
|
source = source_arrays[name]
|
|
replay = replay_arrays[name]
|
|
equal = bool(
|
|
source.dtype == replay.dtype
|
|
and source.shape == replay.shape
|
|
and np.array_equal(source, replay)
|
|
)
|
|
comparisons[name] = {
|
|
"equal": equal,
|
|
"dtype": source.dtype.str,
|
|
"shape": list(source.shape),
|
|
}
|
|
mismatch_count += int(not equal)
|
|
source_logical_sha256 = _logical_content_sha256(source_arrays)
|
|
status = (
|
|
"passed"
|
|
if mismatch_count == 0 and source_logical_sha256 == logical_sha256
|
|
else "failed"
|
|
)
|
|
return {
|
|
"schema_version": LIDAR_EQUIVALENCE_REPORT_SCHEMA,
|
|
"pack_id": pack_id,
|
|
"status": status,
|
|
"comparison": "source-native-capture-vs-persisted-replay-fields",
|
|
"source_logical_content_sha256": source_logical_sha256,
|
|
"replay_logical_content_sha256": logical_sha256,
|
|
"arrays_compared": len(comparisons),
|
|
"array_mismatches": mismatch_count,
|
|
"fields": comparisons,
|
|
"authority": {
|
|
"commands_enabled": False,
|
|
"navigation_or_safety_accepted": False,
|
|
},
|
|
}
|
|
|
|
|
|
def _validate_reports(
|
|
quality: dict[str, Any],
|
|
equivalence: dict[str, Any],
|
|
pack_id: str,
|
|
logical_sha256: str,
|
|
) -> None:
|
|
if (
|
|
quality.get("schema_version") != LIDAR_QUALITY_REPORT_SCHEMA
|
|
or quality.get("pack_id") != pack_id
|
|
or quality.get("logical_content_sha256") != logical_sha256
|
|
or quality.get("profile_id") != K1_LIDAR_PACK_V2_PROFILE.profile_id
|
|
):
|
|
raise LidarReplayError("LiDAR quality report is incompatible")
|
|
if (
|
|
equivalence.get("schema_version") != LIDAR_EQUIVALENCE_REPORT_SCHEMA
|
|
or equivalence.get("pack_id") != pack_id
|
|
or equivalence.get("status") != "passed"
|
|
or equivalence.get("source_logical_content_sha256") != logical_sha256
|
|
or equivalence.get("replay_logical_content_sha256") != logical_sha256
|
|
or equivalence.get("array_mismatches") != 0
|
|
):
|
|
raise LidarReplayError("LiDAR live/replay equivalence gate did not pass")
|
|
|
|
|
|
def _nearest_time_delta_ms(
|
|
points: npt.NDArray[Any],
|
|
poses: npt.NDArray[Any],
|
|
) -> npt.NDArray[np.float64]:
|
|
if points.size == 0:
|
|
return np.empty((0,), dtype=np.float64)
|
|
if poses.size == 0:
|
|
return np.full(points.shape, np.inf, dtype=np.float64)
|
|
pose_values = poses.astype(np.int64)
|
|
result = np.empty(points.shape, dtype=np.float64)
|
|
for index, value in enumerate(points.astype(np.int64)):
|
|
insertion = int(np.searchsorted(pose_values, value))
|
|
candidates: list[int] = []
|
|
if insertion < pose_values.size:
|
|
candidates.append(abs(int(pose_values[insertion]) - int(value)))
|
|
if insertion > 0:
|
|
candidates.append(abs(int(pose_values[insertion - 1]) - int(value)))
|
|
result[index] = min(candidates) / 1_000_000
|
|
return result
|
|
|
|
|
|
def _sequence_report(values: npt.NDArray[Any]) -> dict[str, int]:
|
|
items = [int(value) for value in values]
|
|
differences = [
|
|
current - previous for previous, current in zip(items, items[1:], strict=False)
|
|
]
|
|
return {
|
|
"samples": len(items),
|
|
"nonincreasing": sum(value <= 0 for value in differences),
|
|
"duplicate": sum(value == 0 for value in differences),
|
|
"forward_gaps": sum(max(0, value - 1) for value in differences),
|
|
}
|
|
|
|
|
|
def _timestamp_report(values: npt.NDArray[Any]) -> dict[str, object]:
|
|
items = [int(value) for value in values]
|
|
differences = np.asarray(
|
|
[
|
|
current - previous
|
|
for previous, current in zip(items, items[1:], strict=False)
|
|
],
|
|
dtype=np.float64,
|
|
)
|
|
return {
|
|
"samples": len(items),
|
|
"nonincreasing": int(np.count_nonzero(differences <= 0)),
|
|
"duplicate": int(np.count_nonzero(differences == 0)),
|
|
"positive_delta": _distribution(differences[differences > 0]),
|
|
}
|
|
|
|
|
|
def _distribution(values: npt.NDArray[Any]) -> dict[str, float | int | None]:
|
|
finite = np.asarray(values, dtype=np.float64)
|
|
finite = finite[np.isfinite(finite)]
|
|
if finite.size == 0:
|
|
return {
|
|
"sample_count": 0,
|
|
"minimum": None,
|
|
"mean": None,
|
|
"p50": None,
|
|
"p95": None,
|
|
"maximum": None,
|
|
}
|
|
return {
|
|
"sample_count": int(finite.size),
|
|
"minimum": float(np.min(finite)),
|
|
"mean": float(np.mean(finite)),
|
|
"p50": float(np.percentile(finite, 50)),
|
|
"p95": float(np.percentile(finite, 95)),
|
|
"maximum": float(np.max(finite)),
|
|
}
|
|
|
|
|
|
def _logical_content_sha256(arrays: Any) -> str:
|
|
digest = hashlib.sha256()
|
|
for name in sorted(_ARRAY_DTYPES):
|
|
array = np.ascontiguousarray(arrays[name])
|
|
descriptor = {
|
|
"name": name,
|
|
"dtype": array.dtype.str,
|
|
"shape": list(array.shape),
|
|
}
|
|
encoded = _canonical_json(descriptor)
|
|
digest.update(len(encoded).to_bytes(8, "big"))
|
|
digest.update(encoded)
|
|
digest.update(array.tobytes(order="C"))
|
|
return digest.hexdigest()
|
|
|
|
|
|
def _validate_artifacts(root: Path, raw: object) -> dict[str, Path]:
|
|
expected = {
|
|
"lidar-arrays": (LIDAR_REPLAY_ARRAYS_NAME, "application/x-npz"),
|
|
"lidar-quality": (LIDAR_QUALITY_REPORT_NAME, "application/json"),
|
|
"live-replay-equivalence": (
|
|
LIDAR_EQUIVALENCE_REPORT_NAME,
|
|
"application/json",
|
|
),
|
|
}
|
|
if not isinstance(raw, list) or len(raw) != len(expected):
|
|
raise LidarReplayError("LiDAR replay artifact set is incomplete")
|
|
result: dict[str, Path] = {}
|
|
for value in raw:
|
|
kind = value.get("kind") if isinstance(value, dict) else None
|
|
if not isinstance(kind, str) or kind not in expected or kind in result:
|
|
raise LidarReplayError("LiDAR replay artifact descriptor is invalid")
|
|
name, media_type = expected[kind]
|
|
path = root / name
|
|
if (
|
|
not path.is_file()
|
|
or path.is_symlink()
|
|
or path.resolve(strict=True).parent != root
|
|
or value.get("path") != name
|
|
or value.get("media_type") != media_type
|
|
or value.get("byte_length") != path.stat().st_size
|
|
or value.get("sha256") != _sha256(path)
|
|
):
|
|
raise LidarReplayError("LiDAR replay artifact identity changed")
|
|
result[kind] = path
|
|
return result
|
|
|
|
|
|
def _artifact_identity(path: Path) -> dict[str, object]:
|
|
return {
|
|
"byte_length": path.stat().st_size,
|
|
"sha256": _sha256(path),
|
|
}
|
|
|
|
|
|
def _artifact_descriptor(
|
|
kind: str,
|
|
path: Path,
|
|
media_type: str,
|
|
) -> dict[str, object]:
|
|
return {
|
|
"kind": kind,
|
|
"path": path.name,
|
|
"media_type": media_type,
|
|
"byte_length": path.stat().st_size,
|
|
"sha256": _sha256(path),
|
|
}
|
|
|
|
|
|
def _infer_session_id(capture_path: Path) -> str:
|
|
parents = capture_path.parents
|
|
if len(parents) < 3:
|
|
raise LidarReplayError("LiDAR replay session id cannot be inferred")
|
|
return parents[2].name
|
|
|
|
|
|
def _canonical_json(value: object) -> bytes:
|
|
return json.dumps(
|
|
value,
|
|
ensure_ascii=False,
|
|
sort_keys=True,
|
|
separators=(",", ":"),
|
|
allow_nan=False,
|
|
).encode()
|
|
|
|
|
|
def _write_json(path: Path, value: object) -> None:
|
|
path.write_bytes(
|
|
json.dumps(
|
|
value,
|
|
ensure_ascii=False,
|
|
sort_keys=True,
|
|
indent=2,
|
|
allow_nan=False,
|
|
).encode()
|
|
+ b"\n"
|
|
)
|
|
|
|
|
|
def _read_json_object(path: Path) -> dict[str, Any]:
|
|
try:
|
|
value = json.loads(path.read_text(encoding="utf-8"))
|
|
except (OSError, UnicodeDecodeError, json.JSONDecodeError) as exc:
|
|
raise LidarReplayError(f"{path.name} is not valid JSON") from exc
|
|
return _object(value, path.name)
|
|
|
|
|
|
def _object(value: object, label: str) -> dict[str, Any]:
|
|
if not isinstance(value, dict) or any(not isinstance(key, str) for key in value):
|
|
raise LidarReplayError(f"{label} must be an object")
|
|
return value
|
|
|
|
|
|
def _sha256(path: Path) -> str:
|
|
digest = hashlib.sha256()
|
|
with path.open("rb") as stream:
|
|
while chunk := stream.read(1024 * 1024):
|
|
digest.update(chunk)
|
|
return digest.hexdigest()
|