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(" 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=" npt.NDArray[np.int64]: return np.asarray( [getattr(message, attribute) for message, _ in messages], dtype=" 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()