444 lines
16 KiB
Python
444 lines
16 KiB
Python
"""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",
|
|
]
|