From e6d5411bdd4d50874e3f1ad4d52cde7fff9aba8b Mon Sep 17 00:00:00 2001 From: DCCONSTRUCTIONS Date: Sun, 26 Jul 2026 01:06:04 +0300 Subject: [PATCH] feat: qualify bounded K1 surface shadow --- README.md | 10 + docs/13_LIDAR_WORKER_PRODUCT_AND_ROADMAP.md | 37 +- docs/14_LIDAR_DATASET_GATEWAY.md | 5 +- .../perception/LAB_E27_REPORT_2026-07-26.md | 97 +++ .../perception/run_k1_local_surface_shadow.py | 365 ++++++++++ src/k1link/compute/__init__.py | 14 + .../compute/lidar_local_surface_shadow.py | 681 ++++++++++++++++++ tests/test_lidar_local_surface.py | 175 ++++- 8 files changed, 1370 insertions(+), 14 deletions(-) create mode 100644 experiments/perception/LAB_E27_REPORT_2026-07-26.md create mode 100644 experiments/perception/run_k1_local_surface_shadow.py create mode 100644 src/k1link/compute/lidar_local_surface_shadow.py diff --git a/README.md b/README.md index 0da6ce5..7ea2667 100644 --- a/README.md +++ b/README.md @@ -158,6 +158,16 @@ in-band, above-plane and below-plane observations. On source frame `1254`, localizing the heavy tail without naming an object or granting safety authority. +The same estimator now runs behind +`missioncore.k1-local-surface-shadow-runtime/v1`, a capacity-two +latest-wins queue with a bounded diagnostic result ring. LAB E27 passed a +`14.995 s` RAVNOVES00 slice at recorded 1× pace: `143/143` available frames +were consumed, maximum queue depth was `1/2`, no frame was replaced, p95 +processing latency was `14.777 ms`, and state, point classes, step candidates +and scalar results matched the immutable replay derivative exactly. This is a +recorded-source-paced execution gate only; physical K1 worker binding, +free-space, commands, navigation and safety authority remain unavailable. + The complete RELLIS-3D v1.1 release is now admitted there and its full `2,413`-frame validation split is available in **Полигон → Датасеты**. The sealed Current/Patchwork++ comparison rejected Patchwork++ for navigation: diff --git a/docs/13_LIDAR_WORKER_PRODUCT_AND_ROADMAP.md b/docs/13_LIDAR_WORKER_PRODUCT_AND_ROADMAP.md index 22b1b78..68c9262 100644 --- a/docs/13_LIDAR_WORKER_PRODUCT_AND_ROADMAP.md +++ b/docs/13_LIDAR_WORKER_PRODUCT_AND_ROADMAP.md @@ -4,7 +4,8 @@ Date: 2026-07-26 Status: accepted architecture plan; L0/L1 implemented; L2 diagnostic A/B complete; full GOOSE and RELLIS qualification complete; L2.6d K1 replay local-surface temporal qualification, operator triage and prior-plane residual -explainability implemented; bounded live shadow next +explainability implemented; L2.6e recorded-source-paced bounded shadow +qualified; physical K1 shadow next Scope: passively received real-time K1 point/pose evidence, immutable replay and future live shadow processing Explicitly out of scope: K1 firmware modification, a new onboard exporter, new @@ -419,8 +420,11 @@ Dataset expansion is no longer the next gate. aggregate p95. - [ ] Complete the remaining qualification report with per-frame latency, point age, obstacle preservation and memory growth. -- [ ] Replay the same profiles through a bounded latest-wins live-shadow queue; - no K1 command, navigation or safety authority is added. +- [x] Replay the same profile through a bounded latest-wins shadow queue at the + recorded 1× source rate; no K1 command, navigation or safety authority is + added. +- [ ] Bind that runtime to physical live K1 point/pose evidence through the + existing authenticated external-worker seam. The implemented `missioncore.k1-local-surface/v1` derivative is reproducible through `experiments/perception/run_k1_local_surface.py` and is exposed @@ -496,9 +500,27 @@ independent ground truth. Frame `1195` instead has only seven out-of-band cells together, preserving its separate interpretation as a surface-regime transition. -No fit threshold was changed after this review. The next gate is a bounded -latest-wins live-shadow queue using the same profile and evidence contract, -still without commands, free-space, navigation or safety authority. +No fit threshold was changed after this review. + +The L2.6e runtime contract +`missioncore.k1-local-surface-shadow-runtime/v1` now copies and freezes one +decoded map-point/pose pair, runs the accepted estimator behind a +capacity-two latest-wins queue and retains only a bounded diagnostic result +ring. It has no command method, never turns missing points into free space and +publishes explicit false navigation/safety authority. + +LAB E27 passed a `14.995 s` source-paced 1× RAVNOVES00 slice: all `143` +available frames were consumed, maximum queue depth was `1/2`, no work was +replaced and no processing failed. Processing latency was `12.261 ms` p50, +`14.777 ms` p95 and `33.135 ms` maximum. Every processed result matched the +immutable replay derivative: zero state, point-class or step-candidate +mismatches and zero scalar delta. The accepted result is +`k1-local-surface-shadow-04f14d8c580f74cbd5b0a452867563ebc6b3ef93d872129e4918680932253ab7`. + +The next gate is not another replay tuning pass. It is an explicit bounded +LiDAR↔pose binder on the authenticated external worker stream followed by a +physical K1 shadow run, still without commands, free-space, navigation or +safety authority. Exit: one immutable K1 session yields both a persistent reconstruction and a bounded local world state without hard-coded terrain height or scanner-side @@ -521,7 +543,8 @@ independent gate without increasing unsafe false-free or false-dynamic output. ### L4 — live shadow integration -- [ ] Add a bounded LiDAR queue independent of camera cadence. +- [x] Add a provider-neutral bounded LiDAR local-surface queue independent of + camera cadence and qualify it at recorded 1× source pace. - [ ] Run the accepted K1 local-surface/local-map profile on the NVIDIA worker. - [ ] Fuse K1 geometric evidence with E26 camera evidence as independent sources; a LiDAR-native detector remains optional. diff --git a/docs/14_LIDAR_DATASET_GATEWAY.md b/docs/14_LIDAR_DATASET_GATEWAY.md index 30007cd..b59e0cf 100644 --- a/docs/14_LIDAR_DATASET_GATEWAY.md +++ b/docs/14_LIDAR_DATASET_GATEWAY.md @@ -93,8 +93,9 @@ Not implemented: - no RELLIS ROS bag admission, continuous synchronized playback or production promotion; - no ray-cleared free-space or planner-authoritative rolling occupancy map. -- no bounded live-shadow execution of the accepted K1 local-surface profile - yet; the residual overlay remains replay-only and non-authoritative. +- the accepted K1 local-surface profile now passes a 15-second + recorded-source-paced bounded shadow gate; physical K1 worker binding is not + implemented yet, and the residual overlay remains non-authoritative. ## Product surface boundary diff --git a/experiments/perception/LAB_E27_REPORT_2026-07-26.md b/experiments/perception/LAB_E27_REPORT_2026-07-26.md new file mode 100644 index 0000000..1e0eacc --- /dev/null +++ b/experiments/perception/LAB_E27_REPORT_2026-07-26.md @@ -0,0 +1,97 @@ +# LAB E27 — bounded K1 local-surface shadow qualification + +Date: 2026-07-26 +Status: **accepted for recorded-source-paced shadow diagnostic** +Authority: diagnostic only; commands, navigation and safety acceptance disabled + +## 1. Question + +Can the accepted K1 rolling local-surface profile execute through a real +bounded latest-wins worker loop at the recorded K1 source rate without queue +growth, frame replacement or divergence from the immutable replay result? + +This lab does not ask whether the derived surface is ground truth or +planner-ready. It qualifies execution semantics only. + +## 2. Fixed input + +- session: `20260720T065719Z_viewer_live`; +- source pack: + `e10-lidar-pack-5da0396d32a27f9d1ca537cc2e8a371d386078d6f0dc71737b78620992af9625`; +- source artifact SHA-256: + `72aa73340b20fcfaa21b330ef5b93b975c14a70e2cd16a57752d9952ff05ad9a`; +- replay reference: + `k1-local-surface-628cd024775f02fea99765d1fb457efec2d5cc4d7371e56818dfe8e08c9b3b74`; +- reference logical-content SHA-256: + `b89a92887cedace9d3eab3e1490697fb6bc7fc16a2d1887f3fff7cb4547ceb14`; +- selection: source frames `0–150`, `14.995 s`, 151 timeline entries and + 143 available LiDAR frames; +- pace: recorded 1×; +- work queue: latest-wins, capacity 2; +- result ring: bounded to the 143-frame qualification selection. + +The source pack and replay derivative were opened read-only. No scanner, +firmware, MQTT command or persistent reconstruction was changed. + +## 3. Implemented runtime boundary + +`missioncore.k1-local-surface-shadow-runtime/v1` accepts only an already +decoded map-frame point cloud and a compatible `T_map_from_sensor` pose. It: + +1. copies and freezes the admitted point/pose pair; +2. publishes work into a bounded latest-wins queue; +3. runs the same robust rolling-cell and prior-only prediction profile used by + replay; +4. retains only a bounded diagnostic result ring; +5. reports observed surface, observed occupied-above-surface, negative + outliers, unverified step candidates, latency and freshness; +6. explicitly keeps absence-of-points distinct from free space. + +The runtime has no command method and every result carries +`commands_enabled=false` and `navigation_or_safety_accepted=false`. + +## 4. Result + +Result: +`k1-local-surface-shadow-04f14d8c580f74cbd5b0a452867563ebc6b3ef93d872129e4918680932253ab7` + +| Metric | Result | +| --- | ---: | +| Published / consumed | 143 / 143 | +| Latest-wins replacements | 0 | +| Maximum queue depth | 1 / 2 | +| Processing failures | 0 | +| Processing p50 / p95 / max | 12.261 / 14.777 / 33.135 ms | +| Result age p50 / p95 / max | 12.336 / 14.857 / 33.378 ms | +| Replay state mismatches | 0 | +| Point-class mismatches | 0 | +| Step-candidate mismatches | 0 | +| Maximum scalar delta | 0.0 | + +All 143 results were valid for this selection. Queue accounting closed exactly: +`consumed + dropped_overflow = published`, final depth was zero and the worker +thread stopped cleanly. + +## 5. Decision + +The execution gate passes. The current CPU geometric profile is comfortably +inside the observed roughly 10 Hz K1 publication interval on this 15-second +slice, remains bounded and reproduces replay exactly when no work is replaced. + +This result promotes the profile only from offline replay implementation to a +recorded-source-paced shadow candidate. It does not promote: + +- the surface estimate to ground truth; +- observed occupancy to free-space evidence; +- step candidates to semantic curbs; +- the worker result to navigation or safety authority; +- the replay transport to a physical-live K1 gate. + +## 6. Next gate + +Connect the runtime to the existing authenticated external worker stream using +an explicit bounded LiDAR↔pose binder, then repeat at least 15 seconds against +a physical K1 acquisition. Measure source sequence gaps, pose-binding misses, +queue replacements, result age, memory slope and recovery across reconnect. +React remains a read-only status/review surface; it does not execute the +algorithm. diff --git a/experiments/perception/run_k1_local_surface_shadow.py b/experiments/perception/run_k1_local_surface_shadow.py new file mode 100644 index 0000000..a2fecce --- /dev/null +++ b/experiments/perception/run_k1_local_surface_shadow.py @@ -0,0 +1,365 @@ +#!/usr/bin/env python3 +from __future__ import annotations + +import argparse +import hashlib +import json +import math +import os +import time +from datetime import UTC, datetime +from pathlib import Path +from typing import Any + +import numpy as np + +from k1link.compute import ( + DEFAULT_K1_LOCAL_SURFACE_PROFILE, + E10LidarFieldSource, + K1LocalSurfaceShadowInput, + K1LocalSurfaceShadowResult, + K1LocalSurfaceShadowRuntime, + K1LocalSurfaceV1, +) +from k1link.compute.lidar_local_surface import ( + FRAME_FIT_FAILED, + FRAME_INSUFFICIENT_SURFACE, + FRAME_POSE_STALE, + FRAME_VALID, +) + +QUALIFICATION_SCHEMA = "missioncore.k1-local-surface-shadow-qualification/v1" + + +def _arguments() -> argparse.Namespace: + parser = argparse.ArgumentParser( + description=( + "Run the accepted K1 rolling-surface profile through a bounded " + "latest-wins replay shadow and compare processed frames to the " + "immutable replay derivative." + ) + ) + parser.add_argument("source_pack", type=Path) + parser.add_argument("reference_model", type=Path) + parser.add_argument("output_root", type=Path) + parser.add_argument("--start-frame", type=int, default=0) + parser.add_argument("--duration-seconds", type=float, default=15.0) + parser.add_argument("--pace-scale", type=float, default=1.0) + parser.add_argument("--queue-capacity", type=int, default=2) + return parser.parse_args() + + +def _canonical_json(value: object) -> bytes: + return json.dumps( + value, + ensure_ascii=False, + sort_keys=True, + separators=(",", ":"), + allow_nan=False, + ).encode() + + +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() + + +def _distribution(values: list[float]) -> dict[str, float | int | None]: + if not values: + return { + "sample_count": 0, + "minimum": None, + "mean": None, + "p50": None, + "p95": None, + "maximum": None, + } + array = np.asarray(values, dtype=np.float64) + if not np.isfinite(array).all(): + raise RuntimeError("shadow qualification latency is invalid") + return { + "sample_count": int(array.shape[0]), + "minimum": float(np.min(array)), + "mean": float(np.mean(array)), + "p50": float(np.percentile(array, 50)), + "p95": float(np.percentile(array, 95)), + "maximum": float(np.max(array)), + } + + +def _expected_state(model: K1LocalSurfaceV1, frame_index: int) -> str: + code = int(model.arrays["frame_failure_code"][frame_index]) + return { + FRAME_VALID: "valid", + FRAME_POSE_STALE: "pose-stale", + FRAME_INSUFFICIENT_SURFACE: "insufficient-surface", + FRAME_FIT_FAILED: "fit-failed", + }[code] + + +def _maximum_scalar_delta( + result: K1LocalSurfaceShadowResult, + model: K1LocalSurfaceV1, +) -> float: + frame_index = result.frame_index + pairs = ( + (result.sensor_height_m, "sensor_height_m"), + (result.slope_deg, "slope_deg"), + (result.roughness_m, "roughness_m"), + (result.confidence, "confidence"), + (result.surface_max_age_ms, "surface_max_age_ms"), + ) + deltas = [ + abs(float(value) - float(model.arrays[name][frame_index])) + for value, name in pairs + if value is not None + ] + return max(deltas, default=0.0) + + +def _qualification( + source: E10LidarFieldSource, + model: K1LocalSurfaceV1, + *, + start_frame: int, + duration_seconds: float, + pace_scale: float, + queue_capacity: int, +) -> dict[str, Any]: + if ( + model.identity.get("source_pack_id") != source.pack_id + or model.identity.get("source_pack_identity_sha256") + != source.manifest.get("identity_sha256") + or model.identity.get("source_artifact_sha256") + != source.manifest.get("artifact", {}).get("sha256") + or model.identity.get("profile") != DEFAULT_K1_LOCAL_SURFACE_PROFILE.to_dict() + ): + raise RuntimeError("shadow qualification source/model binding is invalid") + if ( + not 0 <= start_frame < source.frame_count + or not math.isfinite(duration_seconds) + or duration_seconds <= 0 + or not math.isfinite(pace_scale) + or not 0 < pace_scale <= 10 + or not 1 <= queue_capacity <= 8 + ): + raise RuntimeError("shadow qualification selection is invalid") + + arrays = source.arrays + session_times = arrays["session_seconds"] + start_seconds = float(session_times[start_frame]) + selected = [ + frame_index + for frame_index in range(start_frame, source.frame_count) + if float(session_times[frame_index]) - start_seconds <= duration_seconds + ] + if len(selected) < 2: + raise RuntimeError("shadow qualification selection is too short") + expected_available = [ + frame_index for frame_index in selected if bool(arrays["sample_available"][frame_index]) + ] + runtime = K1LocalSurfaceShadowRuntime( + f"{source.identity['session_id']}-qualification", + queue_capacity=queue_capacity, + result_capacity=min(256, max(1, len(expected_available))), + ) + wall_started = time.perf_counter() + offsets = arrays["cloud_offsets"] + try: + for frame_index in selected: + release_at = ( + wall_started + (float(session_times[frame_index]) - start_seconds) * pace_scale + ) + remaining = release_at - time.perf_counter() + if remaining > 0: + time.sleep(remaining) + if not bool(arrays["sample_available"][frame_index]): + continue + start = int(offsets[frame_index]) + end = int(offsets[frame_index + 1]) + runtime.publish( + K1LocalSurfaceShadowInput( + frame_index=frame_index, + source_frame_index=int(arrays["source_frame_indices"][frame_index]), + session_seconds=float(session_times[frame_index]), + pose_binding_age_ms=abs(float(arrays["pose_point_delta_ms"][frame_index])), + points_map=np.asarray( + arrays["cloud_points_map"][start:end], + dtype=np.float64, + ), + position_map=np.asarray( + arrays["pose_positions_map"][frame_index], + dtype=np.float64, + ), + published_monotonic_ns=time.monotonic_ns(), + ) + ) + runtime.close(timeout_seconds=30.0) + results = runtime.results() + runtime_snapshot = runtime.snapshot() + finally: + runtime.close() + + state_mismatches = 0 + point_class_mismatches = 0 + step_candidate_mismatches = 0 + maximum_scalar_delta = 0.0 + for result in results: + frame_index = result.frame_index + state_mismatches += result.state != _expected_state(model, frame_index) + if result.valid: + start = int(offsets[frame_index]) + end = int(offsets[frame_index + 1]) + point_class_mismatches += int( + np.count_nonzero(result.point_class != model.arrays["point_class"][start:end]) + ) + step_candidate_mismatches += int( + np.count_nonzero( + result.point_step_candidate != model.arrays["point_step_candidate"][start:end] + ) + ) + maximum_scalar_delta = max( + maximum_scalar_delta, + _maximum_scalar_delta(result, model), + ) + queue = runtime_snapshot["queue"] + accepted = ( + int(queue["maximum_depth"]) <= queue_capacity + and int(queue["dropped_overflow"]) == 0 + and int(queue["consumed"]) == len(expected_available) + and len(results) == len(expected_available) + and int(runtime_snapshot["results"]["failed"]) == 0 + and state_mismatches == 0 + and point_class_mismatches == 0 + and step_candidate_mismatches == 0 + and maximum_scalar_delta <= 1e-9 + ) + return { + "schema_version": QUALIFICATION_SCHEMA, + "identity": { + "source_pack_id": source.pack_id, + "source_pack_identity_sha256": source.manifest["identity_sha256"], + "source_artifact_sha256": source.manifest["artifact"]["sha256"], + "reference_model_id": model.model_id, + "reference_logical_content_sha256": model.identity["logical_content_sha256"], + "session_id": source.identity["session_id"], + "profile": DEFAULT_K1_LOCAL_SURFACE_PROFILE.to_dict(), + "selection": { + "start_frame": start_frame, + "end_frame": selected[-1], + "source_duration_seconds": (float(session_times[selected[-1]]) - start_seconds), + "selected_frames": len(selected), + "available_frames": len(expected_available), + }, + "runtime": { + "queue_policy": "bounded-latest-wins", + "queue_capacity": queue_capacity, + "result_capacity": min( + 256, + max(1, len(expected_available)), + ), + "pace_scale": pace_scale, + }, + "producer_sha256": _sha256(Path(__file__).resolve(strict=True)), + }, + "state": "accepted" if accepted else "rejected", + "accepted": accepted, + "ground_truth": False, + "metrics": { + "wall_elapsed_seconds": time.perf_counter() - wall_started, + "queue": queue, + "result_states": runtime_snapshot["results"]["state_counts"], + "processing_ms": _distribution([result.processing_ms for result in results]), + "result_age_ms": _distribution([result.result_age_ms for result in results]), + "replay_parity": { + "compared_frames": len(results), + "state_mismatches": state_mismatches, + "point_class_mismatches": point_class_mismatches, + "step_candidate_mismatches": step_candidate_mismatches, + "maximum_scalar_delta": maximum_scalar_delta, + }, + }, + "acceptance": { + "queue_bounded": int(queue["maximum_depth"]) <= queue_capacity, + "zero_latest_wins_replacements": int(queue["dropped_overflow"]) == 0, + "zero_processing_failures": (int(runtime_snapshot["results"]["failed"]) == 0), + "complete_processed_accounting": ( + int(queue["consumed"]) == len(expected_available) + and len(results) == len(expected_available) + ), + "replay_state_parity": state_mismatches == 0, + "replay_point_class_parity": point_class_mismatches == 0, + "replay_step_candidate_parity": step_candidate_mismatches == 0, + "replay_scalar_parity": maximum_scalar_delta <= 1e-9, + "navigation_or_safety_accepted": False, + }, + "occupancy_policy": { + "absence_of_points_means_free": False, + "unknown_is_traversable": False, + }, + "authority": { + "commands_enabled": False, + "navigation_or_safety_accepted": False, + }, + } + + +def main() -> int: + arguments = _arguments() + source = E10LidarFieldSource(arguments.source_pack) + model = K1LocalSurfaceV1(arguments.reference_model) + try: + report = _qualification( + source, + model, + start_frame=arguments.start_frame, + duration_seconds=arguments.duration_seconds, + pace_scale=arguments.pace_scale, + queue_capacity=arguments.queue_capacity, + ) + finally: + model.close() + source.close() + report_sha256 = hashlib.sha256(_canonical_json(report)).hexdigest() + result_id = f"k1-local-surface-shadow-{report_sha256}" + report["result_id"] = result_id + report["report_sha256"] = report_sha256 + report["created_at_utc"] = datetime.now(UTC).isoformat() + output_root = arguments.output_root.expanduser().resolve() + output_root.mkdir(mode=0o700, parents=True, exist_ok=True) + output = output_root / result_id + if output.exists(): + raise RuntimeError("shadow qualification result already exists") + staging = output_root / f".{result_id}.{os.getpid()}.incomplete" + staging.mkdir(mode=0o700, exist_ok=False) + try: + report_path = staging / "report.json" + report_path.write_bytes(_canonical_json(report)) + os.replace(staging, output) + except BaseException: + if staging.exists(): + for path in staging.iterdir(): + path.unlink() + staging.rmdir() + raise + print( + json.dumps( + { + "result_id": result_id, + "output": str(output), + "state": report["state"], + "metrics": report["metrics"], + "authority": report["authority"], + }, + ensure_ascii=False, + indent=2, + ) + ) + return 0 if report["accepted"] else 1 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/src/k1link/compute/__init__.py b/src/k1link/compute/__init__.py index d293ee0..01cf183 100644 --- a/src/k1link/compute/__init__.py +++ b/src/k1link/compute/__init__.py @@ -118,6 +118,14 @@ from .lidar_local_surface import ( build_k1_local_surface, k1_local_surface_catalog_item, ) +from .lidar_local_surface_shadow import ( + K1_LOCAL_SURFACE_SHADOW_FRAME_SCHEMA, + K1_LOCAL_SURFACE_SHADOW_SCHEMA, + K1LocalSurfaceShadowEstimator, + K1LocalSurfaceShadowInput, + K1LocalSurfaceShadowResult, + K1LocalSurfaceShadowRuntime, +) from .lidar_replay import ( LIDAR_EQUIVALENCE_REPORT_SCHEMA, LIDAR_QUALITY_REPORT_SCHEMA, @@ -213,6 +221,8 @@ __all__ = [ "K1_LOCAL_SURFACE_REVIEW_SCHEMA", "K1_LOCAL_SURFACE_SCHEMA", "K1_LOCAL_SURFACE_TIMELINE_SCHEMA", + "K1_LOCAL_SURFACE_SHADOW_FRAME_SCHEMA", + "K1_LOCAL_SURFACE_SHADOW_SCHEMA", "LIDAR_FIELD_REVIEW_REPORT_SCHEMA", "LIDAR_FIELD_REVIEW_SCHEMA", "LIDAR_FIELD_REVIEW_WINDOW_SCHEMA", @@ -236,6 +246,10 @@ __all__ = [ "LidarGroundBenchmarkV1", "LidarGroundError", "K1LocalSurfaceProfile", + "K1LocalSurfaceShadowEstimator", + "K1LocalSurfaceShadowInput", + "K1LocalSurfaceShadowResult", + "K1LocalSurfaceShadowRuntime", "K1LocalSurfaceV1", "LidarReplayError", "LidarReplayPackV2", diff --git a/src/k1link/compute/lidar_local_surface_shadow.py b/src/k1link/compute/lidar_local_surface_shadow.py new file mode 100644 index 0000000..19058d0 --- /dev/null +++ b/src/k1link/compute/lidar_local_surface_shadow.py @@ -0,0 +1,681 @@ +from __future__ import annotations + +import math +import threading +import time +from collections import Counter, deque +from dataclasses import asdict, dataclass +from typing import Any, Final, Literal + +import numpy as np +import numpy.typing as npt + +from k1link.data_plane import DecodedPointCloudView, DecodedPoseView +from k1link.ground_segmentation import GroundSegmentationError as LidarGroundError + +from .lidar_local_surface import ( + DEFAULT_K1_LOCAL_SURFACE_PROFILE, + POINT_BELOW_SURFACE, + POINT_OCCUPIED, + POINT_SURFACE, + K1LocalSurfaceProfile, + _cloud_cell_observations, + _expire_cache, + _fit_surface, + _height_above_plane, + _local_cache_records, + _point_step_candidates, + _prediction_metrics, + _step_candidate_keys, + _update_cache, +) +from .live_perception import LatestWinsQueue + +K1_LOCAL_SURFACE_SHADOW_SCHEMA: Final = "missioncore.k1-local-surface-shadow-runtime/v1" +K1_LOCAL_SURFACE_SHADOW_FRAME_SCHEMA: Final = "missioncore.k1-local-surface-shadow-frame/v1" + +ShadowFrameState = Literal[ + "valid", + "pose-stale", + "insufficient-surface", + "fit-failed", +] + + +@dataclass(frozen=True, slots=True) +class K1LocalSurfaceShadowInput: + """One immutable map-point/pose pair admitted to passive shadow work.""" + + frame_index: int + source_frame_index: int + session_seconds: float + pose_binding_age_ms: float + points_map: npt.NDArray[np.float64] + position_map: npt.NDArray[np.float64] + published_monotonic_ns: int + + def __post_init__(self) -> None: + points = np.array(self.points_map, dtype=np.float64, copy=True) + position = np.array(self.position_map, dtype=np.float64, copy=True) + if ( + self.frame_index < 0 + or self.source_frame_index < 0 + or not math.isfinite(self.session_seconds) + or self.session_seconds < 0 + or not math.isfinite(self.pose_binding_age_ms) + or self.pose_binding_age_ms < 0 + or self.published_monotonic_ns < 0 + or points.ndim != 2 + or points.shape[1:] != (3,) + or position.shape != (3,) + or not np.isfinite(points).all() + or not np.isfinite(position).all() + ): + raise LidarGroundError("K1 local-surface shadow input is invalid") + points.flags.writeable = False + position.flags.writeable = False + object.__setattr__(self, "points_map", points) + object.__setattr__(self, "position_map", position) + + @classmethod + def from_views( + cls, + point_cloud: DecodedPointCloudView, + pose: DecodedPoseView, + ) -> K1LocalSurfaceShadowInput: + """Bind already-normalized views without interpreting vendor payloads.""" + + if ( + point_cloud.frame_id != "map" + or pose.frame_id != "map" + or pose.child_frame_id != "sensor" + ): + raise LidarGroundError( + "K1 local-surface shadow requires map points and map-from-sensor pose" + ) + point_received_ns = point_cloud.context.received_monotonic_ns + pose_received_ns = pose.context.received_monotonic_ns + if point_received_ns is not None and pose_received_ns is not None: + pose_binding_age_ms = abs(point_received_ns - pose_received_ns) / 1_000_000 + else: + pose_binding_age_ms = ( + abs(point_cloud.context.captured_at_epoch_ns - pose.context.captured_at_epoch_ns) + / 1_000_000 + ) + points = np.asarray(point_cloud.positions_xyz, dtype=np.float64).reshape((-1, 3)) + position = np.asarray(pose.position_xyz, dtype=np.float64) + return cls( + frame_index=point_cloud.context.sequence, + source_frame_index=point_cloud.context.sequence, + session_seconds=point_cloud.context.captured_at_epoch_ns / 1_000_000_000, + pose_binding_age_ms=pose_binding_age_ms, + points_map=points, + position_map=position, + published_monotonic_ns=time.monotonic_ns(), + ) + + +@dataclass(frozen=True, slots=True) +class K1LocalSurfaceShadowResult: + """One bounded diagnostic result; it never describes free or safe space.""" + + frame_index: int + source_frame_index: int + session_seconds: float + state: ShadowFrameState + pose_binding_age_ms: float + surface_cell_count: int + surface_inlier_cell_count: int + plane_coefficients_map: tuple[float, float, float, float] | None + sensor_height_m: float | None + slope_deg: float | None + roughness_m: float | None + confidence: float | None + surface_max_age_ms: float | None + prediction_available: bool + prediction_cell_count: int + prediction_residual_p50_m: float | None + prediction_residual_p95_m: float | None + prediction_inlier_fraction: float | None + temporal_compared: bool + temporal_jump: bool + height_delta_m: float | None + slope_delta_deg: float | None + roughness_delta_m: float | None + surface_point_count: int + occupied_point_count: int + below_surface_point_count: int + step_candidate_point_count: int + processing_ms: float + result_age_ms: float + point_class: npt.NDArray[np.uint8] + point_height_m: npt.NDArray[np.float32] + point_step_candidate: npt.NDArray[np.uint8] + + @property + def valid(self) -> bool: + return self.state == "valid" + + def document(self) -> dict[str, object]: + return { + "schema_version": K1_LOCAL_SURFACE_SHADOW_FRAME_SCHEMA, + "frame_index": self.frame_index, + "source_frame_index": self.source_frame_index, + "session_seconds": self.session_seconds, + "state": self.state, + "valid": self.valid, + "surface": { + "pose_binding_age_ms": self.pose_binding_age_ms, + "cell_count": self.surface_cell_count, + "inlier_cell_count": self.surface_inlier_cell_count, + "plane_coefficients_map": ( + list(self.plane_coefficients_map) + if self.plane_coefficients_map is not None + else None + ), + "sensor_height_m": self.sensor_height_m, + "slope_deg": self.slope_deg, + "roughness_m": self.roughness_m, + "confidence": self.confidence, + "maximum_age_ms": self.surface_max_age_ms, + }, + "prediction": { + "available": self.prediction_available, + "cell_count": self.prediction_cell_count, + "residual_p50_m": self.prediction_residual_p50_m, + "residual_p95_m": self.prediction_residual_p95_m, + "inlier_fraction": self.prediction_inlier_fraction, + "current_frame_excluded": True, + }, + "temporal": { + "compared": self.temporal_compared, + "jump": self.temporal_jump, + "height_delta_m": self.height_delta_m, + "slope_delta_deg": self.slope_delta_deg, + "roughness_delta_m": self.roughness_delta_m, + }, + "counts": { + "surface": self.surface_point_count, + "occupied_observed": self.occupied_point_count, + "below_surface": self.below_surface_point_count, + "step_candidate": self.step_candidate_point_count, + }, + "delivery": { + "processing_ms": self.processing_ms, + "result_age_ms": self.result_age_ms, + }, + "ground_truth": False, + "occupancy_policy": { + "absence_of_points_means_free": False, + "unknown_is_traversable": False, + }, + "authority": { + "commands_enabled": False, + "navigation_or_safety_accepted": False, + }, + } + + +class K1LocalSurfaceShadowEstimator: + """Streaming-equivalent state for the accepted replay surface profile.""" + + def __init__( + self, + profile: K1LocalSurfaceProfile = DEFAULT_K1_LOCAL_SURFACE_PROFILE, + ) -> None: + self.profile = profile + self._cache: dict[tuple[int, int], tuple[float, float]] = {} + self._previous_surface: tuple[float, float, float, float] | None = None + self._last_frame_index = -1 + self._last_session_seconds = -math.inf + + def process( + self, + value: K1LocalSurfaceShadowInput, + ) -> K1LocalSurfaceShadowResult: + started_ns = time.monotonic_ns() + if ( + value.frame_index <= self._last_frame_index + or value.session_seconds < self._last_session_seconds + ): + raise LidarGroundError("K1 local-surface shadow input order is not monotonic") + self._last_frame_index = value.frame_index + self._last_session_seconds = value.session_seconds + if value.pose_binding_age_ms > self.profile.maximum_pose_binding_ms: + return self._result( + value, + started_ns=started_ns, + state="pose-stale", + ) + + cloud = value.points_map + position = value.position_map + local = np.sum((cloud[:, :2] - position[:2]) ** 2, axis=1) <= ( + self.profile.local_radius_m**2 + ) + local_cloud = cloud[local] + _expire_cache( + self._cache, + value.session_seconds, + position, + self.profile, + ) + _, prior_cell_points, _ = _local_cache_records( + self._cache, + position, + self.profile, + ) + _, current_cell_points = _cloud_cell_observations( + local_cloud, + self.profile, + ) + prediction = _prediction_metrics( + prior_cell_points, + current_cell_points, + position, + self.profile, + ) + _update_cache( + self._cache, + local_cloud, + value.session_seconds, + self.profile, + ) + cell_keys, cell_points, cell_times = _local_cache_records( + self._cache, + position, + self.profile, + ) + prediction_available = prediction is not None + prediction_cell_count = prediction.cell_points.shape[0] if prediction is not None else 0 + prediction_residual_p50_m = prediction.residual_p50_m if prediction is not None else None + prediction_residual_p95_m = prediction.residual_p95_m if prediction is not None else None + prediction_inlier_fraction = ( + prediction.inlier_fraction(self.profile.surface_band_m) + if prediction is not None + else None + ) + if cell_points.shape[0] < self.profile.minimum_surface_cells: + return self._result( + value, + started_ns=started_ns, + state="insufficient-surface", + surface_cell_count=cell_points.shape[0], + prediction_available=prediction_available, + prediction_cell_count=prediction_cell_count, + prediction_residual_p50_m=prediction_residual_p50_m, + prediction_residual_p95_m=prediction_residual_p95_m, + prediction_inlier_fraction=prediction_inlier_fraction, + ) + fit = _fit_surface(cell_points, position, self.profile) + if fit is None: + return self._result( + value, + started_ns=started_ns, + state="fit-failed", + surface_cell_count=cell_points.shape[0], + prediction_available=prediction_available, + prediction_cell_count=prediction_cell_count, + prediction_residual_p50_m=prediction_residual_p50_m, + prediction_residual_p95_m=prediction_residual_p95_m, + prediction_inlier_fraction=prediction_inlier_fraction, + ) + plane, inliers, residuals = fit + slope_deg = math.degrees( + math.atan2( + math.hypot(float(plane[0]), float(plane[1])), + float(plane[2]), + ) + ) + if not np.isfinite(slope_deg) or slope_deg > self.profile.maximum_slope_deg: + return self._result( + value, + started_ns=started_ns, + state="fit-failed", + surface_cell_count=cell_points.shape[0], + prediction_available=prediction_available, + prediction_cell_count=prediction_cell_count, + prediction_residual_p50_m=prediction_residual_p50_m, + prediction_residual_p95_m=prediction_residual_p95_m, + prediction_inlier_fraction=prediction_inlier_fraction, + ) + + heights = _height_above_plane(cloud, plane) + point_class = np.zeros(cloud.shape[0], dtype=np.uint8) + point_class[local & (np.abs(heights) <= self.profile.surface_band_m)] = POINT_SURFACE + point_class[ + local + & (heights >= self.profile.obstacle_min_height_m) + & (heights <= self.profile.obstacle_max_height_m) + ] = POINT_OCCUPIED + point_class[local & (heights < -self.profile.surface_band_m)] = POINT_BELOW_SURFACE + step_keys = _step_candidate_keys( + cell_keys, + cell_points, + plane, + self.profile, + ) + point_step_candidate = _point_step_candidates( + cloud, + local, + heights, + step_keys, + self.profile, + ) + point_height_m = np.zeros(cloud.shape[0], dtype=np.float32) + point_height_m[local] = heights[local].astype(np.float32) + + sensor_height = float(_height_above_plane(position.reshape(1, 3), plane)[0]) + roughness = float(np.median(np.abs(residuals[inliers]))) + inlier_count = int(np.count_nonzero(inliers)) + coverage = min( + 1.0, + inlier_count / (self.profile.minimum_surface_cells * 3), + ) + roughness_confidence = math.exp(-roughness / max(self.profile.surface_band_m, 1e-6)) + pose_confidence = max( + 0.0, + 1.0 - value.pose_binding_age_ms / self.profile.maximum_pose_binding_ms, + ) + confidence = float( + np.clip( + coverage * roughness_confidence * pose_confidence, + 0.0, + 1.0, + ) + ) + temporal_compared = False + temporal_jump = False + height_delta_m: float | None = None + slope_delta_deg: float | None = None + roughness_delta_m: float | None = None + if ( + self._previous_surface is not None + and value.session_seconds - self._previous_surface[0] <= self.profile.surface_ttl_s + ): + temporal_compared = True + height_delta_m = abs(sensor_height - self._previous_surface[1]) + slope_delta_deg = abs(slope_deg - self._previous_surface[2]) + roughness_delta_m = abs(roughness - self._previous_surface[3]) + temporal_jump = ( + height_delta_m > self.profile.temporal_height_jump_m + or slope_delta_deg > self.profile.temporal_slope_jump_deg + or roughness_delta_m > self.profile.temporal_roughness_jump_m + ) + self._previous_surface = ( + value.session_seconds, + sensor_height, + slope_deg, + roughness, + ) + point_class.flags.writeable = False + point_height_m.flags.writeable = False + point_step_candidate.flags.writeable = False + return self._result( + value, + started_ns=started_ns, + state="valid", + surface_cell_count=cell_points.shape[0], + surface_inlier_cell_count=inlier_count, + plane_coefficients_map=( + float(plane[0]), + float(plane[1]), + float(plane[2]), + float(plane[3]), + ), + sensor_height_m=sensor_height, + slope_deg=slope_deg, + roughness_m=roughness, + confidence=confidence, + surface_max_age_ms=max( + 0.0, + (value.session_seconds - float(np.min(cell_times[inliers]))) * 1_000.0, + ), + temporal_compared=temporal_compared, + temporal_jump=temporal_jump, + height_delta_m=height_delta_m, + slope_delta_deg=slope_delta_deg, + roughness_delta_m=roughness_delta_m, + surface_point_count=int(np.count_nonzero(point_class == POINT_SURFACE)), + occupied_point_count=int(np.count_nonzero(point_class == POINT_OCCUPIED)), + below_surface_point_count=int(np.count_nonzero(point_class == POINT_BELOW_SURFACE)), + step_candidate_point_count=int(np.count_nonzero(point_step_candidate)), + point_class=point_class, + point_height_m=point_height_m, + point_step_candidate=point_step_candidate, + prediction_available=prediction_available, + prediction_cell_count=prediction_cell_count, + prediction_residual_p50_m=prediction_residual_p50_m, + prediction_residual_p95_m=prediction_residual_p95_m, + prediction_inlier_fraction=prediction_inlier_fraction, + ) + + def _result( + self, + value: K1LocalSurfaceShadowInput, + *, + started_ns: int, + state: ShadowFrameState, + surface_cell_count: int = 0, + surface_inlier_cell_count: int = 0, + plane_coefficients_map: tuple[float, float, float, float] | None = None, + sensor_height_m: float | None = None, + slope_deg: float | None = None, + roughness_m: float | None = None, + confidence: float | None = None, + surface_max_age_ms: float | None = None, + prediction_available: bool = False, + prediction_cell_count: int = 0, + prediction_residual_p50_m: float | None = None, + prediction_residual_p95_m: float | None = None, + prediction_inlier_fraction: float | None = None, + temporal_compared: bool = False, + temporal_jump: bool = False, + height_delta_m: float | None = None, + slope_delta_deg: float | None = None, + roughness_delta_m: float | None = None, + surface_point_count: int = 0, + occupied_point_count: int = 0, + below_surface_point_count: int = 0, + step_candidate_point_count: int = 0, + point_class: npt.NDArray[np.uint8] | None = None, + point_height_m: npt.NDArray[np.float32] | None = None, + point_step_candidate: npt.NDArray[np.uint8] | None = None, + ) -> K1LocalSurfaceShadowResult: + finished_ns = time.monotonic_ns() + if point_class is None: + point_class = np.zeros(value.points_map.shape[0], dtype=np.uint8) + point_class.flags.writeable = False + if point_height_m is None: + point_height_m = np.zeros(value.points_map.shape[0], dtype=np.float32) + point_height_m.flags.writeable = False + if point_step_candidate is None: + point_step_candidate = np.zeros( + value.points_map.shape[0], + dtype=np.uint8, + ) + point_step_candidate.flags.writeable = False + return K1LocalSurfaceShadowResult( + frame_index=value.frame_index, + source_frame_index=value.source_frame_index, + session_seconds=value.session_seconds, + state=state, + pose_binding_age_ms=value.pose_binding_age_ms, + surface_cell_count=surface_cell_count, + surface_inlier_cell_count=surface_inlier_cell_count, + plane_coefficients_map=plane_coefficients_map, + sensor_height_m=sensor_height_m, + slope_deg=slope_deg, + roughness_m=roughness_m, + confidence=confidence, + surface_max_age_ms=surface_max_age_ms, + prediction_available=prediction_available, + prediction_cell_count=prediction_cell_count, + prediction_residual_p50_m=prediction_residual_p50_m, + prediction_residual_p95_m=prediction_residual_p95_m, + prediction_inlier_fraction=prediction_inlier_fraction, + temporal_compared=temporal_compared, + temporal_jump=temporal_jump, + height_delta_m=height_delta_m, + slope_delta_deg=slope_delta_deg, + roughness_delta_m=roughness_delta_m, + surface_point_count=surface_point_count, + occupied_point_count=occupied_point_count, + below_surface_point_count=below_surface_point_count, + step_candidate_point_count=step_candidate_point_count, + processing_ms=(finished_ns - started_ns) / 1_000_000, + result_age_ms=max( + 0.0, + (finished_ns - value.published_monotonic_ns) / 1_000_000, + ), + point_class=point_class, + point_height_m=point_height_m, + point_step_candidate=point_step_candidate, + ) + + +class K1LocalSurfaceShadowRuntime: + """One bounded latest-wins worker with a bounded diagnostic result ring.""" + + def __init__( + self, + session_id: str, + *, + profile: K1LocalSurfaceProfile = DEFAULT_K1_LOCAL_SURFACE_PROFILE, + queue_capacity: int = 2, + result_capacity: int = 8, + ) -> None: + if ( + not session_id + or len(session_id) > 160 + or not 1 <= queue_capacity <= 8 + or not 1 <= result_capacity <= 256 + ): + raise LidarGroundError("K1 local-surface shadow runtime bounds are invalid") + self.session_id = session_id + self.profile = profile + self._queue = LatestWinsQueue[K1LocalSurfaceShadowInput](queue_capacity) + self._result_capacity = result_capacity + self._results: deque[K1LocalSurfaceShadowResult] = deque(maxlen=result_capacity) + self._result_dropped = 0 + self._processed = 0 + self._failed = 0 + self._state_counts: Counter[str] = Counter() + self._last_error: str | None = None + self._inflight = False + self._condition = threading.Condition() + self._closed = False + self._estimator = K1LocalSurfaceShadowEstimator(profile) + self._thread = threading.Thread( + target=self._run, + name=f"k1-local-surface-shadow-{session_id}", + daemon=True, + ) + self._thread.start() + + def publish(self, value: K1LocalSurfaceShadowInput) -> None: + with self._condition: + if self._closed: + raise RuntimeError("K1 local-surface shadow runtime is closed") + self._queue.publish(value) + + def publish_views( + self, + point_cloud: DecodedPointCloudView, + pose: DecodedPoseView, + ) -> None: + self.publish(K1LocalSurfaceShadowInput.from_views(point_cloud, pose)) + + def wait_until_idle(self, timeout_seconds: float) -> bool: + if timeout_seconds < 0: + raise ValueError("K1 local-surface shadow wait timeout is invalid") + deadline = time.monotonic() + timeout_seconds + while True: + queue_snapshot = self._queue.snapshot() + with self._condition: + accounted = ( + queue_snapshot.consumed + queue_snapshot.dropped_overflow + == queue_snapshot.published + ) + if accounted and queue_snapshot.depth == 0 and not self._inflight: + return True + remaining = deadline - time.monotonic() + if remaining <= 0: + return False + self._condition.wait(timeout=remaining) + + def close(self, *, timeout_seconds: float = 30.0) -> None: + if timeout_seconds <= 0: + raise ValueError("K1 local-surface shadow close timeout is invalid") + with self._condition: + already_closed = self._closed + self._closed = True + if not already_closed: + self._queue.close() + self._thread.join(timeout=timeout_seconds) + if self._thread.is_alive(): + raise RuntimeError("K1 local-surface shadow worker did not stop") + + def results(self) -> tuple[K1LocalSurfaceShadowResult, ...]: + with self._condition: + return tuple(self._results) + + def snapshot(self) -> dict[str, Any]: + queue_snapshot = self._queue.snapshot() + with self._condition: + latest = self._results[-1] if self._results else None + return { + "schema_version": K1_LOCAL_SURFACE_SHADOW_SCHEMA, + "mode": "live-shadow-diagnostic-only", + "session_id": self.session_id, + "profile": self.profile.to_dict(), + "queue_policy": "bounded-latest-wins", + "queue": asdict(queue_snapshot), + "results": { + "capacity": self._result_capacity, + "depth": len(self._results), + "published": self._processed, + "dropped_ring_overflow": self._result_dropped, + "state_counts": dict(sorted(self._state_counts.items())), + "failed": self._failed, + "latest": latest.document() if latest is not None else None, + }, + "last_error": self._last_error, + "closed": self._closed and not self._thread.is_alive(), + "ground_truth": False, + "occupancy_policy": { + "absence_of_points_means_free": False, + "unknown_is_traversable": False, + }, + "authority": { + "commands_enabled": False, + "navigation_or_safety_accepted": False, + }, + } + + def _run(self) -> None: + while True: + value = self._queue.take_next() + if value is None: + with self._condition: + self._condition.notify_all() + return + with self._condition: + self._inflight = True + try: + result = self._estimator.process(value) + except (LidarGroundError, ValueError, np.linalg.LinAlgError) as exc: + with self._condition: + self._failed += 1 + self._last_error = type(exc).__name__ + else: + with self._condition: + if len(self._results) == self._result_capacity: + self._result_dropped += 1 + self._results.append(result) + self._processed += 1 + self._state_counts[result.state] += 1 + finally: + with self._condition: + self._inflight = False + self._condition.notify_all() diff --git a/tests/test_lidar_local_surface.py b/tests/test_lidar_local_surface.py index a1699a5..62ab882 100644 --- a/tests/test_lidar_local_surface.py +++ b/tests/test_lidar_local_surface.py @@ -2,9 +2,11 @@ from __future__ import annotations import hashlib import json +import time from pathlib import Path import numpy as np +import pytest from fastapi import APIRouter from fastapi.routing import APIRoute @@ -12,6 +14,8 @@ from k1link.compute import ( E10_LIDAR_PACK_SCHEMA, E10LidarFieldSource, K1LocalSurfaceProfile, + K1LocalSurfaceShadowInput, + K1LocalSurfaceShadowRuntime, K1LocalSurfaceV1, build_k1_local_surface, ) @@ -51,9 +55,7 @@ def _source_pack(root: Path) -> Path: zz = 0.04 * xx - 0.015 * yy + 0.008 * np.sin(xx * 2 + frame_index) ground = np.column_stack((xx.ravel(), yy.ravel(), zz.ravel())) obstacle_xy = ground[::13, :2] - obstacle_z = ( - 0.04 * obstacle_xy[:, 0] - 0.015 * obstacle_xy[:, 1] + 0.75 - ) + obstacle_z = 0.04 * obstacle_xy[:, 0] - 0.015 * obstacle_xy[:, 1] + 0.75 obstacle = np.column_stack((obstacle_xy, obstacle_z)) cloud = np.concatenate((ground, obstacle)).astype(" K1LocalSurfaceShadowInput: + offsets = source.arrays["cloud_offsets"] + start = int(offsets[frame_index]) + end = int(offsets[frame_index + 1]) + points = np.asarray( + source.arrays["cloud_points_map"][start:end], + dtype=np.float64, + ).copy() + position = np.asarray( + source.arrays["pose_positions_map"][frame_index], + dtype=np.float64, + ).copy() + points.flags.writeable = False + position.flags.writeable = False + return K1LocalSurfaceShadowInput( + frame_index=frame_index, + source_frame_index=int(source.arrays["source_frame_indices"][frame_index]), + session_seconds=float(source.arrays["session_seconds"][frame_index]), + pose_binding_age_ms=abs(float(source.arrays["pose_point_delta_ms"][frame_index])), + points_map=points, + position_map=position, + published_monotonic_ns=time.monotonic_ns(), + ) + + +def test_k1_local_surface_shadow_matches_replay_and_stays_non_authoritative( + tmp_path: Path, +) -> None: + source_path = _source_pack(tmp_path / "source") + profile = K1LocalSurfaceProfile( + profile_id="synthetic-shadow-local-surface/v1", + local_radius_m=5.0, + cell_size_m=0.5, + surface_ttl_s=0.5, + minimum_surface_cells=12, + ) + source = E10LidarFieldSource(source_path) + try: + output = build_k1_local_surface( + source, + tmp_path / "models", + profile=profile, + ) + finally: + source.close() + + source = E10LidarFieldSource(source_path) + model = K1LocalSurfaceV1(output) + runtime = K1LocalSurfaceShadowRuntime( + "synthetic-shadow", + profile=profile, + queue_capacity=2, + result_capacity=16, + ) + try: + published = [] + for frame_index in range(source.frame_count): + if not bool(source.arrays["sample_available"][frame_index]): + continue + runtime.publish(_shadow_input(source, frame_index)) + assert runtime.wait_until_idle(2.0) + published.append(frame_index) + runtime.close() + results = runtime.results() + assert [item.frame_index for item in results] == published + offsets = source.arrays["cloud_offsets"] + for result in results: + frame_index = result.frame_index + start = int(offsets[frame_index]) + end = int(offsets[frame_index + 1]) + if bool(model.arrays["frame_valid"][frame_index]): + assert result.state == "valid" + assert result.sensor_height_m == pytest.approx( + float(model.arrays["sensor_height_m"][frame_index]), + abs=1e-12, + ) + assert result.slope_deg == pytest.approx( + float(model.arrays["slope_deg"][frame_index]), + abs=1e-12, + ) + assert result.roughness_m == pytest.approx( + float(model.arrays["roughness_m"][frame_index]), + abs=1e-12, + ) + assert result.confidence == pytest.approx( + float(model.arrays["confidence"][frame_index]), + abs=1e-12, + ) + assert np.array_equal( + result.point_class, + model.arrays["point_class"][start:end], + ) + assert np.array_equal( + result.point_step_candidate, + model.arrays["point_step_candidate"][start:end], + ) + else: + assert result.state == "pose-stale" + snapshot = runtime.snapshot() + assert snapshot["queue"]["capacity"] == 2 + assert snapshot["queue"]["published"] == len(published) + assert snapshot["queue"]["consumed"] == len(published) + assert snapshot["queue"]["dropped_overflow"] == 0 + assert snapshot["results"]["failed"] == 0 + assert snapshot["occupancy_policy"]["absence_of_points_means_free"] is False + assert snapshot["authority"]["commands_enabled"] is False + assert snapshot["authority"]["navigation_or_safety_accepted"] is False + assert snapshot["closed"] is True + finally: + runtime.close() + source.close() + model.close() + + +def test_k1_local_surface_shadow_overload_is_bounded_and_latest_wins( + tmp_path: Path, +) -> None: + source_path = _source_pack(tmp_path / "source") + source = E10LidarFieldSource(source_path) + runtime = K1LocalSurfaceShadowRuntime( + "synthetic-overload", + profile=K1LocalSurfaceProfile( + profile_id="synthetic-shadow-overload/v1", + local_radius_m=5.0, + cell_size_m=0.5, + minimum_surface_cells=12, + ), + queue_capacity=1, + result_capacity=2, + ) + try: + template = _shadow_input(source, 0) + for frame_index in range(200): + runtime.publish( + K1LocalSurfaceShadowInput( + frame_index=frame_index, + source_frame_index=10_000 + frame_index, + session_seconds=10.0 + frame_index * 0.1, + pose_binding_age_ms=4.0, + points_map=template.points_map, + position_map=template.position_map, + published_monotonic_ns=time.monotonic_ns(), + ) + ) + runtime.close() + snapshot = runtime.snapshot() + queue = snapshot["queue"] + assert queue["maximum_depth"] <= queue["capacity"] == 1 + assert queue["dropped_overflow"] > 0 + assert queue["consumed"] + queue["dropped_overflow"] == queue["published"] + assert snapshot["results"]["depth"] <= 2 + assert runtime.results()[-1].frame_index == 199 + assert snapshot["results"]["latest"]["frame_index"] == 199 + assert snapshot["authority"]["commands_enabled"] is False + finally: + runtime.close() + source.close()