141 lines
5.7 KiB
Python
141 lines
5.7 KiB
Python
"""Expiry of individual TGS cells, independent of models and transport.
|
|
|
|
Uses the existing TGS states 0=unobserved, 1=ground, 2=occupied, 3=rejected
|
|
and advisory actions 0=allow-candidate, 1=high-cost, 2=no-go. Expiry can only
|
|
remove permission. Retained occupied cells keep their prohibition. This is not
|
|
vehicle clearance, spatial reprojection validation, or motor authority.
|
|
"""
|
|
|
|
from __future__ import annotations
|
|
|
|
from dataclasses import dataclass
|
|
from typing import Final
|
|
|
|
from .realtime_contract import RealtimeContractError, _integer, _number
|
|
from .realtime_scene import _read_wire_integer, _wire_integer
|
|
|
|
CELL_FRESHNESS_SCHEMA: Final = "missioncore.costmap-cell-freshness/v1"
|
|
MAXIMUM_CELLS: Final = 8192
|
|
|
|
|
|
@dataclass(frozen=True, slots=True)
|
|
class CellAssessment:
|
|
states: tuple[int, ...]
|
|
actions: tuple[int, ...]
|
|
expired_cells: int
|
|
missing_support_cells: int
|
|
suppressed_permissions: int
|
|
oldest_permissive_source_time_ns: int
|
|
|
|
|
|
@dataclass(frozen=True, slots=True)
|
|
class CostmapCellEvidence:
|
|
"""Index-aligned with one immutable grid/state/material payload in a scene.
|
|
|
|
The enclosing scene binds epoch/clock/sequence and hashes this descriptor
|
|
together with the grid outputs. No timestamp is manufactured for unseen
|
|
cells; no coordinate transformation or observation is generated here.
|
|
"""
|
|
|
|
observed_source_time_ns: tuple[int | None, ...]
|
|
|
|
def __post_init__(self) -> None:
|
|
if (
|
|
not isinstance(self.observed_source_time_ns, tuple)
|
|
or not 1 <= len(self.observed_source_time_ns) <= MAXIMUM_CELLS
|
|
):
|
|
raise RealtimeContractError("costmap cell count outside bound")
|
|
for stamp in self.observed_source_time_ns:
|
|
if stamp is not None:
|
|
_wire_integer(stamp)
|
|
|
|
def to_dict(self) -> dict[str, object]:
|
|
return {
|
|
"schema_version": CELL_FRESHNESS_SCHEMA,
|
|
"observed_source_time_ns": [
|
|
None if stamp is None else _wire_integer(stamp)
|
|
for stamp in self.observed_source_time_ns
|
|
],
|
|
}
|
|
|
|
@classmethod
|
|
def from_dict(cls, value: object) -> CostmapCellEvidence:
|
|
if (
|
|
not isinstance(value, dict)
|
|
or set(value) != {"schema_version", "observed_source_time_ns"}
|
|
or value["schema_version"] != CELL_FRESHNESS_SCHEMA
|
|
or not isinstance(value["observed_source_time_ns"], list)
|
|
or not 1 <= len(value["observed_source_time_ns"]) <= MAXIMUM_CELLS
|
|
):
|
|
raise RealtimeContractError("costmap cell evidence schema or bound changed")
|
|
return cls(
|
|
tuple(
|
|
None if stamp is None else _read_wire_integer(stamp)
|
|
for stamp in value["observed_source_time_ns"]
|
|
)
|
|
)
|
|
|
|
def assess(
|
|
self,
|
|
*,
|
|
states: tuple[int, ...],
|
|
actions: tuple[int, ...],
|
|
source_time_ns: int,
|
|
observed_source_time_ns: int,
|
|
maximum_age_ms: float,
|
|
clock_uncertainty_ms: float,
|
|
) -> CellAssessment:
|
|
"""Produce a new view; repeated aging can never restore permissions.
|
|
|
|
The same mapped source clock as SceneFreshness must be used. Mandatory
|
|
segmentation/pose/geometry freshness is still checked for the WHOLE scene
|
|
separately. Passing individual cells cannot override missing inputs.
|
|
"""
|
|
_integer(source_time_ns, "source_time_ns")
|
|
_integer(observed_source_time_ns, "observed_source_time_ns")
|
|
_number(maximum_age_ms, "maximum_age_ms", minimum=0.001)
|
|
_number(clock_uncertainty_ms, "clock_uncertainty_ms")
|
|
if observed_source_time_ns < source_time_ns:
|
|
raise RealtimeContractError("costmap assessment precedes source observation")
|
|
if (
|
|
not isinstance(states, tuple)
|
|
or not isinstance(actions, tuple)
|
|
or len(states) != len(actions)
|
|
or len(states) != len(self.observed_source_time_ns)
|
|
):
|
|
raise RealtimeContractError("costmap arrays must share one bounded grid")
|
|
output_states, output_actions, permissive_stamps = [], [], []
|
|
expired = missing = suppressed = 0
|
|
for state, action, stamp in zip(states, actions, self.observed_source_time_ns, strict=True):
|
|
if type(state) is not int or not 0 <= state <= 3:
|
|
raise RealtimeContractError("invalid TGS state")
|
|
if type(action) is not int or not 0 <= action <= 2:
|
|
raise RealtimeContractError("invalid advisory action")
|
|
if state != 1 and action != 2:
|
|
raise RealtimeContractError("non-ground cell attempted terrain permission")
|
|
if stamp is not None and stamp > source_time_ns:
|
|
raise RealtimeContractError("future cell observation")
|
|
too_old = stamp is not None and (
|
|
(observed_source_time_ns - stamp) / 1e6 + clock_uncertainty_ms > maximum_age_ms
|
|
)
|
|
# Existing occupied/unknown/rejected cells never become free. Only
|
|
# positive ground evidence requires a current supporting observation.
|
|
if state == 1 and (stamp is None or too_old):
|
|
expired += int(too_old)
|
|
missing += int(stamp is None)
|
|
suppressed += int(action != 2)
|
|
state, action = 3, 2
|
|
if action != 2:
|
|
assert stamp is not None # validated positive ground support above
|
|
permissive_stamps.append(stamp)
|
|
output_states.append(state)
|
|
output_actions.append(action)
|
|
return CellAssessment(
|
|
tuple(output_states),
|
|
tuple(output_actions),
|
|
expired,
|
|
missing,
|
|
suppressed,
|
|
min(permissive_stamps, default=source_time_ns),
|
|
)
|