From c23a2fdc2cd35e6930e40a0674af54d9b1639e3a Mon Sep 17 00:00:00 2001 From: ll7 <32880741+ll7@users.noreply.github.com> Date: Fri, 4 Sep 2026 13:00:29 +0200 Subject: [PATCH 01/10] feat(planner): add identity-keyed belief projection --- ...2538_scenario_belief_planner_projection.md | 18 +- robot_sf/planner/scenario_belief_adapter.py | 738 +++++++++++++++++- .../test_scenario_belief_track_projection.py | 221 ++++++ 3 files changed, 972 insertions(+), 5 deletions(-) create mode 100644 tests/planner/test_scenario_belief_track_projection.py diff --git a/docs/context/issue_2538_scenario_belief_planner_projection.md b/docs/context/issue_2538_scenario_belief_planner_projection.md index 39b245e2ea..c967a26452 100644 --- a/docs/context/issue_2538_scenario_belief_planner_projection.md +++ b/docs/context/issue_2538_scenario_belief_planner_projection.md @@ -29,6 +29,19 @@ Issue #2538 adds a planner-facing ScenarioBelief projection helper: - The stream-gap planner remains opt-in for uncertainty consumption. Missing or malformed uncertainty metadata still keeps deterministic pedestrian rows. +The additive issue #8050 diagnostic seam also provides +`project_belief_aware_planner_input(...)` for the explicitly named +`BeliefGuidedLocalPlanner`. It retains every canonical `ScenarioBelief.agents` entry in an +immutable, ID-keyed `tracks` mapping, including entries absent from the visible legacy rows, and +reports distinct `no_belief`, `empty_belief`, `unsupported_planner`, `invalid_belief`, and +`projected` statuses. Serialization is versioned and deterministic; legacy observations and the +existing stream-gap path are unchanged. + +The current `ScenarioBelief` owner does not expose retirement generations. The new diagnostic +therefore reports `identity_generation_available: false`, marks its entity-ID token as not +reuse-safe, and requires stateful consumers to reset at an externally supplied lifecycle boundary. +It does not infer retirement, reuse, or benchmark/safety benefit. + ## Claim Boundary This proves only that ScenarioBelief uncertainty metadata can reach one planner-compatible local @@ -47,5 +60,6 @@ uv run ruff format --check robot_sf/planner/scenario_belief_adapter.py tests/pla ## Follow-Up The next useful step is a runtime observation-builder path that produces a ScenarioBelief during an -environment step and routes this projection into a planner selection or smoke command. Until that -exists, this remains a unit-level planner interface smoke. +environment step and routes this projection into a planner selection or smoke command. A canonical +track-generation/retirement owner is also required before a stateful planner can claim reuse-safe +identity semantics. Until those gates exist, this remains a unit-level planner interface smoke. diff --git a/robot_sf/planner/scenario_belief_adapter.py b/robot_sf/planner/scenario_belief_adapter.py index 12ac55b80c..f7bdc18ae8 100644 --- a/robot_sf/planner/scenario_belief_adapter.py +++ b/robot_sf/planner/scenario_belief_adapter.py @@ -7,16 +7,22 @@ from __future__ import annotations +import json +from collections.abc import Mapping from dataclasses import dataclass -from typing import TYPE_CHECKING, Any +from types import MappingProxyType +from typing import Any import numpy as np -if TYPE_CHECKING: - from robot_sf.representation import ScenarioBelief +from robot_sf.representation.scenario_belief import ScenarioBelief, VisibilityState SCENARIO_BELIEF_PLANNER_PROJECTION_SCHEMA_VERSION = "scenario-belief-planner-projection.v1" SUPPORTED_UNCERTAINTY_PLANNER_KEYS = frozenset({"stream_gap"}) +BELIEF_AWARE_PLANNER_INPUT_SCHEMA_VERSION = "belief-aware-planner-input.v1" +SUPPORTED_BELIEF_AWARE_PLANNER_NAMES = frozenset({"BeliefGuidedLocalPlanner"}) +# Keep the key-shaped alias discoverable for callers that use the existing adapter vocabulary. +SUPPORTED_BELIEF_AWARE_PLANNER_KEYS = SUPPORTED_BELIEF_AWARE_PLANNER_NAMES @dataclass(frozen=True) @@ -128,9 +134,735 @@ def project_scenario_belief_for_planner( return ScenarioBeliefPlannerProjection(observation=observation, compatibility=compatibility) +def _readonly_float_array( + name: str, + value: Any, + *, + shape: tuple[int, ...], +) -> np.ndarray: + """Validate and own one finite floating-point array for a planner record. + + Returns: + An owned, read-only float64 array with the requested shape. + """ + try: + array = np.asarray(value) + except (TypeError, ValueError) as exc: + raise ValueError(f"{name} must be a numeric array") from exc + if array.shape != shape: + raise ValueError(f"{name} must have shape {shape}, got {array.shape}") + if not np.issubdtype(array.dtype, np.number): + raise ValueError(f"{name} must use a numeric dtype") + try: + owned = np.array(array, dtype=np.float64, copy=True) + except (TypeError, ValueError) as exc: + raise ValueError(f"{name} must be a numeric array") from exc + if not np.all(np.isfinite(owned)): + raise ValueError(f"{name} must contain only finite values") + owned.setflags(write=False) + return owned + + +def _readonly_covariance(value: Any) -> np.ndarray: + """Validate the canonical 5D state covariance and return an owned copy. + + ``ScenarioBelief`` owns independent 2D position and velocity covariance + matrices. The planner state is ``[x, y, vx, vy, radius]``; the adapter + embeds those two blocks and uses a deterministic zero-variance radius block + because the current canonical owner has no radius uncertainty or cross terms. + A 4x4 block matrix is accepted for standalone typed-record construction and + is normalized to the same 5x5 representation. + + Returns: + An owned, read-only 5x5 positive-semidefinite covariance matrix. + """ + try: + array = np.asarray(value) + except (TypeError, ValueError) as exc: + raise ValueError("covariance must be a numeric array") from exc + if array.shape == (4, 4): + try: + normalized = np.zeros((5, 5), dtype=np.float64) + normalized[:4, :4] = np.asarray(array, dtype=np.float64) + except (TypeError, ValueError) as exc: + raise ValueError("covariance must be numeric") from exc + elif array.shape == (5, 5): + try: + normalized = np.asarray(array, dtype=np.float64) + except (TypeError, ValueError) as exc: + raise ValueError("covariance must be numeric") from exc + else: + raise ValueError(f"covariance must have shape (5, 5) or (4, 4), got {array.shape}") + if not np.all(np.isfinite(normalized)): + raise ValueError("covariance must contain only finite values") + if not np.allclose(normalized, normalized.T, atol=1e-8, rtol=0.0): + raise ValueError("covariance must be symmetric") + if np.any(np.linalg.eigvalsh(normalized) < -1e-8): + raise ValueError("covariance must be positive semidefinite") + owned = np.array(normalized, dtype=np.float64, copy=True) + owned.setflags(write=False) + return owned + + +def _validate_probability(name: str, value: Any) -> float: + """Return a finite probability in the closed unit interval.""" + try: + normalized = float(value) + except (TypeError, ValueError) as exc: + raise ValueError(f"{name} must be a finite value in [0, 1]") from exc + if not np.isfinite(normalized) or not 0.0 <= normalized <= 1.0: + raise ValueError(f"{name} must be a finite value in [0, 1]") + return normalized + + +def _validate_nonnegative_int(name: str, value: Any) -> int: + """Return a non-negative integer without truncating fractional input.""" + try: + normalized = int(value) + except (TypeError, ValueError, OverflowError) as exc: + raise ValueError(f"{name} must be a non-negative integer") from exc + if normalized < 0 or normalized != value: + raise ValueError(f"{name} must be a non-negative integer") + return normalized + + +def _validate_track_id(value: Any) -> str | int: + """Validate a stable string or integer track identifier. + + Current ``ScenarioBelief`` uses string entity IDs. Integer IDs remain + accepted for interoperability with the canonical prediction types, but are + never synthesized from an observation-row position. + + Returns: + The validated string or normalized built-in integer ID. + """ + if isinstance(value, bool): + raise ValueError("track_id must be a non-empty string or integer") + if isinstance(value, (int, np.integer)): + return int(value) + if isinstance(value, str) and value: + return value + raise ValueError("track_id must be a non-empty string or integer") + + +def _track_sort_key(track_id: str | int) -> tuple[int, str | int]: + """Return a deterministic ordering key for supported track-ID types.""" + if isinstance(track_id, int): + return (0, track_id) + return (1, track_id) + + +@dataclass(frozen=True) +class PlannerTrackBelief: + """Immutable, track-keyed planner state from one maintained belief. + + ``track_id`` is the canonical entity identity, not a row number. The + current ScenarioBelief owner supplies string IDs; integer IDs are retained + only for standalone typed interoperability. ``covariance`` uses state + order ``[x, y, vx, vy, radius]`` and is a 5x5 owned, read-only array. + + The current ScenarioBelief contract does not expose a track generation or + retirement epoch. ``identity_lifecycle_token`` therefore identifies the + canonical entity ID only and is explicitly *not* a reuse-safe generation. + Stateful consumers must reset on a canonical lifecycle event until the + representation owner supplies that missing generation. + """ + + track_id: str | int + mean_state: np.ndarray + covariance: np.ndarray + confidence: float + existence_probability: float + visibility: bool + age_steps: int + source: str + position_confidence: float | None = None + velocity_confidence: float | None = None + visibility_state: str | None = None + identity_lifecycle_token: str | None = None + + def __post_init__(self) -> None: + """Validate and defensively normalize all planner-track fields.""" + track_id = _validate_track_id(self.track_id) + object.__setattr__(self, "track_id", track_id) + object.__setattr__( + self, + "mean_state", + _readonly_float_array("mean_state", self.mean_state, shape=(5,)), + ) + object.__setattr__(self, "covariance", _readonly_covariance(self.covariance)) + object.__setattr__(self, "confidence", _validate_probability("confidence", self.confidence)) + object.__setattr__( + self, + "existence_probability", + _validate_probability("existence_probability", self.existence_probability), + ) + if not isinstance(self.visibility, (bool, np.bool_)): + raise ValueError("visibility must be a boolean") + object.__setattr__(self, "visibility", bool(self.visibility)) + object.__setattr__( + self, "age_steps", _validate_nonnegative_int("age_steps", self.age_steps) + ) + if not isinstance(self.source, str) or not self.source: + raise ValueError("source must be a non-empty string") + if self.position_confidence is not None: + object.__setattr__( + self, + "position_confidence", + _validate_probability("position_confidence", self.position_confidence), + ) + if self.velocity_confidence is not None: + object.__setattr__( + self, + "velocity_confidence", + _validate_probability("velocity_confidence", self.velocity_confidence), + ) + if self.visibility_state is not None and ( + not isinstance(self.visibility_state, str) or not self.visibility_state + ): + raise ValueError("visibility_state must be a non-empty string when provided") + lifecycle_token = self.identity_lifecycle_token + if lifecycle_token is None: + lifecycle_token = f"entity-id:{track_id}" + if not isinstance(lifecycle_token, str) or not lifecycle_token: + raise ValueError("identity_lifecycle_token must be a non-empty string") + object.__setattr__(self, "identity_lifecycle_token", lifecycle_token) + + def to_dict(self) -> dict[str, Any]: + """Return a deterministic JSON-safe track mapping.""" + payload: dict[str, Any] = { + "track_id": self.track_id, + "mean_state": [float(value) for value in self.mean_state], + "covariance": self.covariance.tolist(), + "confidence": float(self.confidence), + "existence_probability": float(self.existence_probability), + "visibility": self.visibility, + "age_steps": self.age_steps, + "source": self.source, + "identity_lifecycle_token": self.identity_lifecycle_token, + } + if self.position_confidence is not None: + payload["position_confidence"] = float(self.position_confidence) + if self.velocity_confidence is not None: + payload["velocity_confidence"] = float(self.velocity_confidence) + if self.visibility_state is not None: + payload["visibility_state"] = self.visibility_state + return payload + + +def _copy_runtime_value(value: Any) -> Any: + """Copy nested observation values while owning and freezing NumPy arrays. + + Returns: + A recursively copied runtime value with independent read-only arrays. + """ + if isinstance(value, np.ndarray): + copied = np.array(value, copy=True) + copied.setflags(write=False) + return copied + if isinstance(value, Mapping): + return {key: _copy_runtime_value(nested) for key, nested in value.items()} + if isinstance(value, list): + return [_copy_runtime_value(nested) for nested in value] + if isinstance(value, tuple): + return tuple(_copy_runtime_value(nested) for nested in value) + if isinstance(value, np.generic): + return value.item() + return value + + +def _runtime_value_is_finite(value: Any) -> bool: + """Return whether nested numeric runtime values are finite.""" + if isinstance(value, np.ndarray): + if value.dtype.kind in "fc": + return bool(np.all(np.isfinite(value))) + return value.dtype.kind in "biu" + if isinstance(value, np.generic): + if np.issubdtype(value.dtype, np.floating): + return bool(np.isfinite(value)) + return True + if isinstance(value, Mapping): + return all(_runtime_value_is_finite(nested) for nested in value.values()) + if isinstance(value, (list, tuple)): + return all(_runtime_value_is_finite(nested) for nested in value) + if isinstance(value, float): + return bool(np.isfinite(value)) + return True + + +def _json_safe(value: Any) -> Any: + """Convert nested runtime values to JSON primitives, rejecting non-finite data. + + Returns: + A value composed only of JSON-compatible primitives and containers. + """ + if isinstance(value, np.ndarray): + return _json_safe(value.tolist()) + if isinstance(value, np.generic): + return _json_safe(value.item()) + if isinstance(value, Mapping): + return {str(key): _json_safe(nested) for key, nested in value.items()} + if isinstance(value, (list, tuple)): + return [_json_safe(nested) for nested in value] + if isinstance(value, float): + if not np.isfinite(value): + raise ValueError("JSON export cannot contain NaN or Inf") + return value + return value + + +def _validate_planner_mapping( + mapping: Mapping[str | int, PlannerTrackBelief], +) -> dict[str | int, PlannerTrackBelief]: + """Validate a track mapping without allowing key/embedded-ID drift. + + Returns: + A shallow copy of the validated mapping. + """ + normalized: dict[str | int, PlannerTrackBelief] = {} + for key, track in mapping.items(): + normalized_key = _validate_track_id(key) + if not isinstance(track, PlannerTrackBelief): + raise TypeError("tracks must contain PlannerTrackBelief values") + if normalized_key != track.track_id: + raise ValueError( + f"track key mismatch: mapping key {normalized_key!r} != track_id {track.track_id!r}" + ) + normalized[normalized_key] = track + return {track_id: normalized[track_id] for track_id in sorted(normalized, key=_track_sort_key)} + + +@dataclass(frozen=True) +class BeliefAwarePlannerInput: + """Versioned planner input preserving legacy observations and ID-keyed tracks.""" + + legacy_observation: Mapping[str, Any] + tracks: Mapping[str | int, PlannerTrackBelief] + belief_step: int + schema_version: str = BELIEF_AWARE_PLANNER_INPUT_SCHEMA_VERSION + diagnostics: Mapping[str, Any] = MappingProxyType({}) + + def __post_init__(self) -> None: + """Validate mappings and make caller-owned observation data independent.""" + if not isinstance(self.legacy_observation, Mapping): + raise TypeError("legacy_observation must be a mapping") + object.__setattr__(self, "legacy_observation", _copy_runtime_value(self.legacy_observation)) + if not isinstance(self.tracks, Mapping): + raise TypeError("tracks must be a mapping") + normalized_tracks = _validate_planner_mapping(self.tracks) + object.__setattr__(self, "tracks", MappingProxyType(normalized_tracks)) + belief_step = _validate_nonnegative_int("belief_step", self.belief_step) + object.__setattr__(self, "belief_step", belief_step) + if not isinstance(self.schema_version, str) or not self.schema_version: + raise ValueError("schema_version must be a non-empty string") + if not isinstance(self.diagnostics, Mapping): + raise TypeError("diagnostics must be a mapping") + object.__setattr__( + self, "diagnostics", MappingProxyType(_copy_runtime_value(self.diagnostics)) + ) + + @property + def projection(self) -> Mapping[str, Any]: + """Return the compact projection diagnostics under the issue vocabulary.""" + return self.diagnostics + + def ordered_track_ids(self) -> tuple[str | int, ...]: + """Return track IDs in canonical deterministic order.""" + return tuple(sorted(self.tracks, key=_track_sort_key)) + + def to_dict(self) -> dict[str, Any]: + """Return deterministic JSON-safe input and projection diagnostics.""" + tracks: dict[str, Any] = {} + for track_id in self.ordered_track_ids(): + serialized_id = str(track_id) + if serialized_id in tracks: + raise ValueError("track IDs collide after JSON object-key normalization") + tracks[serialized_id] = self.tracks[track_id].to_dict() + payload = { + "schema_version": self.schema_version, + "belief_step": self.belief_step, + "legacy_observation": _json_safe(self.legacy_observation), + "tracks": tracks, + "diagnostics": _json_safe(self.diagnostics), + } + try: + json.dumps(payload, allow_nan=False, sort_keys=True) + except (TypeError, ValueError) as exc: + raise ValueError("belief-aware planner input is not JSON-safe") from exc + return payload + + def to_json(self) -> str: + """Return a stable compact JSON representation of this planner input.""" + return json.dumps(self.to_dict(), allow_nan=False, sort_keys=True, separators=(",", ":")) + + +def _planner_name(*, planner_name: str | None, planner_key: str | None) -> str: + """Resolve the explicit planner-name spelling without permitting wildcards. + + Returns: + The one non-empty planner name supplied by the caller. + """ + if planner_name is not None and planner_key is not None and planner_name != planner_key: + raise ValueError("planner_name and planner_key must match when both are supplied") + resolved = planner_name if planner_name is not None else planner_key + if not isinstance(resolved, str) or not resolved: + raise ValueError("planner_name must be a non-empty string") + return resolved + + +def _belief_step(belief: Any) -> int: + """Derive an integral step from canonical simulation time and timestep. + + Returns: + A non-negative step aligned to the belief timestep. + """ + try: + sim_time_s = float(belief.sim_time_s) + timestep_s = float(belief.timestep_s) + except (AttributeError, TypeError, ValueError) as exc: + raise ValueError("belief time metadata is malformed") from exc + if not np.isfinite(sim_time_s) or sim_time_s < 0.0: + raise ValueError("belief sim_time_s must be finite and non-negative") + if not np.isfinite(timestep_s) or timestep_s < 0.0: + raise ValueError("belief timestep_s must be finite and non-negative") + if timestep_s == 0.0: + if sim_time_s == 0.0: + return 0 + raise ValueError("belief timestep_s must be positive when sim_time_s is non-zero") + ratio = sim_time_s / timestep_s + rounded = round(ratio) + if not np.isclose(ratio, rounded, atol=1e-6, rtol=0.0): + raise ValueError("belief sim_time_s is not aligned to timestep_s") + return rounded + + +def _age_steps(age_s: Any, timestep_s: Any) -> int: + """Convert canonical observation age in seconds to conservative whole steps. + + Returns: + A non-negative integer, rounded upward so age is never understated. + """ + try: + age = float(age_s) + timestep = float(timestep_s) + except (TypeError, ValueError) as exc: + raise ValueError("last_observed_age_s must be numeric") from exc + if not np.isfinite(age) or age < 0.0: + raise ValueError("last_observed_age_s must be finite and non-negative") + if timestep <= 0.0: + if age == 0.0: + return 0 + raise ValueError("positive observation age requires a positive belief timestep") + return max(0, int(np.ceil(age / timestep - 1e-9))) + + +def _planner_track_from_entity(agent: Any, *, timestep_s: float) -> PlannerTrackBelief: + """Build one planner track from public EntityBelief fields only. + + Returns: + An immutable planner track containing only canonical public belief data. + """ + + if not isinstance(agent.entity_id, (str, int)) or isinstance(agent.entity_id, bool): + raise ValueError("entity_id must be a stable string or integer") + if not isinstance(agent.visibility_state, VisibilityState): + raise ValueError("visibility_state is malformed") + try: + position = np.asarray(agent.position.mean_xy, dtype=np.float64).reshape(-1) + velocity = np.asarray(agent.velocity.mean_xy, dtype=np.float64).reshape(-1) + position_covariance = np.asarray(agent.position.covariance_xy, dtype=np.float64) + velocity_covariance = np.asarray(agent.velocity.covariance_xy, dtype=np.float64) + except (AttributeError, TypeError, ValueError) as exc: + raise ValueError("entity state or covariance is malformed") from exc + if position.shape != (2,) or velocity.shape != (2,): + raise ValueError("entity position and velocity must have two coordinates") + if position_covariance.shape != (2, 2) or velocity_covariance.shape != (2, 2): + raise ValueError("entity position and velocity covariance must be 2x2") + try: + radius = float(agent.radius) + except (AttributeError, TypeError, ValueError) as exc: + raise ValueError("entity radius is malformed") from exc + if not np.isfinite(radius) or radius < 0.0: + raise ValueError("entity radius must be finite and non-negative") + state = np.asarray([*position, *velocity, radius], dtype=np.float64) + covariance = np.zeros((5, 5), dtype=np.float64) + covariance[:2, :2] = position_covariance + covariance[2:4, 2:4] = velocity_covariance + position_confidence = _validate_probability("position confidence", agent.position.confidence) + velocity_confidence = _validate_probability("velocity confidence", agent.velocity.confidence) + confidence = min(position_confidence, velocity_confidence) + existence_probability = _validate_probability( + "existence_probability", agent.existence_probability + ) + age_steps = _age_steps(agent.last_observed_age_s, timestep_s) + source = getattr(agent.source, "adapter", None) + if not isinstance(source, str) or not source: + raise ValueError("entity source adapter must be a non-empty string") + track_id = _validate_track_id(agent.entity_id) + return PlannerTrackBelief( + track_id=track_id, + mean_state=state, + covariance=covariance, + confidence=confidence, + existence_probability=existence_probability, + visibility=agent.visibility_state is VisibilityState.VISIBLE, + age_steps=age_steps, + source=source, + position_confidence=position_confidence, + velocity_confidence=velocity_confidence, + visibility_state=agent.visibility_state.value, + identity_lifecycle_token=f"entity-id:{track_id}", + ) + + +def _safe_legacy_observation(belief: Any) -> tuple[dict[str, Any], str | None]: + """Build a finite legacy fallback, or an empty non-authoritative mapping. + + Returns: + A legacy observation and an optional fail-closed reason. + """ + try: + observation = belief.to_socnav_struct() + except Exception: # noqa: BLE001 - fail closed at the planner adapter boundary + return {}, "legacy_observation_unavailable" + if not isinstance(observation, Mapping) or not _runtime_value_is_finite(observation): + return {}, "legacy_observation_nonfinite" + return dict(observation), None + + +def _belief_projection_diagnostics( + *, + planner_name: str, + status: str, + belief_step: int, + tracks: tuple[PlannerTrackBelief, ...] = (), + fallback_reason: str | None = None, +) -> dict[str, Any]: + """Build the required compact, deterministic belief-projection diagnostics. + + Returns: + A JSON-safe diagnostic mapping with deterministic track ordering. + """ + ordered_tracks = tuple(sorted(tracks, key=lambda track: _track_sort_key(track.track_id))) + diagnostics: dict[str, Any] = { + "schema_version": BELIEF_AWARE_PLANNER_INPUT_SCHEMA_VERSION, + "status": status, + "planner_name": planner_name, + "belief_step": belief_step, + "visible_track_count": sum(track.visibility for track in ordered_tracks), + "occluded_track_count": sum(not track.visibility for track in ordered_tracks), + "stale_track_count": sum(track.age_steps > 0 for track in ordered_tracks), + "retained_track_count": len(ordered_tracks), + "retired_track_count": 0, + "dropped_track_count": 0, + "per_reason_drop_count": {}, + "fallback_reason": fallback_reason, + "ordered_track_ids": [track.track_id for track in ordered_tracks], + "identity_lifecycle_tokens": { + str(track.track_id): track.identity_lifecycle_token for track in ordered_tracks + }, + "identity_lifecycle_status": "entity_id_only", + "identity_generation_available": False, + "identity_reuse_safe": False, + "retirement_tracking": "unavailable_at_scenario_belief_boundary", + "lifecycle_reset_required": True, + "claim_boundary": "diagnostic_interface_smoke", + } + return diagnostics + + +def _build_belief_aware_input( + *, + belief: Any, + planner_name: str, + legacy_observation: Mapping[str, Any], + belief_step: int, + tracks: Mapping[str | int, PlannerTrackBelief], + status: str, + fallback_reason: str | None = None, +) -> BeliefAwarePlannerInput: + """Construct one validated typed input and its diagnostics. + + Returns: + A validated typed planner input. + """ + del belief + ordered_tracks = tuple( + sorted(tracks.values(), key=lambda track: _track_sort_key(track.track_id)) + ) + diagnostics = _belief_projection_diagnostics( + planner_name=planner_name, + status=status, + belief_step=belief_step, + tracks=ordered_tracks, + fallback_reason=fallback_reason, + ) + return BeliefAwarePlannerInput( + legacy_observation=legacy_observation, + tracks=tracks, + belief_step=belief_step, + diagnostics=diagnostics, + ) + + +def project_belief_aware_planner_input( + belief: ScenarioBelief | None, + *, + planner_name: str | None = None, + planner_key: str | None = None, +) -> BeliefAwarePlannerInput: + """Project a ScenarioBelief into an explicit identity-safe planner input. + + The only admitted name is ``BeliefGuidedLocalPlanner``. Missing belief, + empty belief, unsupported planner, and invalid belief are represented by + distinct statuses. A valid projection retains every entity in + ``ScenarioBelief.agents`` regardless of visibility, age, confidence, or + existence; canonical retirement policy is not reimplemented here. + + This is an additive diagnostic seam. It does not register a planner, + alter a default roster, or change ``to_socnav_struct()``/the existing + stream-gap adapter. + + Returns: + A typed input with an explicit status and safe legacy fallback. + """ + resolved_name = _planner_name(planner_name=planner_name, planner_key=planner_key) + if belief is None: + diagnostics = _belief_projection_diagnostics( + planner_name=resolved_name, + status="no_belief", + belief_step=0, + fallback_reason="belief_not_supplied", + ) + return BeliefAwarePlannerInput( + legacy_observation={}, + tracks={}, + belief_step=0, + diagnostics=diagnostics, + ) + + if not isinstance(belief, ScenarioBelief): + diagnostics = _belief_projection_diagnostics( + planner_name=resolved_name, + status="invalid_belief", + belief_step=0, + fallback_reason="belief_type_unsupported", + ) + return BeliefAwarePlannerInput( + legacy_observation={}, + tracks={}, + belief_step=0, + diagnostics=diagnostics, + ) + + legacy_observation, legacy_reason = _safe_legacy_observation(belief) + try: + belief_step = _belief_step(belief) + except ValueError as exc: + diagnostics = _belief_projection_diagnostics( + planner_name=resolved_name, + status="invalid_belief", + belief_step=0, + fallback_reason=str(exc), + ) + return BeliefAwarePlannerInput( + legacy_observation=legacy_observation, + tracks={}, + belief_step=0, + diagnostics=diagnostics, + ) + + if legacy_reason is not None: + diagnostics = _belief_projection_diagnostics( + planner_name=resolved_name, + status="invalid_belief", + belief_step=belief_step, + fallback_reason=legacy_reason, + ) + return BeliefAwarePlannerInput( + legacy_observation={}, + tracks={}, + belief_step=belief_step, + diagnostics=diagnostics, + ) + + if resolved_name not in SUPPORTED_BELIEF_AWARE_PLANNER_NAMES: + diagnostics = _belief_projection_diagnostics( + planner_name=resolved_name, + status="unsupported_planner", + belief_step=belief_step, + fallback_reason="planner_not_explicitly_admitted", + ) + return BeliefAwarePlannerInput( + legacy_observation=legacy_observation, + tracks={}, + belief_step=belief_step, + diagnostics=diagnostics, + ) + + try: + seen_ids: set[str | int] = set() + tracks = {} + timestep_s = float(belief.timestep_s) + for agent in belief.agents: + track = _planner_track_from_entity(agent, timestep_s=timestep_s) + if track.track_id in seen_ids: + raise ValueError(f"duplicate track_id {track.track_id!r}") + seen_ids.add(track.track_id) + tracks[track.track_id] = track + except (AttributeError, TypeError, ValueError, OverflowError) as exc: + diagnostics = _belief_projection_diagnostics( + planner_name=resolved_name, + status="invalid_belief", + belief_step=belief_step, + fallback_reason=str(exc), + ) + return BeliefAwarePlannerInput( + legacy_observation=legacy_observation, + tracks={}, + belief_step=belief_step, + diagnostics=diagnostics, + ) + + status = "empty_belief" if not tracks else "projected" + return _build_belief_aware_input( + belief=belief, + planner_name=resolved_name, + legacy_observation=legacy_observation, + belief_step=belief_step, + tracks=tracks, + status=status, + ) + + +def project_scenario_belief_for_belief_aware_planner( + belief: ScenarioBelief | None, + *, + planner_name: str | None = None, + planner_key: str | None = None, +) -> BeliefAwarePlannerInput: + """Readable alias for :func:`project_belief_aware_planner_input`. + + Returns: + The same typed input returned by the canonical projection helper. + """ + return project_belief_aware_planner_input( + belief, + planner_name=planner_name, + planner_key=planner_key, + ) + + __all__ = [ + "BELIEF_AWARE_PLANNER_INPUT_SCHEMA_VERSION", "SCENARIO_BELIEF_PLANNER_PROJECTION_SCHEMA_VERSION", + "SUPPORTED_BELIEF_AWARE_PLANNER_KEYS", + "SUPPORTED_BELIEF_AWARE_PLANNER_NAMES", "SUPPORTED_UNCERTAINTY_PLANNER_KEYS", + "BeliefAwarePlannerInput", + "PlannerTrackBelief", "ScenarioBeliefPlannerProjection", + "project_belief_aware_planner_input", + "project_scenario_belief_for_belief_aware_planner", "project_scenario_belief_for_planner", ] diff --git a/tests/planner/test_scenario_belief_track_projection.py b/tests/planner/test_scenario_belief_track_projection.py new file mode 100644 index 0000000000..36ef18d6e1 --- /dev/null +++ b/tests/planner/test_scenario_belief_track_projection.py @@ -0,0 +1,221 @@ +"""Diagnostic contract tests for the identity-keyed ScenarioBelief projection. + +These tests cover the additive interface only. They do not claim planner +performance, identity-generation support, safety improvement, or benchmark +evidence. +""" + +from __future__ import annotations + +import json +from dataclasses import replace +from types import SimpleNamespace + +import numpy as np +import pytest + +from robot_sf.gym_env.unified_config import RobotSimulationConfig +from robot_sf.planner.scenario_belief_adapter import ( + BELIEF_AWARE_PLANNER_INPUT_SCHEMA_VERSION, + SUPPORTED_BELIEF_AWARE_PLANNER_NAMES, + BeliefAwarePlannerInput, + PlannerTrackBelief, + project_belief_aware_planner_input, +) +from robot_sf.representation import VisibilityState, scenario_belief_from_simulator_oracle + + +def _belief_fixture(): + """Return a small public simulator-like ScenarioBelief fixture.""" + simulator = SimpleNamespace( + ped_pos=np.array([[2.0, 0.0], [0.0, 3.0]], dtype=np.float32), + ped_vel=np.array([[0.5, 0.0], [0.0, -0.25]], dtype=np.float32), + robots=[ + SimpleNamespace( + pose=((0.0, 0.0), 0.0), + current_speed=np.array([0.1, 0.0], dtype=np.float32), + config=SimpleNamespace(radius=0.4), + ) + ], + goal_pos=[np.array([5.0, 0.0], dtype=np.float32)], + next_goal_pos=[None], + map_def=SimpleNamespace(width=10.0, height=8.0, obstacles=[]), + config=SimpleNamespace(time_per_step_in_secs=0.1), + ) + belief = scenario_belief_from_simulator_oracle( + simulator, + env_config=RobotSimulationConfig(), + max_pedestrians=4, + ) + occluded = replace( + belief.agents[1], + visibility_state=VisibilityState.OCCLUDED, + last_observed_age_s=0.25, + ) + return replace(belief, sim_time_s=0.5, agents=(belief.agents[0], occluded)) + + +def test_projection_retains_visible_and_occluded_tracks_by_canonical_id() -> None: + """Visible legacy rows and complete ID-keyed maintained tracks stay distinct.""" + belief = _belief_fixture() + + projected = project_belief_aware_planner_input( + belief, + planner_name="BeliefGuidedLocalPlanner", + ) + + assert projected.schema_version == BELIEF_AWARE_PLANNER_INPUT_SCHEMA_VERSION + assert projected.diagnostics["status"] == "projected" + assert tuple(projected.tracks) == ("ped_000", "ped_001") + assert projected.diagnostics["visible_track_count"] == 1 + assert projected.diagnostics["occluded_track_count"] == 1 + assert projected.diagnostics["stale_track_count"] == 1 + assert projected.diagnostics["retained_track_count"] == 2 + assert projected.ordered_track_ids() == ("ped_000", "ped_001") + assert projected.tracks["ped_000"].visibility is True + assert projected.tracks["ped_001"].visibility is False + assert projected.tracks["ped_001"].age_steps == 3 + assert projected.legacy_observation["pedestrians"]["count"][0] == pytest.approx(1.0) + + +def test_projection_is_independent_of_scenario_agent_order() -> None: + """Reordering source agents cannot exchange ID-keyed uncertainty metadata.""" + belief = _belief_fixture() + reordered = replace(belief, agents=tuple(reversed(belief.agents))) + + first = project_belief_aware_planner_input( + belief, + planner_name="BeliefGuidedLocalPlanner", + ) + second = project_belief_aware_planner_input( + reordered, + planner_name="BeliefGuidedLocalPlanner", + ) + + assert first.to_dict() == second.to_dict() + assert first.tracks["ped_000"].confidence == second.tracks["ped_000"].confidence + assert ( + first.tracks["ped_001"].covariance.tolist() == second.tracks["ped_001"].covariance.tolist() + ) + + +def test_projection_distinguishes_missing_empty_and_unsupported() -> None: + """Missing belief, empty belief, and unsupported planner fallback are explicit.""" + belief = _belief_fixture() + missing = project_belief_aware_planner_input( + None, + planner_name="BeliefGuidedLocalPlanner", + ) + empty = project_belief_aware_planner_input( + replace(belief, agents=()), + planner_name="BeliefGuidedLocalPlanner", + ) + unsupported = project_belief_aware_planner_input( + belief, + planner_name="stream_gap", + ) + + assert missing.diagnostics["status"] == "no_belief" + assert missing.legacy_observation == {} + assert empty.diagnostics["status"] == "empty_belief" + assert empty.tracks == {} + assert unsupported.diagnostics["status"] == "unsupported_planner" + assert unsupported.tracks == {} + assert unsupported.legacy_observation["pedestrians"]["count"][0] == pytest.approx(1.0) + assert SUPPORTED_BELIEF_AWARE_PLANNER_NAMES == frozenset({"BeliefGuidedLocalPlanner"}) + + +def test_projection_rejects_malformed_track_and_keeps_safe_legacy_fallback() -> None: + """A malformed maintained track rejects the complete typed projection.""" + belief = _belief_fixture() + malformed = replace(belief.agents[0], radius=-1.0) + rejected = project_belief_aware_planner_input( + replace(belief, agents=(malformed, belief.agents[1])), + planner_name="BeliefGuidedLocalPlanner", + ) + + assert rejected.diagnostics["status"] == "invalid_belief" + assert "radius" in rejected.diagnostics["fallback_reason"] + assert rejected.tracks == {} + assert rejected.diagnostics["dropped_track_count"] == 0 + assert "pedestrians" in rejected.legacy_observation + + +def test_typed_records_own_arrays_and_export_deterministically() -> None: + """Validated arrays are read-only copies and JSON export rejects non-finite data.""" + mean = np.array([1.0, 2.0, 0.1, 0.2, 0.3], dtype=np.float32) + covariance = np.eye(5, dtype=np.float32) + track = PlannerTrackBelief( + track_id=2, + mean_state=mean, + covariance=covariance, + confidence=0.8, + existence_probability=0.7, + visibility=True, + age_steps=0, + source="unit_test", + ) + wrapper = BeliefAwarePlannerInput( + legacy_observation={"array": mean}, + tracks={2: track}, + belief_step=4, + diagnostics={"status": "projected"}, + ) + mean[0] = 99.0 + covariance[0, 0] = 99.0 + + assert track.mean_state[0] == pytest.approx(1.0) + assert track.covariance[0, 0] == pytest.approx(1.0) + with pytest.raises(ValueError, match="read-only"): + track.mean_state[0] = 4.0 + payload = wrapper.to_dict() + assert list(payload["tracks"]) == ["2"] + assert json.loads(wrapper.to_json()) == payload + assert payload["diagnostics"]["status"] == "projected" + + +def test_typed_record_rejects_non_psd_covariance_and_key_mismatch() -> None: + """Standalone construction rejects unsafe covariance and mapping identity drift.""" + with pytest.raises(ValueError, match="positive semidefinite"): + PlannerTrackBelief( + track_id="ped_001", + mean_state=np.zeros(5), + covariance=np.diag([-1.0, 0.0, 0.0, 0.0, 0.0]), + confidence=1.0, + existence_probability=1.0, + visibility=False, + age_steps=1, + source="unit_test", + ) + + track = PlannerTrackBelief( + track_id="ped_001", + mean_state=np.zeros(5), + covariance=np.eye(5), + confidence=1.0, + existence_probability=1.0, + visibility=True, + age_steps=0, + source="unit_test", + ) + with pytest.raises(ValueError, match="track key mismatch"): + BeliefAwarePlannerInput( + legacy_observation={}, + tracks={"ped_002": track}, + belief_step=0, + ) + + +def test_identity_lifecycle_limitation_is_explicit() -> None: + """The adapter does not fabricate a generation for numeric-ID reuse.""" + projected = project_belief_aware_planner_input( + _belief_fixture(), + planner_name="BeliefGuidedLocalPlanner", + ) + + assert projected.diagnostics["identity_generation_available"] is False + assert projected.diagnostics["identity_reuse_safe"] is False + assert projected.diagnostics["lifecycle_reset_required"] is True + assert projected.diagnostics["retirement_tracking"] == ( + "unavailable_at_scenario_belief_boundary" + ) From 3cfdeeb11110b6b038e5e200b3f6b36baf8ae3ff Mon Sep 17 00:00:00 2001 From: ll7 <32880741+ll7@users.noreply.github.com> Date: Fri, 4 Sep 2026 13:13:54 +0200 Subject: [PATCH 02/10] fix(planner): use portable belief diagnostics default --- robot_sf/planner/scenario_belief_adapter.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/robot_sf/planner/scenario_belief_adapter.py b/robot_sf/planner/scenario_belief_adapter.py index f7bdc18ae8..4a1bce015a 100644 --- a/robot_sf/planner/scenario_belief_adapter.py +++ b/robot_sf/planner/scenario_belief_adapter.py @@ -9,7 +9,7 @@ import json from collections.abc import Mapping -from dataclasses import dataclass +from dataclasses import dataclass, field from types import MappingProxyType from typing import Any @@ -440,7 +440,7 @@ class BeliefAwarePlannerInput: tracks: Mapping[str | int, PlannerTrackBelief] belief_step: int schema_version: str = BELIEF_AWARE_PLANNER_INPUT_SCHEMA_VERSION - diagnostics: Mapping[str, Any] = MappingProxyType({}) + diagnostics: Mapping[str, Any] = field(default_factory=dict) def __post_init__(self) -> None: """Validate mappings and make caller-owned observation data independent.""" From b23a951e93d8f627a777ff4eda8cc668be4b050b Mon Sep 17 00:00:00 2001 From: ll7 <32880741+ll7@users.noreply.github.com> Date: Fri, 4 Sep 2026 13:22:21 +0200 Subject: [PATCH 03/10] fix(planner): harden belief projection compatibility --- robot_sf/planner/scenario_belief_adapter.py | 81 +++++++++++++++---- .../test_scenario_belief_track_projection.py | 29 +++++++ 2 files changed, 96 insertions(+), 14 deletions(-) diff --git a/robot_sf/planner/scenario_belief_adapter.py b/robot_sf/planner/scenario_belief_adapter.py index 4a1bce015a..cf0b5f4e72 100644 --- a/robot_sf/planner/scenario_belief_adapter.py +++ b/robot_sf/planner/scenario_belief_adapter.py @@ -11,11 +11,12 @@ from collections.abc import Mapping from dataclasses import dataclass, field from types import MappingProxyType -from typing import Any +from typing import TYPE_CHECKING, Any import numpy as np -from robot_sf.representation.scenario_belief import ScenarioBelief, VisibilityState +if TYPE_CHECKING: + from robot_sf.representation.scenario_belief import ScenarioBelief SCENARIO_BELIEF_PLANNER_PROJECTION_SCHEMA_VERSION = "scenario-belief-planner-projection.v1" SUPPORTED_UNCERTAINTY_PLANNER_KEYS = frozenset({"stream_gap"}) @@ -25,6 +26,27 @@ SUPPORTED_BELIEF_AWARE_PLANNER_KEYS = SUPPORTED_BELIEF_AWARE_PLANNER_NAMES +def _load_scenario_belief_types() -> tuple[type[Any], type[Any]] | None: + """Load canonical belief and visibility types only when the new seam is used. + + The existing adapter is imported by dependency-light legacy planner paths. Keep the + optional SciPy-backed ScenarioBelief representation out of that import path; callers that + invoke the typed projection get an explicit unavailable/invalid fallback instead. + + Returns: + The canonical ``ScenarioBelief`` and ``VisibilityState`` classes, or ``None`` when the + optional representation dependencies are unavailable. + """ + try: + from robot_sf.representation.scenario_belief import ( # noqa: PLC0415 + ScenarioBelief, + VisibilityState, + ) + except (ImportError, ModuleNotFoundError): + return None + return ScenarioBelief, VisibilityState + + @dataclass(frozen=True) class ScenarioBeliefPlannerProjection: """ScenarioBelief observation plus explicit planner uncertainty compatibility status.""" @@ -285,11 +307,10 @@ def __post_init__(self) -> None: """Validate and defensively normalize all planner-track fields.""" track_id = _validate_track_id(self.track_id) object.__setattr__(self, "track_id", track_id) - object.__setattr__( - self, - "mean_state", - _readonly_float_array("mean_state", self.mean_state, shape=(5,)), - ) + mean_state = _readonly_float_array("mean_state", self.mean_state, shape=(5,)) + if mean_state[4] < 0.0: + raise ValueError("mean_state radius must be finite and non-negative") + object.__setattr__(self, "mean_state", mean_state) object.__setattr__(self, "covariance", _readonly_covariance(self.covariance)) object.__setattr__(self, "confidence", _validate_probability("confidence", self.confidence)) object.__setattr__( @@ -556,7 +577,12 @@ def _age_steps(age_s: Any, timestep_s: Any) -> int: return max(0, int(np.ceil(age / timestep - 1e-9))) -def _planner_track_from_entity(agent: Any, *, timestep_s: float) -> PlannerTrackBelief: +def _planner_track_from_entity( + agent: Any, + *, + timestep_s: float, + visibility_type: type[Any], +) -> PlannerTrackBelief: """Build one planner track from public EntityBelief fields only. Returns: @@ -565,7 +591,8 @@ def _planner_track_from_entity(agent: Any, *, timestep_s: float) -> PlannerTrack if not isinstance(agent.entity_id, (str, int)) or isinstance(agent.entity_id, bool): raise ValueError("entity_id must be a stable string or integer") - if not isinstance(agent.visibility_state, VisibilityState): + visibility_state = agent.visibility_state + if not isinstance(visibility_state, visibility_type): raise ValueError("visibility_state is malformed") try: position = np.asarray(agent.position.mean_xy, dtype=np.float64).reshape(-1) @@ -605,12 +632,12 @@ def _planner_track_from_entity(agent: Any, *, timestep_s: float) -> PlannerTrack covariance=covariance, confidence=confidence, existence_probability=existence_probability, - visibility=agent.visibility_state is VisibilityState.VISIBLE, + visibility=visibility_state.value == "visible", age_steps=age_steps, source=source, position_confidence=position_confidence, velocity_confidence=velocity_confidence, - visibility_state=agent.visibility_state.value, + visibility_state=visibility_state.value, identity_lifecycle_token=f"entity-id:{track_id}", ) @@ -650,7 +677,14 @@ def _belief_projection_diagnostics( "planner_name": planner_name, "belief_step": belief_step, "visible_track_count": sum(track.visibility for track in ordered_tracks), - "occluded_track_count": sum(not track.visibility for track in ordered_tracks), + "occluded_track_count": sum( + ( + track.visibility_state == "occluded" + if track.visibility_state is not None + else not track.visibility + ) + for track in ordered_tracks + ), "stale_track_count": sum(track.age_steps > 0 for track in ordered_tracks), "retained_track_count": len(ordered_tracks), "retired_track_count": 0, @@ -741,7 +775,22 @@ def project_belief_aware_planner_input( diagnostics=diagnostics, ) - if not isinstance(belief, ScenarioBelief): + scenario_belief_types = _load_scenario_belief_types() + if scenario_belief_types is None: + diagnostics = _belief_projection_diagnostics( + planner_name=resolved_name, + status="invalid_belief", + belief_step=0, + fallback_reason="scenario_belief_representation_unavailable", + ) + return BeliefAwarePlannerInput( + legacy_observation={}, + tracks={}, + belief_step=0, + diagnostics=diagnostics, + ) + scenario_belief_type, visibility_type = scenario_belief_types + if not isinstance(belief, scenario_belief_type): diagnostics = _belief_projection_diagnostics( planner_name=resolved_name, status="invalid_belief", @@ -805,7 +854,11 @@ def project_belief_aware_planner_input( tracks = {} timestep_s = float(belief.timestep_s) for agent in belief.agents: - track = _planner_track_from_entity(agent, timestep_s=timestep_s) + track = _planner_track_from_entity( + agent, + timestep_s=timestep_s, + visibility_type=visibility_type, + ) if track.track_id in seen_ids: raise ValueError(f"duplicate track_id {track.track_id!r}") seen_ids.add(track.track_id) diff --git a/tests/planner/test_scenario_belief_track_projection.py b/tests/planner/test_scenario_belief_track_projection.py index 36ef18d6e1..6829544f3c 100644 --- a/tests/planner/test_scenario_belief_track_projection.py +++ b/tests/planner/test_scenario_belief_track_projection.py @@ -125,6 +125,24 @@ def test_projection_distinguishes_missing_empty_and_unsupported() -> None: assert SUPPORTED_BELIEF_AWARE_PLANNER_NAMES == frozenset({"BeliefGuidedLocalPlanner"}) +def test_projection_counts_out_of_range_as_non_visible_not_occluded() -> None: + """Visibility diagnostics distinguish explicit occlusion from other hidden states.""" + belief = _belief_fixture() + out_of_range = replace( + belief.agents[1], + visibility_state=VisibilityState.OUT_OF_RANGE, + ) + + projected = project_belief_aware_planner_input( + replace(belief, agents=(belief.agents[0], out_of_range)), + planner_name="BeliefGuidedLocalPlanner", + ) + + assert projected.diagnostics["visible_track_count"] == 1 + assert projected.diagnostics["occluded_track_count"] == 0 + assert projected.diagnostics["retained_track_count"] == 2 + + def test_projection_rejects_malformed_track_and_keeps_safe_legacy_fallback() -> None: """A malformed maintained track rejects the complete typed projection.""" belief = _belief_fixture() @@ -187,6 +205,17 @@ def test_typed_record_rejects_non_psd_covariance_and_key_mismatch() -> None: age_steps=1, source="unit_test", ) + with pytest.raises(ValueError, match="radius"): + PlannerTrackBelief( + track_id="ped_negative_radius", + mean_state=np.array([0.0, 0.0, 0.0, 0.0, -0.1]), + covariance=np.eye(5), + confidence=1.0, + existence_probability=1.0, + visibility=True, + age_steps=0, + source="unit_test", + ) track = PlannerTrackBelief( track_id="ped_001", From 186c992138f657b3edfebca46941d0c48e68af3b Mon Sep 17 00:00:00 2001 From: ll7 <32880741+ll7@users.noreply.github.com> Date: Fri, 4 Sep 2026 13:58:56 +0200 Subject: [PATCH 04/10] test(planner): route scenario belief contracts through fast CI --- tests/conftest.py | 5 + .../test_scenario_belief_track_projection.py | 293 ++++++++++++++++++ 2 files changed, 298 insertions(+) diff --git a/tests/conftest.py b/tests/conftest.py index 9b6d8246a7..4ed3669d34 100644 --- a/tests/conftest.py +++ b/tests/conftest.py @@ -472,6 +472,11 @@ def classify(self, duration_seconds: float): # coverage for the changed adapter; keep them in fast PR shards for the # exact-head changed-coverage gate. "test_topology_guided_local_policy.py", + # ScenarioBelief projection and stream-gap uncertainty tests are deterministic + # planner-contract coverage for the diagnostic adapter in issue #8050. + "test_scenario_belief_track_projection.py", + "test_scenario_belief_uncertainty_gate.py", + "test_stream_gap_planner.py", # Route-side/homotopy observability tests are deterministic pure-metric # contracts with numpy fixtures; keep them in fast pull-request shards for # the exact-head changed-coverage gate (issue #7890). diff --git a/tests/planner/test_scenario_belief_track_projection.py b/tests/planner/test_scenario_belief_track_projection.py index 6829544f3c..bb7c6b490b 100644 --- a/tests/planner/test_scenario_belief_track_projection.py +++ b/tests/planner/test_scenario_belief_track_projection.py @@ -7,6 +7,7 @@ from __future__ import annotations +import builtins import json from dataclasses import replace from types import SimpleNamespace @@ -14,6 +15,7 @@ import numpy as np import pytest +import robot_sf.planner.scenario_belief_adapter as adapter from robot_sf.gym_env.unified_config import RobotSimulationConfig from robot_sf.planner.scenario_belief_adapter import ( BELIEF_AWARE_PLANNER_INPUT_SCHEMA_VERSION, @@ -21,6 +23,8 @@ BeliefAwarePlannerInput, PlannerTrackBelief, project_belief_aware_planner_input, + project_scenario_belief_for_belief_aware_planner, + project_scenario_belief_for_planner, ) from robot_sf.representation import VisibilityState, scenario_belief_from_simulator_oracle @@ -55,6 +59,22 @@ def _belief_fixture(): return replace(belief, sim_time_s=0.5, agents=(belief.agents[0], occluded)) +def _standalone_track(**overrides): + """Build a valid standalone planner track for validation-edge tests.""" + values = { + "track_id": "ped_000", + "mean_state": np.zeros(5), + "covariance": np.eye(5), + "confidence": 1.0, + "existence_probability": 1.0, + "visibility": True, + "age_steps": 0, + "source": "unit_test", + } + values.update(overrides) + return PlannerTrackBelief(**values) + + def test_projection_retains_visible_and_occluded_tracks_by_canonical_id() -> None: """Visible legacy rows and complete ID-keyed maintained tracks stay distinct.""" belief = _belief_fixture() @@ -248,3 +268,276 @@ def test_identity_lifecycle_limitation_is_explicit() -> None: assert projected.diagnostics["retirement_tracking"] == ( "unavailable_at_scenario_belief_boundary" ) + + +def test_lazy_representation_import_fails_closed(monkeypatch) -> None: + """Optional representation imports should report unavailable instead of leaking ImportError.""" + original_import = builtins.__import__ + + def blocked_import(name, *args, **kwargs): + if name.startswith("robot_sf.representation.scenario_belief"): + raise ModuleNotFoundError("scenario belief dependencies blocked") + return original_import(name, *args, **kwargs) + + monkeypatch.setattr(builtins, "__import__", blocked_import) + assert adapter._load_scenario_belief_types() is None + + +def test_legacy_projection_fails_closed_for_malformed_inputs() -> None: + """The legacy sidecar adapter distinguishes malformed observations and reports.""" + malformed_observation = SimpleNamespace(to_socnav_struct=lambda: {}) + result = project_scenario_belief_for_planner( + malformed_observation, + planner_key="stream_gap", + ) + assert result.compatibility["reason"] == "malformed_legacy_observation" + + malformed_count = SimpleNamespace( + to_socnav_struct=lambda: {"pedestrians": {"count": ["not-a-number"]}} + ) + result = project_scenario_belief_for_planner(malformed_count, planner_key="stream_gap") + assert result.compatibility["reason"] == "malformed_pedestrian_count" + + incomplete_report = SimpleNamespace( + to_socnav_struct=lambda: {"pedestrians": {"count": np.asarray([1.0])}}, + to_uncertainty_report=lambda: {"agents": []}, + ) + result = project_scenario_belief_for_planner(incomplete_report, planner_key="stream_gap") + assert result.compatibility["reason"] == "malformed_uncertainty_report" + assert adapter._pedestrian_count({"pedestrians": []}) is None + assert adapter._pedestrian_count({"pedestrians": {"count": []}}) is None + + +def test_projection_invalid_belief_fallbacks_and_alias(monkeypatch) -> None: + """Typed projection failures retain an explicit status and safe legacy fallback.""" + belief = _belief_fixture() + original_loader = adapter._load_scenario_belief_types + monkeypatch.setattr(adapter, "_load_scenario_belief_types", lambda: None) + unavailable = project_belief_aware_planner_input( + belief, + planner_name="BeliefGuidedLocalPlanner", + ) + assert unavailable.diagnostics["fallback_reason"] == ( + "scenario_belief_representation_unavailable" + ) + monkeypatch.setattr(adapter, "_load_scenario_belief_types", original_loader) + + unsupported = project_belief_aware_planner_input( + object(), + planner_name="BeliefGuidedLocalPlanner", + ) + assert unsupported.diagnostics["fallback_reason"] == "belief_type_unsupported" + + bad_time = project_belief_aware_planner_input( + replace(belief, sim_time_s=-0.1), + planner_name="BeliefGuidedLocalPlanner", + ) + assert bad_time.diagnostics["status"] == "invalid_belief" + assert "sim_time_s" in bad_time.diagnostics["fallback_reason"] + + duplicate = replace( + belief, + agents=(belief.agents[0], replace(belief.agents[1], entity_id=belief.agents[0].entity_id)), + ) + duplicate_result = project_belief_aware_planner_input( + duplicate, + planner_name="BeliefGuidedLocalPlanner", + ) + assert duplicate_result.diagnostics["status"] == "invalid_belief" + assert "duplicate track_id" in duplicate_result.diagnostics["fallback_reason"] + + alias = project_scenario_belief_for_belief_aware_planner( + belief, + planner_key="BeliefGuidedLocalPlanner", + ) + assert ( + alias.to_dict() + == project_belief_aware_planner_input( + belief, + planner_name="BeliefGuidedLocalPlanner", + ).to_dict() + ) + + def fail_legacy(_belief): + raise RuntimeError("legacy observation unavailable") + + monkeypatch.setattr(type(belief), "to_socnav_struct", fail_legacy) + legacy_failure = project_belief_aware_planner_input( + belief, + planner_name="BeliefGuidedLocalPlanner", + ) + assert legacy_failure.diagnostics["fallback_reason"] == "legacy_observation_unavailable" + + +def test_validation_helpers_reject_malformed_values() -> None: + """Array, covariance, probability, integer, and ID validators fail closed.""" + + class _BadArray: + def __array__(self): + raise TypeError("cannot convert") + + with pytest.raises(ValueError, match="numeric array"): + adapter._readonly_float_array("state", _BadArray(), shape=(2,)) + with pytest.raises(ValueError, match="shape"): + adapter._readonly_float_array("state", [1.0], shape=(2,)) + with pytest.raises(ValueError, match="numeric dtype"): + adapter._readonly_float_array("state", ["x", "y"], shape=(2,)) + with pytest.raises(ValueError, match="finite"): + adapter._readonly_float_array("state", [np.nan, 1.0], shape=(2,)) + + with pytest.raises(ValueError, match="numeric"): + adapter._readonly_covariance([[1.0], [1.0, 2.0]]) + assert adapter._readonly_covariance(np.eye(4)).shape == (5, 5) + with pytest.raises(ValueError, match="numeric"): + adapter._readonly_covariance(np.full((5, 5), "x", dtype=object)) + with pytest.raises(ValueError, match="shape"): + adapter._readonly_covariance(np.eye(3)) + nonfinite_covariance = np.eye(5) + nonfinite_covariance[0, 0] = np.nan + with pytest.raises(ValueError, match="finite"): + adapter._readonly_covariance(nonfinite_covariance) + asymmetric_covariance = np.eye(5) + asymmetric_covariance[0, 1] = 1.0 + with pytest.raises(ValueError, match="symmetric"): + adapter._readonly_covariance(asymmetric_covariance) + + with pytest.raises(ValueError, match="finite value"): + adapter._validate_probability("probability", object()) + with pytest.raises(ValueError, match="finite value"): + adapter._validate_probability("probability", 2.0) + with pytest.raises(ValueError, match="finite value"): + adapter._validate_probability("probability", np.nan) + + with pytest.raises(ValueError, match="non-negative integer"): + adapter._validate_nonnegative_int("steps", object()) + with pytest.raises(ValueError, match="non-negative integer"): + adapter._validate_nonnegative_int("steps", -1) + with pytest.raises(ValueError, match="non-negative integer"): + adapter._validate_nonnegative_int("steps", 1.5) + with pytest.raises(ValueError, match="track_id"): + adapter._validate_track_id(True) + with pytest.raises(ValueError, match="track_id"): + adapter._validate_track_id("") + with pytest.raises(ValueError, match="track_id"): + adapter._validate_track_id(1.5) + + +def test_typed_input_validation_and_json_edges() -> None: + """Typed records own nested values and reject invalid mappings or JSON payloads.""" + track = _standalone_track() + wrapper = BeliefAwarePlannerInput( + legacy_observation={ + "scalar": np.float32(1.0), + "nested": (np.asarray([2.0]), [np.asarray([3.0])]), + }, + tracks={"ped_000": track}, + belief_step=0, + diagnostics={"status": "projected"}, + ) + assert wrapper.projection == wrapper.diagnostics + assert wrapper.legacy_observation["scalar"] == 1.0 + + assert adapter._runtime_value_is_finite(np.asarray([1], dtype=np.int64)) + assert not adapter._runtime_value_is_finite(np.asarray([np.inf])) + assert not adapter._runtime_value_is_finite(np.float32(np.nan)) + assert adapter._runtime_value_is_finite({"values": [1.0, (2.0,)]}) + assert not adapter._runtime_value_is_finite({"values": [float("nan")]}) + assert adapter._json_safe(np.float32(1.25)) == pytest.approx(1.25) + with pytest.raises(ValueError, match="NaN or Inf"): + adapter._json_safe(float("nan")) + + with pytest.raises(TypeError, match="legacy_observation"): + BeliefAwarePlannerInput(legacy_observation=None, tracks={}, belief_step=0) + with pytest.raises(TypeError, match="tracks"): + BeliefAwarePlannerInput(legacy_observation={}, tracks=None, belief_step=0) + with pytest.raises(ValueError, match="schema_version"): + BeliefAwarePlannerInput(legacy_observation={}, tracks={}, belief_step=0, schema_version="") + with pytest.raises(TypeError, match="diagnostics"): + BeliefAwarePlannerInput(legacy_observation={}, tracks={}, belief_step=0, diagnostics=None) + with pytest.raises(TypeError, match="PlannerTrackBelief"): + BeliefAwarePlannerInput(legacy_observation={}, tracks={"bad": object()}, belief_step=0) + + numeric_track = _standalone_track(track_id=1) + text_track = _standalone_track(track_id="1") + colliding = BeliefAwarePlannerInput( + legacy_observation={}, + tracks={1: numeric_track, "1": text_track}, + belief_step=0, + ) + with pytest.raises(ValueError, match="collide"): + colliding.to_dict() + + non_json = BeliefAwarePlannerInput( + legacy_observation={}, + tracks={}, + belief_step=0, + diagnostics={"unserializable": object()}, + ) + with pytest.raises(ValueError, match="JSON-safe"): + non_json.to_dict() + + +def test_track_field_and_time_validation_edges() -> None: + """Planner-track fields and canonical time metadata reject unsafe values.""" + for overrides, match in ( + ({"visibility": "yes"}, "visibility"), + ({"source": ""}, "source"), + ({"visibility_state": ""}, "visibility_state"), + ({"identity_lifecycle_token": ""}, "identity_lifecycle_token"), + ): + with pytest.raises(ValueError, match=match): + _standalone_track(**overrides) + + with pytest.raises(ValueError, match="belief time metadata"): + adapter._belief_step(SimpleNamespace(sim_time_s="bad", timestep_s=0.1)) + with pytest.raises(ValueError, match="sim_time_s"): + adapter._belief_step(SimpleNamespace(sim_time_s=-1.0, timestep_s=0.1)) + with pytest.raises(ValueError, match="timestep_s"): + adapter._belief_step(SimpleNamespace(sim_time_s=0.0, timestep_s=-1.0)) + with pytest.raises(ValueError, match="positive"): + adapter._belief_step(SimpleNamespace(sim_time_s=1.0, timestep_s=0.0)) + with pytest.raises(ValueError, match="aligned"): + adapter._belief_step(SimpleNamespace(sim_time_s=0.15, timestep_s=0.1)) + assert adapter._belief_step(SimpleNamespace(sim_time_s=0.0, timestep_s=0.0)) == 0 + assert adapter._belief_step(SimpleNamespace(sim_time_s=0.2, timestep_s=0.1)) == 2 + + with pytest.raises(ValueError, match="last_observed_age_s must be numeric"): + adapter._age_steps(object(), 0.1) + with pytest.raises(ValueError, match="finite and non-negative"): + adapter._age_steps(-1.0, 0.1) + with pytest.raises(ValueError, match="positive observation age"): + adapter._age_steps(1.0, 0.0) + assert adapter._age_steps(0.0, 0.0) == 0 + assert adapter._age_steps(0.21, 0.1) == 3 + + +def test_entity_projection_rejects_malformed_public_fields() -> None: + """Entity-to-track projection validates every public state and provenance field.""" + agent = _belief_fixture().agents[0] + + def project(candidate): + return adapter._planner_track_from_entity( + candidate, + timestep_s=0.1, + visibility_type=VisibilityState, + ) + + with pytest.raises(ValueError, match="entity_id"): + project(replace(agent, entity_id=object())) + with pytest.raises(ValueError, match="visibility_state"): + project(replace(agent, visibility_state="visible")) + with pytest.raises(ValueError, match="state or covariance"): + project(replace(agent, position=SimpleNamespace(mean_xy=(0.0, 0.0)))) + with pytest.raises(ValueError, match="two coordinates"): + project(replace(agent, position=replace(agent.position, mean_xy=(0.0,)))) + with pytest.raises(ValueError, match="2x2"): + project( + replace( + agent, + position=replace(agent.position, covariance_xy=((1.0, 0.0, 0.0),) * 3), + ) + ) + with pytest.raises(ValueError, match="radius"): + project(replace(agent, radius="bad")) + with pytest.raises(ValueError, match="source adapter"): + project(replace(agent, source=replace(agent.source, adapter=""))) From 5c991021147702885fa3cc22aa4ff97e92fa9d1b Mon Sep 17 00:00:00 2001 From: ll7 <32880741+ll7@users.noreply.github.com> Date: Fri, 4 Sep 2026 22:37:22 +0200 Subject: [PATCH 05/10] fix(planner): narrow ScenarioBelief projection identity claims --- ...2538_scenario_belief_planner_projection.md | 53 +++++---- robot_sf/planner/scenario_belief_adapter.py | 107 ++++++++++-------- .../test_scenario_belief_track_projection.py | 78 +++++++++++-- 3 files changed, 158 insertions(+), 80 deletions(-) diff --git a/docs/context/issue_2538_scenario_belief_planner_projection.md b/docs/context/issue_2538_scenario_belief_planner_projection.md index c967a26452..38ca702787 100644 --- a/docs/context/issue_2538_scenario_belief_planner_projection.md +++ b/docs/context/issue_2538_scenario_belief_planner_projection.md @@ -30,36 +30,49 @@ Issue #2538 adds a planner-facing ScenarioBelief projection helper: uncertainty metadata still keeps deterministic pedestrian rows. The additive issue #8050 diagnostic seam also provides -`project_belief_aware_planner_input(...)` for the explicitly named -`BeliefGuidedLocalPlanner`. It retains every canonical `ScenarioBelief.agents` entry in an -immutable, ID-keyed `tracks` mapping, including entries absent from the visible legacy rows, and -reports distinct `no_belief`, `empty_belief`, `unsupported_planner`, `invalid_belief`, and -`projected` statuses. Serialization is versioned and deterministic; legacy observations and the -existing stream-gap path are unchanged. +`project_belief_aware_planner_input(...)` for the currently supported projection target +`BeliefGuidedLocalPlanner`. It retains every `ScenarioBelief.agents` entry in an immutable, +entity-ID-keyed `tracks` mapping, including entries absent from the visible legacy rows, and +reports distinct `no_belief`, `empty_belief`, `projection_target_not_supported`, `invalid_belief`, +and `projected` statuses. Serialization is versioned and deterministic; legacy observations and +the existing stream-gap path are unchanged. -The current `ScenarioBelief` owner does not expose retirement generations. The new diagnostic -therefore reports `identity_generation_available: false`, marks its entity-ID token as not -reuse-safe, and requires stateful consumers to reset at an externally supplied lifecycle boundary. -It does not infer retirement, reuse, or benchmark/safety benefit. +`track_id` is the entity identifier supplied by one `ScenarioBelief` snapshot, not a +visible-observation row number. The current representation does not expose retirement generations, +so the diagnostic reports `identity_generation_available: false`, +`identity_reuse_safe: false`, `retired_track_count: null`, and +`stateful_identity_admitted: false`. Stateful consumers must reset at an externally supplied +lifecycle boundary. No generation or continuity token is fabricated, and no retirement, reuse, or +benchmark/safety benefit is inferred. Aggregate confidence and the radius covariance block are +explicitly labelled as adapter-derived and unavailable-as-modelled in the diagnostics. ## Claim Boundary -This proves only that ScenarioBelief uncertainty metadata can reach one planner-compatible local -observation shape and can be consumed by the existing stream-gap uncertainty gate on a fixture. It -does not prove better navigation, safety, SNQI, planner performance, perception calibration, or -benchmark movement. +Safe claim: this is a deterministic, entity-ID-keyed projection of one `ScenarioBelief` snapshot +that prevents joining uncertainty metadata by visible-row position. It proves only that +ScenarioBelief uncertainty metadata can reach one planner-compatible local observation shape and +can be consumed by the existing stream-gap uncertainty gate on a fixture. It does not prove +cross-lifecycle identity continuity, better navigation, safety, SNQI, planner performance, +perception calibration, or benchmark movement. ## Validation ```bash -uv run pytest tests/planner/test_stream_gap_planner.py -k "uncertainty or scenario_belief" -q -uv run ruff check robot_sf/planner/scenario_belief_adapter.py tests/planner/test_stream_gap_planner.py -uv run ruff format --check robot_sf/planner/scenario_belief_adapter.py tests/planner/test_stream_gap_planner.py +uv run pytest tests/planner/test_scenario_belief_track_projection.py \ + tests/planner/test_scenario_belief_uncertainty_gate.py \ + tests/planner/test_stream_gap_planner.py \ + tests/representation/test_scenario_belief.py -q +uv run ruff check robot_sf/planner/scenario_belief_adapter.py \ + tests/planner/test_scenario_belief_track_projection.py +uv run ruff format --check robot_sf/planner/scenario_belief_adapter.py \ + tests/planner/test_scenario_belief_track_projection.py ``` ## Follow-Up The next useful step is a runtime observation-builder path that produces a ScenarioBelief during an -environment step and routes this projection into a planner selection or smoke command. A canonical -track-generation/retirement owner is also required before a stateful planner can claim reuse-safe -identity semantics. Until those gates exist, this remains a unit-level planner interface smoke. +environment step and routes this projection into a planner selection or smoke command. A +track-generation/retirement owner is required before a stateful planner can claim reuse-safe +identity semantics. Until those gates exist, this remains a unit-level planner interface smoke; +#8050 stays open with `implementation_admitted: false`, and no downstream planner work is part of +this slice. diff --git a/robot_sf/planner/scenario_belief_adapter.py b/robot_sf/planner/scenario_belief_adapter.py index cf0b5f4e72..de7775b7b7 100644 --- a/robot_sf/planner/scenario_belief_adapter.py +++ b/robot_sf/planner/scenario_belief_adapter.py @@ -1,8 +1,9 @@ """Planner-facing ScenarioBelief uncertainty projection helpers. These helpers are diagnostic interface smoke, not benchmark evidence. They bridge -the uncertainty-preserving ScenarioBelief report into one planner-compatible observation shape -without changing legacy policy projections. +the uncertainty-preserving ScenarioBelief report into planner-compatible observation shapes +without changing legacy policy projections. The typed seam is an entity-ID-keyed projection of +one ScenarioBelief snapshot; it does not establish cross-lifecycle identity continuity. """ from __future__ import annotations @@ -21,9 +22,10 @@ SCENARIO_BELIEF_PLANNER_PROJECTION_SCHEMA_VERSION = "scenario-belief-planner-projection.v1" SUPPORTED_UNCERTAINTY_PLANNER_KEYS = frozenset({"stream_gap"}) BELIEF_AWARE_PLANNER_INPUT_SCHEMA_VERSION = "belief-aware-planner-input.v1" -SUPPORTED_BELIEF_AWARE_PLANNER_NAMES = frozenset({"BeliefGuidedLocalPlanner"}) -# Keep the key-shaped alias discoverable for callers that use the existing adapter vocabulary. -SUPPORTED_BELIEF_AWARE_PLANNER_KEYS = SUPPORTED_BELIEF_AWARE_PLANNER_NAMES +SUPPORTED_PROJECTION_TARGETS = frozenset({"BeliefGuidedLocalPlanner"}) +# Keep the existing names discoverable for callers that use the initial adapter vocabulary. +SUPPORTED_BELIEF_AWARE_PLANNER_NAMES = SUPPORTED_PROJECTION_TARGETS +SUPPORTED_BELIEF_AWARE_PLANNER_KEYS = SUPPORTED_PROJECTION_TARGETS def _load_scenario_belief_types() -> tuple[type[Any], type[Any]] | None: @@ -186,14 +188,15 @@ def _readonly_float_array( def _readonly_covariance(value: Any) -> np.ndarray: - """Validate the canonical 5D state covariance and return an owned copy. + """Validate the adapter's 5D state covariance and return an owned copy. ``ScenarioBelief`` owns independent 2D position and velocity covariance matrices. The planner state is ``[x, y, vx, vy, radius]``; the adapter embeds those two blocks and uses a deterministic zero-variance radius block - because the current canonical owner has no radius uncertainty or cross terms. - A 4x4 block matrix is accepted for standalone typed-record construction and - is normalized to the same 5x5 representation. + because radius uncertainty and cross terms are unavailable as modelled at + the ScenarioBelief boundary. The zero block is not evidence of measured + zero radius uncertainty. A 4x4 block matrix is accepted for standalone + typed-record construction and is normalized to the same 5x5 representation. Returns: An owned, read-only 5x5 positive-semidefinite covariance matrix. @@ -249,7 +252,7 @@ def _validate_nonnegative_int(name: str, value: Any) -> int: def _validate_track_id(value: Any) -> str | int: - """Validate a stable string or integer track identifier. + """Validate a snapshot-supplied string or integer track identifier. Current ``ScenarioBelief`` uses string entity IDs. Integer IDs remain accepted for interoperability with the canonical prediction types, but are @@ -276,18 +279,21 @@ def _track_sort_key(track_id: str | int) -> tuple[int, str | int]: @dataclass(frozen=True) class PlannerTrackBelief: - """Immutable, track-keyed planner state from one maintained belief. - - ``track_id`` is the canonical entity identity, not a row number. The - current ScenarioBelief owner supplies string IDs; integer IDs are retained - only for standalone typed interoperability. ``covariance`` uses state - order ``[x, y, vx, vy, radius]`` and is a 5x5 owned, read-only array. - - The current ScenarioBelief contract does not expose a track generation or - retirement epoch. ``identity_lifecycle_token`` therefore identifies the - canonical entity ID only and is explicitly *not* a reuse-safe generation. - Stateful consumers must reset on a canonical lifecycle event until the - representation owner supplies that missing generation. + """Immutable, entity-ID-keyed planner state from one belief snapshot. + + ``track_id`` is the entity identifier supplied by this ``ScenarioBelief`` + snapshot, not a visible-observation row number. No cross-lifecycle + continuity is implied. The current representation supplies string IDs; + integer IDs are retained only for standalone typed interoperability. + ``covariance`` uses state order ``[x, y, vx, vy, radius]`` and is a 5x5 + owned, read-only array. The position/velocity blocks are adapter-projected; + the radius block is zero because radius uncertainty is unavailable as + modelled, not because it is known to be zero. + + The aggregate ``confidence`` is adapter-derived as the minimum of position + and velocity confidence. Stateful consumers must reset at an externally + supplied lifecycle boundary until the representation owner supplies a + generation or retirement epoch. """ track_id: str | int @@ -301,7 +307,6 @@ class PlannerTrackBelief: position_confidence: float | None = None velocity_confidence: float | None = None visibility_state: str | None = None - identity_lifecycle_token: str | None = None def __post_init__(self) -> None: """Validate and defensively normalize all planner-track fields.""" @@ -342,12 +347,6 @@ def __post_init__(self) -> None: not isinstance(self.visibility_state, str) or not self.visibility_state ): raise ValueError("visibility_state must be a non-empty string when provided") - lifecycle_token = self.identity_lifecycle_token - if lifecycle_token is None: - lifecycle_token = f"entity-id:{track_id}" - if not isinstance(lifecycle_token, str) or not lifecycle_token: - raise ValueError("identity_lifecycle_token must be a non-empty string") - object.__setattr__(self, "identity_lifecycle_token", lifecycle_token) def to_dict(self) -> dict[str, Any]: """Return a deterministic JSON-safe track mapping.""" @@ -360,7 +359,6 @@ def to_dict(self) -> dict[str, Any]: "visibility": self.visibility, "age_steps": self.age_steps, "source": self.source, - "identity_lifecycle_token": self.identity_lifecycle_token, } if self.position_confidence is not None: payload["position_confidence"] = float(self.position_confidence) @@ -455,7 +453,7 @@ def _validate_planner_mapping( @dataclass(frozen=True) class BeliefAwarePlannerInput: - """Versioned planner input preserving legacy observations and ID-keyed tracks.""" + """Versioned planner input preserving legacy observations and snapshot-keyed tracks.""" legacy_observation: Mapping[str, Any] tracks: Mapping[str | int, PlannerTrackBelief] @@ -484,11 +482,11 @@ def __post_init__(self) -> None: @property def projection(self) -> Mapping[str, Any]: - """Return the compact projection diagnostics under the issue vocabulary.""" + """Return the compact entity-ID-keyed snapshot projection diagnostics.""" return self.diagnostics def ordered_track_ids(self) -> tuple[str | int, ...]: - """Return track IDs in canonical deterministic order.""" + """Return track IDs in deterministic order.""" return tuple(sorted(self.tracks, key=_track_sort_key)) def to_dict(self) -> dict[str, Any]: @@ -583,14 +581,14 @@ def _planner_track_from_entity( timestep_s: float, visibility_type: type[Any], ) -> PlannerTrackBelief: - """Build one planner track from public EntityBelief fields only. + """Build one planner track from public fields of one belief snapshot. Returns: - An immutable planner track containing only canonical public belief data. + An immutable planner track containing only public snapshot data. """ if not isinstance(agent.entity_id, (str, int)) or isinstance(agent.entity_id, bool): - raise ValueError("entity_id must be a stable string or integer") + raise ValueError("entity_id must be a non-empty string or integer") visibility_state = agent.visibility_state if not isinstance(visibility_state, visibility_type): raise ValueError("visibility_state is malformed") @@ -638,7 +636,6 @@ def _planner_track_from_entity( position_confidence=position_confidence, velocity_confidence=velocity_confidence, visibility_state=visibility_state.value, - identity_lifecycle_token=f"entity-id:{track_id}", ) @@ -675,6 +672,8 @@ def _belief_projection_diagnostics( "schema_version": BELIEF_AWARE_PLANNER_INPUT_SCHEMA_VERSION, "status": status, "planner_name": planner_name, + "projection_target": planner_name, + "supported_projection_target": planner_name in SUPPORTED_PROJECTION_TARGETS, "belief_step": belief_step, "visible_track_count": sum(track.visibility for track in ordered_tracks), "occluded_track_count": sum( @@ -686,20 +685,24 @@ def _belief_projection_diagnostics( for track in ordered_tracks ), "stale_track_count": sum(track.age_steps > 0 for track in ordered_tracks), - "retained_track_count": len(ordered_tracks), - "retired_track_count": 0, + "projected_track_count": len(ordered_tracks), + "retired_track_count": None, "dropped_track_count": 0, "per_reason_drop_count": {}, "fallback_reason": fallback_reason, "ordered_track_ids": [track.track_id for track in ordered_tracks], - "identity_lifecycle_tokens": { - str(track.track_id): track.identity_lifecycle_token for track in ordered_tracks + "uncertainty_semantics": { + "source": "adapter_derived", + "aggregate_confidence": "min(position_confidence, velocity_confidence)", + "state_covariance": "position_velocity_blocks_plus_zero_radius_block", + "radius_uncertainty": "unavailable_as_modelled", }, "identity_lifecycle_status": "entity_id_only", "identity_generation_available": False, "identity_reuse_safe": False, "retirement_tracking": "unavailable_at_scenario_belief_boundary", "lifecycle_reset_required": True, + "stateful_identity_admitted": False, "claim_boundary": "diagnostic_interface_smoke", } return diagnostics @@ -745,13 +748,16 @@ def project_belief_aware_planner_input( planner_name: str | None = None, planner_key: str | None = None, ) -> BeliefAwarePlannerInput: - """Project a ScenarioBelief into an explicit identity-safe planner input. + """Project one ScenarioBelief snapshot into an entity-ID-keyed planner input. - The only admitted name is ``BeliefGuidedLocalPlanner``. Missing belief, - empty belief, unsupported planner, and invalid belief are represented by - distinct statuses. A valid projection retains every entity in - ``ScenarioBelief.agents`` regardless of visibility, age, confidence, or - existence; canonical retirement policy is not reimplemented here. + ``BeliefGuidedLocalPlanner`` is the only currently supported projection + target; this allow-list does not admit a planner implementation or stateful + identity semantics. Missing belief, empty belief, unsupported target, and + invalid belief are represented by distinct statuses. A valid projection + retains every entity in ``ScenarioBelief.agents`` regardless of visibility, + age, confidence, or existence. ``track_id`` is the identifier supplied by + this snapshot, not a visible-row position, and no cross-lifecycle + continuity or retirement policy is inferred here. This is an additive diagnostic seam. It does not register a planner, alter a default roster, or change ``to_socnav_struct()``/the existing @@ -835,12 +841,12 @@ def project_belief_aware_planner_input( diagnostics=diagnostics, ) - if resolved_name not in SUPPORTED_BELIEF_AWARE_PLANNER_NAMES: + if resolved_name not in SUPPORTED_PROJECTION_TARGETS: diagnostics = _belief_projection_diagnostics( planner_name=resolved_name, - status="unsupported_planner", + status="projection_target_not_supported", belief_step=belief_step, - fallback_reason="planner_not_explicitly_admitted", + fallback_reason="projection_target_not_supported", ) return BeliefAwarePlannerInput( legacy_observation=legacy_observation, @@ -911,6 +917,7 @@ def project_scenario_belief_for_belief_aware_planner( "SCENARIO_BELIEF_PLANNER_PROJECTION_SCHEMA_VERSION", "SUPPORTED_BELIEF_AWARE_PLANNER_KEYS", "SUPPORTED_BELIEF_AWARE_PLANNER_NAMES", + "SUPPORTED_PROJECTION_TARGETS", "SUPPORTED_UNCERTAINTY_PLANNER_KEYS", "BeliefAwarePlannerInput", "PlannerTrackBelief", diff --git a/tests/planner/test_scenario_belief_track_projection.py b/tests/planner/test_scenario_belief_track_projection.py index bb7c6b490b..fd6da7490f 100644 --- a/tests/planner/test_scenario_belief_track_projection.py +++ b/tests/planner/test_scenario_belief_track_projection.py @@ -1,4 +1,4 @@ -"""Diagnostic contract tests for the identity-keyed ScenarioBelief projection. +"""Diagnostic contract tests for the entity-ID-keyed ScenarioBelief snapshot projection. These tests cover the additive interface only. They do not claim planner performance, identity-generation support, safety improvement, or benchmark @@ -20,6 +20,7 @@ from robot_sf.planner.scenario_belief_adapter import ( BELIEF_AWARE_PLANNER_INPUT_SCHEMA_VERSION, SUPPORTED_BELIEF_AWARE_PLANNER_NAMES, + SUPPORTED_PROJECTION_TARGETS, BeliefAwarePlannerInput, PlannerTrackBelief, project_belief_aware_planner_input, @@ -75,8 +76,8 @@ def _standalone_track(**overrides): return PlannerTrackBelief(**values) -def test_projection_retains_visible_and_occluded_tracks_by_canonical_id() -> None: - """Visible legacy rows and complete ID-keyed maintained tracks stay distinct.""" +def test_projection_retains_visible_and_occluded_tracks_by_snapshot_entity_id() -> None: + """Visible legacy rows and complete snapshot-keyed tracks stay distinct.""" belief = _belief_fixture() projected = project_belief_aware_planner_input( @@ -90,7 +91,8 @@ def test_projection_retains_visible_and_occluded_tracks_by_canonical_id() -> Non assert projected.diagnostics["visible_track_count"] == 1 assert projected.diagnostics["occluded_track_count"] == 1 assert projected.diagnostics["stale_track_count"] == 1 - assert projected.diagnostics["retained_track_count"] == 2 + assert projected.diagnostics["projected_track_count"] == 2 + assert projected.diagnostics["supported_projection_target"] is True assert projected.ordered_track_ids() == ("ped_000", "ped_001") assert projected.tracks["ped_000"].visibility is True assert projected.tracks["ped_001"].visibility is False @@ -99,7 +101,7 @@ def test_projection_retains_visible_and_occluded_tracks_by_canonical_id() -> Non def test_projection_is_independent_of_scenario_agent_order() -> None: - """Reordering source agents cannot exchange ID-keyed uncertainty metadata.""" + """Reordering source agents cannot exchange entity-ID-keyed snapshot metadata.""" belief = _belief_fixture() reordered = replace(belief, agents=tuple(reversed(belief.agents))) @@ -119,6 +121,50 @@ def test_projection_is_independent_of_scenario_agent_order() -> None: ) +def test_reused_entity_id_exposes_no_cross_snapshot_continuity_token() -> None: + """A reused snapshot ID has no fabricated generation or continuity token.""" + first_belief = _belief_fixture() + first_agent = first_belief.agents[0] + replacement_agent = replace( + first_agent, + position=replace(first_agent.position, mean_xy=(7.0, 7.0)), + velocity=replace(first_agent.velocity, mean_xy=(-0.4, 0.2)), + ) + second_belief = replace( + first_belief, + sim_time_s=0.6, + agents=(replacement_agent, first_belief.agents[1]), + ) + + first = project_belief_aware_planner_input( + first_belief, + planner_name="BeliefGuidedLocalPlanner", + ) + second = project_belief_aware_planner_input( + second_belief, + planner_name="BeliefGuidedLocalPlanner", + ) + first_track = first.tracks["ped_000"] + second_track = second.tracks["ped_000"] + + assert first_track.track_id == second_track.track_id == "ped_000" + assert not np.array_equal(first_track.mean_state, second_track.mean_state) + first_track_payload = first_track.to_dict() + second_track_payload = second_track.to_dict() + assert not hasattr(first_track, "identity_lifecycle_token") + assert not hasattr(second_track, "identity_lifecycle_token") + for payload in (first_track_payload, second_track_payload): + assert not any( + marker in key.lower() + for key in payload + for marker in ("token", "generation", "continuity") + ) + assert "identity_lifecycle_tokens" not in first.diagnostics + assert "identity_lifecycle_tokens" not in second.diagnostics + assert first.diagnostics["identity_generation_available"] is False + assert second.diagnostics["identity_generation_available"] is False + + def test_projection_distinguishes_missing_empty_and_unsupported() -> None: """Missing belief, empty belief, and unsupported planner fallback are explicit.""" belief = _belief_fixture() @@ -139,10 +185,13 @@ def test_projection_distinguishes_missing_empty_and_unsupported() -> None: assert missing.legacy_observation == {} assert empty.diagnostics["status"] == "empty_belief" assert empty.tracks == {} - assert unsupported.diagnostics["status"] == "unsupported_planner" + assert unsupported.diagnostics["status"] == "projection_target_not_supported" + assert unsupported.diagnostics["fallback_reason"] == "projection_target_not_supported" + assert unsupported.diagnostics["supported_projection_target"] is False assert unsupported.tracks == {} assert unsupported.legacy_observation["pedestrians"]["count"][0] == pytest.approx(1.0) assert SUPPORTED_BELIEF_AWARE_PLANNER_NAMES == frozenset({"BeliefGuidedLocalPlanner"}) + assert SUPPORTED_PROJECTION_TARGETS == SUPPORTED_BELIEF_AWARE_PLANNER_NAMES def test_projection_counts_out_of_range_as_non_visible_not_occluded() -> None: @@ -160,7 +209,7 @@ def test_projection_counts_out_of_range_as_non_visible_not_occluded() -> None: assert projected.diagnostics["visible_track_count"] == 1 assert projected.diagnostics["occluded_track_count"] == 0 - assert projected.diagnostics["retained_track_count"] == 2 + assert projected.diagnostics["projected_track_count"] == 2 def test_projection_rejects_malformed_track_and_keeps_safe_legacy_fallback() -> None: @@ -176,6 +225,7 @@ def test_projection_rejects_malformed_track_and_keeps_safe_legacy_fallback() -> assert "radius" in rejected.diagnostics["fallback_reason"] assert rejected.tracks == {} assert rejected.diagnostics["dropped_track_count"] == 0 + assert rejected.diagnostics["retired_track_count"] is None assert "pedestrians" in rejected.legacy_observation @@ -210,6 +260,7 @@ def test_typed_records_own_arrays_and_export_deterministically() -> None: assert list(payload["tracks"]) == ["2"] assert json.loads(wrapper.to_json()) == payload assert payload["diagnostics"]["status"] == "projected" + assert "identity_lifecycle_token" not in payload["tracks"]["2"] def test_typed_record_rejects_non_psd_covariance_and_key_mismatch() -> None: @@ -255,8 +306,8 @@ def test_typed_record_rejects_non_psd_covariance_and_key_mismatch() -> None: ) -def test_identity_lifecycle_limitation_is_explicit() -> None: - """The adapter does not fabricate a generation for numeric-ID reuse.""" +def test_snapshot_identity_limitation_is_explicit() -> None: + """The adapter exposes snapshot IDs without claiming lifecycle continuity.""" projected = project_belief_aware_planner_input( _belief_fixture(), planner_name="BeliefGuidedLocalPlanner", @@ -265,6 +316,14 @@ def test_identity_lifecycle_limitation_is_explicit() -> None: assert projected.diagnostics["identity_generation_available"] is False assert projected.diagnostics["identity_reuse_safe"] is False assert projected.diagnostics["lifecycle_reset_required"] is True + assert projected.diagnostics["stateful_identity_admitted"] is False + assert projected.diagnostics["retired_track_count"] is None + assert projected.diagnostics["uncertainty_semantics"] == { + "source": "adapter_derived", + "aggregate_confidence": "min(position_confidence, velocity_confidence)", + "state_covariance": "position_velocity_blocks_plus_zero_radius_block", + "radius_uncertainty": "unavailable_as_modelled", + } assert projected.diagnostics["retirement_tracking"] == ( "unavailable_at_scenario_belief_boundary" ) @@ -483,7 +542,6 @@ def test_track_field_and_time_validation_edges() -> None: ({"visibility": "yes"}, "visibility"), ({"source": ""}, "source"), ({"visibility_state": ""}, "visibility_state"), - ({"identity_lifecycle_token": ""}, "identity_lifecycle_token"), ): with pytest.raises(ValueError, match=match): _standalone_track(**overrides) From 82dc7ac67929be4a720a6162f72cfdf99135e0ca Mon Sep 17 00:00:00 2001 From: ll7 <32880741+ll7@users.noreply.github.com> Date: Fri, 4 Sep 2026 22:43:40 +0200 Subject: [PATCH 06/10] fix(planner): keep optional belief import guard compatible --- robot_sf/planner/scenario_belief_adapter.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/robot_sf/planner/scenario_belief_adapter.py b/robot_sf/planner/scenario_belief_adapter.py index de7775b7b7..fbe0492f05 100644 --- a/robot_sf/planner/scenario_belief_adapter.py +++ b/robot_sf/planner/scenario_belief_adapter.py @@ -44,7 +44,7 @@ def _load_scenario_belief_types() -> tuple[type[Any], type[Any]] | None: ScenarioBelief, VisibilityState, ) - except (ImportError, ModuleNotFoundError): + except ImportError: return None return ScenarioBelief, VisibilityState From 66e0426be26784ffece0b1dbbec9796dd1e33485 Mon Sep 17 00:00:00 2001 From: ll7 <32880741+ll7@users.noreply.github.com> Date: Fri, 4 Sep 2026 22:47:55 +0200 Subject: [PATCH 07/10] fix(planner): use canonical optional belief import --- robot_sf/planner/scenario_belief_adapter.py | 12 +++++------- .../planner/test_scenario_belief_track_projection.py | 10 +--------- 2 files changed, 6 insertions(+), 16 deletions(-) diff --git a/robot_sf/planner/scenario_belief_adapter.py b/robot_sf/planner/scenario_belief_adapter.py index fbe0492f05..bc1e82c8f6 100644 --- a/robot_sf/planner/scenario_belief_adapter.py +++ b/robot_sf/planner/scenario_belief_adapter.py @@ -16,6 +16,8 @@ import numpy as np +from robot_sf.common.optional_import import try_import + if TYPE_CHECKING: from robot_sf.representation.scenario_belief import ScenarioBelief @@ -39,14 +41,10 @@ def _load_scenario_belief_types() -> tuple[type[Any], type[Any]] | None: The canonical ``ScenarioBelief`` and ``VisibilityState`` classes, or ``None`` when the optional representation dependencies are unavailable. """ - try: - from robot_sf.representation.scenario_belief import ( # noqa: PLC0415 - ScenarioBelief, - VisibilityState, - ) - except ImportError: + scenario_belief_module = try_import("robot_sf.representation.scenario_belief") + if scenario_belief_module is None: return None - return ScenarioBelief, VisibilityState + return scenario_belief_module.ScenarioBelief, scenario_belief_module.VisibilityState @dataclass(frozen=True) diff --git a/tests/planner/test_scenario_belief_track_projection.py b/tests/planner/test_scenario_belief_track_projection.py index fd6da7490f..079056695a 100644 --- a/tests/planner/test_scenario_belief_track_projection.py +++ b/tests/planner/test_scenario_belief_track_projection.py @@ -7,7 +7,6 @@ from __future__ import annotations -import builtins import json from dataclasses import replace from types import SimpleNamespace @@ -331,14 +330,7 @@ def test_snapshot_identity_limitation_is_explicit() -> None: def test_lazy_representation_import_fails_closed(monkeypatch) -> None: """Optional representation imports should report unavailable instead of leaking ImportError.""" - original_import = builtins.__import__ - - def blocked_import(name, *args, **kwargs): - if name.startswith("robot_sf.representation.scenario_belief"): - raise ModuleNotFoundError("scenario belief dependencies blocked") - return original_import(name, *args, **kwargs) - - monkeypatch.setattr(builtins, "__import__", blocked_import) + monkeypatch.setattr(adapter, "try_import", lambda name: None) assert adapter._load_scenario_belief_types() is None From 4b2e3d5620d416a4ab979d3e6de19eb6a9d015f1 Mon Sep 17 00:00:00 2001 From: ll7 <32880741+ll7@users.noreply.github.com> Date: Sat, 5 Sep 2026 00:08:11 +0200 Subject: [PATCH 08/10] fix(planner): complete belief projection diagnostics --- .../issue_2538_scenario_belief_planner_projection.md | 3 ++- robot_sf/planner/scenario_belief_adapter.py | 9 ++++++++- tests/planner/test_scenario_belief_track_projection.py | 2 ++ 3 files changed, 12 insertions(+), 2 deletions(-) diff --git a/docs/context/issue_2538_scenario_belief_planner_projection.md b/docs/context/issue_2538_scenario_belief_planner_projection.md index 38ca702787..8cc8e37c23 100644 --- a/docs/context/issue_2538_scenario_belief_planner_projection.md +++ b/docs/context/issue_2538_scenario_belief_planner_projection.md @@ -34,7 +34,8 @@ The additive issue #8050 diagnostic seam also provides `BeliefGuidedLocalPlanner`. It retains every `ScenarioBelief.agents` entry in an immutable, entity-ID-keyed `tracks` mapping, including entries absent from the visible legacy rows, and reports distinct `no_belief`, `empty_belief`, `projection_target_not_supported`, `invalid_belief`, -and `projected` statuses. Serialization is versioned and deterministic; legacy observations and +and `projected` statuses. Its diagnostics expose both retained and projected track counts. +Serialization is versioned and deterministic; legacy observations and the existing stream-gap path are unchanged. `track_id` is the entity identifier supplied by one `ScenarioBelief` snapshot, not a diff --git a/robot_sf/planner/scenario_belief_adapter.py b/robot_sf/planner/scenario_belief_adapter.py index bc1e82c8f6..ee1d2e1af6 100644 --- a/robot_sf/planner/scenario_belief_adapter.py +++ b/robot_sf/planner/scenario_belief_adapter.py @@ -570,7 +570,13 @@ def _age_steps(age_s: Any, timestep_s: Any) -> int: if age == 0.0: return 0 raise ValueError("positive observation age requires a positive belief timestep") - return max(0, int(np.ceil(age / timestep - 1e-9))) + ratio = age / timestep + if ratio <= 0.0: + return 0 + nearest_step = round(ratio) + if nearest_step > 0 and np.isclose(ratio, nearest_step, atol=1e-9, rtol=0.0): + return nearest_step + return max(1, int(np.ceil(ratio))) def _planner_track_from_entity( @@ -683,6 +689,7 @@ def _belief_projection_diagnostics( for track in ordered_tracks ), "stale_track_count": sum(track.age_steps > 0 for track in ordered_tracks), + "retained_track_count": len(ordered_tracks), "projected_track_count": len(ordered_tracks), "retired_track_count": None, "dropped_track_count": 0, diff --git a/tests/planner/test_scenario_belief_track_projection.py b/tests/planner/test_scenario_belief_track_projection.py index 079056695a..8bc35443e9 100644 --- a/tests/planner/test_scenario_belief_track_projection.py +++ b/tests/planner/test_scenario_belief_track_projection.py @@ -90,6 +90,7 @@ def test_projection_retains_visible_and_occluded_tracks_by_snapshot_entity_id() assert projected.diagnostics["visible_track_count"] == 1 assert projected.diagnostics["occluded_track_count"] == 1 assert projected.diagnostics["stale_track_count"] == 1 + assert projected.diagnostics["retained_track_count"] == 2 assert projected.diagnostics["projected_track_count"] == 2 assert projected.diagnostics["supported_projection_target"] is True assert projected.ordered_track_ids() == ("ped_000", "ped_001") @@ -558,6 +559,7 @@ def test_track_field_and_time_validation_edges() -> None: with pytest.raises(ValueError, match="positive observation age"): adapter._age_steps(1.0, 0.0) assert adapter._age_steps(0.0, 0.0) == 0 + assert adapter._age_steps(5e-10, 1.0) == 1 assert adapter._age_steps(0.21, 0.1) == 3 From f6c71c1a7962244cb9f9f0b7db8d36b072855591 Mon Sep 17 00:00:00 2001 From: ll7 <32880741+ll7@users.noreply.github.com> Date: Sat, 5 Sep 2026 00:53:51 +0200 Subject: [PATCH 09/10] fix(planner): harden belief input contracts --- robot_sf/planner/scenario_belief_adapter.py | 63 +++++++++++-- .../test_scenario_belief_track_projection.py | 90 +++++++++++++++++++ 2 files changed, 145 insertions(+), 8 deletions(-) diff --git a/robot_sf/planner/scenario_belief_adapter.py b/robot_sf/planner/scenario_belief_adapter.py index ee1d2e1af6..2e42f08fd9 100644 --- a/robot_sf/planner/scenario_belief_adapter.py +++ b/robot_sf/planner/scenario_belief_adapter.py @@ -175,6 +175,8 @@ def _readonly_float_array( raise ValueError(f"{name} must have shape {shape}, got {array.shape}") if not np.issubdtype(array.dtype, np.number): raise ValueError(f"{name} must use a numeric dtype") + if np.iscomplexobj(array): + raise ValueError(f"{name} must contain real-valued data") try: owned = np.array(array, dtype=np.float64, copy=True) except (TypeError, ValueError) as exc: @@ -203,6 +205,8 @@ def _readonly_covariance(value: Any) -> np.ndarray: array = np.asarray(value) except (TypeError, ValueError) as exc: raise ValueError("covariance must be a numeric array") from exc + if np.iscomplexobj(array): + raise ValueError("covariance must contain real-valued data") if array.shape == (4, 4): try: normalized = np.zeros((5, 5), dtype=np.float64) @@ -388,6 +392,41 @@ def _copy_runtime_value(value: Any) -> Any: return value +def _freeze_runtime_value(value: Any) -> Any: + """Copy nested diagnostics into immutable containers with owned arrays. + + Returns: + A recursively copied value whose mappings and sequences cannot be mutated. + """ + if isinstance(value, np.ndarray): + copied = np.array(value, copy=True) + copied.setflags(write=False) + return copied + if isinstance(value, Mapping): + return MappingProxyType( + {key: _freeze_runtime_value(nested) for key, nested in value.items()} + ) + if isinstance(value, (list, tuple)): + return tuple(_freeze_runtime_value(nested) for nested in value) + if isinstance(value, np.generic): + return value.item() + return value + + +def _entity_float_array(value: np.ndarray) -> np.ndarray: + """Convert one already-shaped entity array after rejecting complex values. + + Returns: + A float64 entity array. + """ + if np.iscomplexobj(value): + raise ValueError("entity state or covariance must contain real-valued data") + try: + return np.asarray(value, dtype=np.float64) + except (TypeError, ValueError) as exc: + raise ValueError("entity state or covariance is malformed") from exc + + def _runtime_value_is_finite(value: Any) -> bool: """Return whether nested numeric runtime values are finite.""" if isinstance(value, np.ndarray): @@ -474,9 +513,7 @@ def __post_init__(self) -> None: raise ValueError("schema_version must be a non-empty string") if not isinstance(self.diagnostics, Mapping): raise TypeError("diagnostics must be a mapping") - object.__setattr__( - self, "diagnostics", MappingProxyType(_copy_runtime_value(self.diagnostics)) - ) + object.__setattr__(self, "diagnostics", _freeze_runtime_value(self.diagnostics)) @property def projection(self) -> Mapping[str, Any]: @@ -547,6 +584,8 @@ def _belief_step(belief: Any) -> int: return 0 raise ValueError("belief timestep_s must be positive when sim_time_s is non-zero") ratio = sim_time_s / timestep_s + if not np.isfinite(ratio): + raise ValueError("belief sim_time_s/timestep_s ratio must be finite") rounded = round(ratio) if not np.isclose(ratio, rounded, atol=1e-6, rtol=0.0): raise ValueError("belief sim_time_s is not aligned to timestep_s") @@ -566,11 +605,15 @@ def _age_steps(age_s: Any, timestep_s: Any) -> int: raise ValueError("last_observed_age_s must be numeric") from exc if not np.isfinite(age) or age < 0.0: raise ValueError("last_observed_age_s must be finite and non-negative") + if not np.isfinite(timestep): + raise ValueError("belief timestep_s must be finite") if timestep <= 0.0: if age == 0.0: return 0 raise ValueError("positive observation age requires a positive belief timestep") ratio = age / timestep + if not np.isfinite(ratio): + raise ValueError("last_observed_age_s/timestep_s ratio must be finite") if ratio <= 0.0: return 0 nearest_step = round(ratio) @@ -597,16 +640,20 @@ def _planner_track_from_entity( if not isinstance(visibility_state, visibility_type): raise ValueError("visibility_state is malformed") try: - position = np.asarray(agent.position.mean_xy, dtype=np.float64).reshape(-1) - velocity = np.asarray(agent.velocity.mean_xy, dtype=np.float64).reshape(-1) - position_covariance = np.asarray(agent.position.covariance_xy, dtype=np.float64) - velocity_covariance = np.asarray(agent.velocity.covariance_xy, dtype=np.float64) + position = np.asarray(agent.position.mean_xy) + velocity = np.asarray(agent.velocity.mean_xy) + position_covariance = np.asarray(agent.position.covariance_xy) + velocity_covariance = np.asarray(agent.velocity.covariance_xy) except (AttributeError, TypeError, ValueError) as exc: raise ValueError("entity state or covariance is malformed") from exc if position.shape != (2,) or velocity.shape != (2,): - raise ValueError("entity position and velocity must have two coordinates") + raise ValueError("entity position and velocity must each have shape (2,) (two coordinates)") if position_covariance.shape != (2, 2) or velocity_covariance.shape != (2, 2): raise ValueError("entity position and velocity covariance must be 2x2") + position = _entity_float_array(position) + velocity = _entity_float_array(velocity) + position_covariance = _entity_float_array(position_covariance) + velocity_covariance = _entity_float_array(velocity_covariance) try: radius = float(agent.radius) except (AttributeError, TypeError, ValueError) as exc: diff --git a/tests/planner/test_scenario_belief_track_projection.py b/tests/planner/test_scenario_belief_track_projection.py index 8bc35443e9..0d06ce2119 100644 --- a/tests/planner/test_scenario_belief_track_projection.py +++ b/tests/planner/test_scenario_belief_track_projection.py @@ -421,6 +421,35 @@ def fail_legacy(_belief): assert legacy_failure.diagnostics["fallback_reason"] == "legacy_observation_unavailable" +def test_projection_rejects_nonfinite_time_ratios_as_invalid_belief() -> None: + """Overflowing finite time ratios fail closed before integer conversion.""" + belief = _belief_fixture() + + with pytest.raises(ValueError, match="ratio"): + adapter._belief_step(SimpleNamespace(sim_time_s=1e308, timestep_s=1e-308)) + with pytest.raises(ValueError, match="ratio"): + adapter._age_steps(1e308, 1e-308) + + overflowing_time = project_belief_aware_planner_input( + replace(belief, sim_time_s=1e308, timestep_s=1e-308), + planner_name="BeliefGuidedLocalPlanner", + ) + overflowing_age = project_belief_aware_planner_input( + replace( + belief, + agents=( + replace(belief.agents[0], last_observed_age_s=1e308), + belief.agents[1], + ), + ), + planner_name="BeliefGuidedLocalPlanner", + ) + + for rejected in (overflowing_time, overflowing_age): + assert rejected.diagnostics["status"] == "invalid_belief" + assert "ratio" in rejected.diagnostics["fallback_reason"] + + def test_validation_helpers_reject_malformed_values() -> None: """Array, covariance, probability, integer, and ID validators fail closed.""" @@ -436,10 +465,14 @@ def __array__(self): adapter._readonly_float_array("state", ["x", "y"], shape=(2,)) with pytest.raises(ValueError, match="finite"): adapter._readonly_float_array("state", [np.nan, 1.0], shape=(2,)) + with pytest.raises(ValueError, match="real-valued"): + adapter._readonly_float_array("state", np.array([1.0 + 2.0j, 1.0]), shape=(2,)) with pytest.raises(ValueError, match="numeric"): adapter._readonly_covariance([[1.0], [1.0, 2.0]]) assert adapter._readonly_covariance(np.eye(4)).shape == (5, 5) + with pytest.raises(ValueError, match="real-valued"): + adapter._readonly_covariance(np.eye(4, dtype=np.complex128)) with pytest.raises(ValueError, match="numeric"): adapter._readonly_covariance(np.full((5, 5), "x", dtype=object)) with pytest.raises(ValueError, match="shape"): @@ -529,6 +562,34 @@ def test_typed_input_validation_and_json_edges() -> None: non_json.to_dict() +def test_diagnostics_are_deeply_immutable_for_stable_json() -> None: + """Nested diagnostics mutation cannot change a constructed input's JSON.""" + diagnostics = { + "nested": {"status": "projected"}, + "values": [1], + "array": np.asarray([1.0]), + } + wrapper = BeliefAwarePlannerInput( + legacy_observation={}, + tracks={}, + belief_step=0, + diagnostics=diagnostics, + ) + initial_json = wrapper.to_json() + + with pytest.raises(TypeError): + wrapper.diagnostics["nested"]["status"] = "changed" + with pytest.raises(AttributeError): + wrapper.diagnostics["values"].append(2) + with pytest.raises(ValueError, match="read-only"): + wrapper.diagnostics["array"][0] = 2.0 + + diagnostics["nested"]["status"] = "changed" + diagnostics["values"].append(2) + diagnostics["array"][0] = 2.0 + assert wrapper.to_json() == initial_json + + def test_track_field_and_time_validation_edges() -> None: """Planner-track fields and canonical time metadata reject unsafe values.""" for overrides, match in ( @@ -582,6 +643,20 @@ def project(candidate): project(replace(agent, position=SimpleNamespace(mean_xy=(0.0, 0.0)))) with pytest.raises(ValueError, match="two coordinates"): project(replace(agent, position=replace(agent.position, mean_xy=(0.0,)))) + with pytest.raises(ValueError, match="shape"): + project( + replace( + agent, + position=replace(agent.position, mean_xy=np.asarray([[0.0, 0.0]])), + ) + ) + with pytest.raises(ValueError, match="shape"): + project( + replace( + agent, + velocity=replace(agent.velocity, mean_xy=np.asarray([[0.0, 0.0]])), + ) + ) with pytest.raises(ValueError, match="2x2"): project( replace( @@ -589,6 +664,21 @@ def project(candidate): position=replace(agent.position, covariance_xy=((1.0, 0.0, 0.0),) * 3), ) ) + complex_entity_values = ( + ("position", replace(agent.position, mean_xy=np.asarray([1.0 + 2.0j, 0.0]))), + ("velocity", replace(agent.velocity, mean_xy=np.asarray([1.0 + 2.0j, 0.0]))), + ( + "position", + replace(agent.position, covariance_xy=np.eye(2, dtype=np.complex128)), + ), + ( + "velocity", + replace(agent.velocity, covariance_xy=np.eye(2, dtype=np.complex128)), + ), + ) + for field, value in complex_entity_values: + with pytest.raises(ValueError, match="real-valued"): + project(replace(agent, **{field: value})) with pytest.raises(ValueError, match="radius"): project(replace(agent, radius="bad")) with pytest.raises(ValueError, match="source adapter"): From 2f45dcddd408c9141df58d08389673f2ec9c47cb Mon Sep 17 00:00:00 2001 From: ll7 <32880741+ll7@users.noreply.github.com> Date: Sat, 5 Sep 2026 03:14:08 +0200 Subject: [PATCH 10/10] fix(planner): preserve legacy fallback for invalid belief inputs --- robot_sf/planner/scenario_belief_adapter.py | 6 +++--- tests/planner/test_scenario_belief_track_projection.py | 1 + 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/robot_sf/planner/scenario_belief_adapter.py b/robot_sf/planner/scenario_belief_adapter.py index 2e42f08fd9..582bbde976 100644 --- a/robot_sf/planner/scenario_belief_adapter.py +++ b/robot_sf/planner/scenario_belief_adapter.py @@ -833,6 +833,7 @@ def project_belief_aware_planner_input( diagnostics=diagnostics, ) + legacy_observation, legacy_reason = _safe_legacy_observation(belief) scenario_belief_types = _load_scenario_belief_types() if scenario_belief_types is None: diagnostics = _belief_projection_diagnostics( @@ -842,7 +843,7 @@ def project_belief_aware_planner_input( fallback_reason="scenario_belief_representation_unavailable", ) return BeliefAwarePlannerInput( - legacy_observation={}, + legacy_observation=legacy_observation, tracks={}, belief_step=0, diagnostics=diagnostics, @@ -856,13 +857,12 @@ def project_belief_aware_planner_input( fallback_reason="belief_type_unsupported", ) return BeliefAwarePlannerInput( - legacy_observation={}, + legacy_observation=legacy_observation, tracks={}, belief_step=0, diagnostics=diagnostics, ) - legacy_observation, legacy_reason = _safe_legacy_observation(belief) try: belief_step = _belief_step(belief) except ValueError as exc: diff --git a/tests/planner/test_scenario_belief_track_projection.py b/tests/planner/test_scenario_belief_track_projection.py index 0d06ce2119..69210cb00f 100644 --- a/tests/planner/test_scenario_belief_track_projection.py +++ b/tests/planner/test_scenario_belief_track_projection.py @@ -372,6 +372,7 @@ def test_projection_invalid_belief_fallbacks_and_alias(monkeypatch) -> None: assert unavailable.diagnostics["fallback_reason"] == ( "scenario_belief_representation_unavailable" ) + assert unavailable.legacy_observation["pedestrians"]["count"][0] == pytest.approx(1.0) monkeypatch.setattr(adapter, "_load_scenario_belief_types", original_loader) unsupported = project_belief_aware_planner_input(