"""Bounded rolling occupancy reconstructed from registered map increments. The K1 ``lio_pcl`` recording is a sequence of post-LIO map increments, not a complete scan at every timestamp. This provider preserves the exact current increment elsewhere and materializes only the still-valid *retained* cells which a later increment did not need to publish again. Missing points never clear occupancy. """ from __future__ import annotations import hashlib import json import math from collections import deque from dataclasses import dataclass from pathlib import Path from typing import Final, Protocol from .contracts import GridCell, HistorySample, MotionState, TemporalObstacle, TemporalState from .providers import SourcePacket ROLLING_MAP_PROFILE_SCHEMA: Final = "missioncore.rolling-local-map-profile/v1" ROLLING_MAP_PROVIDER_ID: Final = "rolling-local-obstacle-map/v1" DEFAULT_ROLLING_MAP_PROFILE_PATH: Final = Path( "config/perception/m4-rolling-local-map-v1.json" ) class RollingMapError(RuntimeError): """The rolling map input, bounds or state is incompatible.""" class ReplayPoseResolver(Protocol): def pose_values_for_frame( self, frame_id: str, ) -> tuple[tuple[float, float, float], tuple[float, float, float, float]] | None: ... @dataclass(frozen=True, slots=True) class RollingMapBounds: coordinate_frame: str voxel_size_m: float retention_seconds: float local_radius_m: float maximum_cells: int maximum_cells_per_component: int maximum_components: int neighbor_radius_cells: int def __post_init__(self) -> None: numeric = ( self.voxel_size_m, self.retention_seconds, self.local_radius_m, ) if any(not _positive_finite(value) for value in numeric): raise RollingMapError("rolling map numeric bound is invalid") integer = ( self.maximum_cells, self.maximum_cells_per_component, self.maximum_components, self.neighbor_radius_cells, ) if any(not _positive_integer(value) for value in integer): raise RollingMapError("rolling map integer bound is invalid") if self.maximum_cells_per_component > self.maximum_cells: raise RollingMapError("rolling component bound exceeds map capacity") @property def retention_ns(self) -> int: return round(self.retention_seconds * 1_000_000_000) @dataclass(frozen=True, slots=True) class RollingMapProfile: profile_id: str source_id: str session_id: str representation_id: str bounds: RollingMapBounds profile_sha256: str @dataclass(frozen=True, slots=True) class RollingMapSnapshot: voxel_size_m: float retention_ns: int local_radius_m: float maximum_cells: int input_frames: int current_increment_cells: int retained_component_publications: int retained_cell_publications: int time_evicted_cells: int radius_evicted_cells: int capacity_evicted_cells: int active_cells_at_end: int peak_active_cells: int peak_retained_components: int maximum_retained_age_ns: int @dataclass(slots=True) class _CellEvidence: first_hit_ns: int last_hit_ns: int last_frame_id: str hit_count: int class RollingLocalObstacleMapProvider: """Accumulate occupied map cells without inventing scan-based clearing.""" provider_id: str = ROLLING_MAP_PROVIDER_ID def __init__( self, *, pose_resolver: ReplayPoseResolver, profile: RollingMapProfile, ) -> None: self.pose_resolver = pose_resolver self.profile = profile self.config = profile.bounds self._cells: dict[GridCell, _CellEvidence] = {} self._previous_sequence: int | None = None self._previous_time_ns: int | None = None self._input_frames = 0 self._current_increment_cells = 0 self._retained_component_publications = 0 self._retained_cell_publications = 0 self._time_evicted_cells = 0 self._radius_evicted_cells = 0 self._peak_active_cells = 0 self._peak_retained_components = 0 self._maximum_retained_age_ns = 0 def update( self, packet: SourcePacket, temporal_obstacles: tuple[TemporalObstacle, ...], ) -> tuple[TemporalObstacle, ...]: self._validate(packet) now_ns = packet.envelope.timestamps.source_ns current_cells = { cell for obstacle in temporal_obstacles if obstacle.state is TemporalState.CURRENT for cell in obstacle.cells } self._current_increment_cells += len(current_cells) for cell in current_cells: evidence = self._cells.get(cell) if evidence is None: self._cells[cell] = _CellEvidence( first_hit_ns=now_ns, last_hit_ns=now_ns, last_frame_id=packet.envelope.frame_id, hit_count=1, ) else: evidence.last_hit_ns = now_ns evidence.last_frame_id = packet.envelope.frame_id evidence.hit_count += 1 self._evict_by_time(now_ns) self._evict_by_radius(packet) if len(self._cells) > self.config.maximum_cells: raise RollingMapError( "rolling map capacity exceeded; dropping occupied cells is forbidden" ) retained_cells = set(self._cells) - current_cells components = self._components(retained_cells) if len(components) > self.config.maximum_components: raise RollingMapError("rolling map component bound exceeded") result = tuple(self._contract(packet, cells) for cells in components) self._retained_component_publications += len(result) self._retained_cell_publications += sum(len(item.cells) for item in result) self._peak_active_cells = max(self._peak_active_cells, len(self._cells)) self._peak_retained_components = max( self._peak_retained_components, len(result), ) self._previous_sequence = packet.envelope.sequence self._previous_time_ns = now_ns self._input_frames += 1 return result def _validate(self, packet: SourcePacket) -> None: envelope = packet.envelope if ( envelope.source_id != self.profile.source_id or envelope.session_id != self.profile.session_id or envelope.representation_id != self.profile.representation_id ): raise RollingMapError("packet escaped the rolling map source profile") now_ns = envelope.timestamps.source_ns if self._previous_sequence is not None and ( envelope.sequence <= self._previous_sequence or self._previous_time_ns is None or now_ns <= self._previous_time_ns ): raise RollingMapError("rolling map packet order is not monotonic") def _evict_by_time(self, now_ns: int) -> None: expired = tuple( cell for cell, evidence in self._cells.items() if now_ns - evidence.last_hit_ns > self.config.retention_ns ) for cell in expired: del self._cells[cell] self._time_evicted_cells += len(expired) def _evict_by_radius(self, packet: SourcePacket) -> None: pose = self.pose_resolver.pose_values_for_frame(packet.envelope.frame_id) if pose is None: return x, y, _ = pose[0] radius_squared = self.config.local_radius_m**2 evicted = tuple( cell for cell in self._cells if ( ((cell.x + 0.5) * self.config.voxel_size_m - x) ** 2 + ((cell.y + 0.5) * self.config.voxel_size_m - y) ** 2 > radius_squared ) ) for cell in evicted: del self._cells[cell] self._radius_evicted_cells += len(evicted) def _components(self, cells: set[GridCell]) -> tuple[frozenset[GridCell], ...]: remaining = set(cells) components: list[frozenset[GridCell]] = [] radius = self.config.neighbor_radius_cells offsets = tuple( (dx, dy, dz) for dx in range(-radius, radius + 1) for dy in range(-radius, radius + 1) for dz in range(-radius, radius + 1) if dx or dy or dz ) while remaining: start = min(remaining, key=_cell_key) remaining.remove(start) connected = {start} queue = deque((start,)) while queue: cell = queue.popleft() for dx, dy, dz in offsets: neighbor = GridCell(cell.x + dx, cell.y + dy, cell.z + dz) if neighbor not in remaining: continue remaining.remove(neighbor) connected.add(neighbor) queue.append(neighbor) if len(connected) > self.config.maximum_cells_per_component: raise RollingMapError("rolling map component cell bound exceeded") components.append(frozenset(connected)) return tuple(sorted(components, key=lambda value: _cell_key(min(value, key=_cell_key)))) def _contract( self, packet: SourcePacket, cells: frozenset[GridCell], ) -> TemporalObstacle: now_ns = packet.envelope.timestamps.source_ns last_hit_ns = max(self._cells[cell].last_hit_ns for cell in cells) age_ns = now_ns - last_hit_ns if not 0 < age_ns <= self.config.retention_ns: raise RollingMapError("retained component escaped rolling bounds") centers = tuple( ( (cell.x + 0.5) * self.config.voxel_size_m, (cell.y + 0.5) * self.config.voxel_size_m, (cell.z + 0.5) * self.config.voxel_size_m, ) for cell in cells ) centroid = ( sum(point[0] for point in centers) / len(centers), sum(point[1] for point in centers) / len(centers), sum(point[2] for point in centers) / len(centers), ) digest = hashlib.sha256( ";".join( f"{cell.x},{cell.y},{cell.z}" for cell in sorted(cells, key=_cell_key) ).encode() ).hexdigest()[:24] self._maximum_retained_age_ns = max(self._maximum_retained_age_ns, age_ns) latest_frame = min( evidence.last_frame_id for cell in cells if (evidence := self._cells[cell]).last_hit_ns == last_hit_ns ) return TemporalObstacle( component_id=f"rolling-{digest}", identity_scope="ephemeral", state=TemporalState.RETAINED, ttl_ns=self.config.retention_ns, last_hit_ns=last_hit_ns, age_ns=age_ns, association_basis="registered-map-increment-retention", history=( HistorySample( frame_id=latest_frame, evidence_time_ns=last_hit_ns, centroid_xyz_m=centroid, ), ), cells=tuple(sorted(cells, key=_cell_key)), coordinate_frame=self.config.coordinate_frame, last_centroid_xyz_m=centroid, motion=MotionState.UNKNOWN, motion_confidence=0.0, motion_reason="retained-map-increment-no-current-motion", semantic_hint=None, ) def snapshot(self) -> RollingMapSnapshot: return RollingMapSnapshot( voxel_size_m=self.config.voxel_size_m, retention_ns=self.config.retention_ns, local_radius_m=self.config.local_radius_m, maximum_cells=self.config.maximum_cells, input_frames=self._input_frames, current_increment_cells=self._current_increment_cells, retained_component_publications=self._retained_component_publications, retained_cell_publications=self._retained_cell_publications, time_evicted_cells=self._time_evicted_cells, radius_evicted_cells=self._radius_evicted_cells, capacity_evicted_cells=0, active_cells_at_end=len(self._cells), peak_active_cells=self._peak_active_cells, peak_retained_components=self._peak_retained_components, maximum_retained_age_ns=self._maximum_retained_age_ns, ) def load_rolling_map_profile(path: Path) -> RollingMapProfile: resolved = path.resolve(strict=True) if resolved.is_symlink() or not resolved.is_file(): raise RollingMapError("rolling map profile is not a regular file") raw = resolved.read_bytes() try: document = _object(json.loads(raw), "rolling map profile") except json.JSONDecodeError as exc: raise RollingMapError("rolling map profile JSON is invalid") from exc _exact_keys( document, {"schema_version", "profile_id", "provider_id", "source", "bounds", "policy", "authority"}, "rolling map profile", ) if ( document["schema_version"] != ROLLING_MAP_PROFILE_SCHEMA or document["provider_id"] != ROLLING_MAP_PROVIDER_ID ): raise RollingMapError("rolling map profile identity is incompatible") source = _object(document["source"], "rolling map source") bounds = _object(document["bounds"], "rolling map bounds") _exact_keys(source, {"source_id", "session_id", "representation_id"}, "rolling map source") _exact_keys(bounds, set(RollingMapBounds.__dataclass_fields__), "rolling map bounds") if document["policy"] != { "input_is_complete_scan": False, "input_is_registered_map_increment": True, "absence_of_republication_means_free": False, "clearing_from_missing_points": False, "retained_occupancy_can_assert_threat": True, "retained_motion_claimed": False, "local_radius_eviction": True, "time_bound_eviction": True, "capacity_eviction_allowed": False, }: raise RollingMapError("rolling map policy is incompatible") if document["authority"] != { "ground_truth": False, "physical_live": False, "commands_enabled": False, "actuation_allowed": False, "navigation_or_safety_accepted": False, }: raise RollingMapError("rolling map authority is incompatible") return RollingMapProfile( profile_id=_string(document, "profile_id"), source_id=_string(source, "source_id"), session_id=_string(source, "session_id"), representation_id=_string(source, "representation_id"), bounds=RollingMapBounds(**bounds), # type: ignore[arg-type] profile_sha256=hashlib.sha256(raw).hexdigest(), ) def _cell_key(cell: GridCell) -> tuple[int, int, int]: return cell.x, cell.y, cell.z def _positive_finite(value: object) -> bool: return ( isinstance(value, (int, float)) and not isinstance(value, bool) and math.isfinite(float(value)) and float(value) > 0.0 ) def _positive_integer(value: object) -> bool: return isinstance(value, int) and not isinstance(value, bool) and value > 0 def _object(value: object, label: str) -> dict[str, object]: if not isinstance(value, dict) or any(not isinstance(key, str) for key in value): raise RollingMapError(f"{label} must be an object") return value def _exact_keys(document: dict[str, object], expected: set[str], label: str) -> None: if set(document) != expected: raise RollingMapError(f"{label} fields are incompatible") def _string(document: dict[str, object], key: str) -> str: value = document.get(key) if not isinstance(value, str) or not value: raise RollingMapError(f"{key} must be a nonempty string") return value __all__ = [ "DEFAULT_ROLLING_MAP_PROFILE_PATH", "ROLLING_MAP_PROFILE_SCHEMA", "ROLLING_MAP_PROVIDER_ID", "RollingLocalObstacleMapProvider", "RollingMapBounds", "RollingMapError", "RollingMapProfile", "RollingMapSnapshot", "load_rolling_map_profile", ]