424 lines
15 KiB
Python
424 lines
15 KiB
Python
from __future__ import annotations
|
|
|
|
import copy
|
|
import json
|
|
from dataclasses import replace
|
|
|
|
import numpy as np
|
|
import pytest
|
|
|
|
from k1link.compute.temporal_occupied_layer import TemporalFrameProjection
|
|
from k1link.compute.track_geometry import (
|
|
PointSlab,
|
|
TrackGeometry,
|
|
TrackGeometryCurrentness,
|
|
TrackGeometryEvidenceState,
|
|
TrackGeometryFrame,
|
|
TrackGeometryMetricBasis,
|
|
TrackGeometryOwnerKind,
|
|
TrackGeometrySourceBinding,
|
|
)
|
|
from k1link.perception.adapters import (
|
|
observations_from_track_geometry,
|
|
temporal_obstacles_from_e34_projection,
|
|
)
|
|
from k1link.perception.contracts import (
|
|
BoundingRegion2D,
|
|
ClockBasis,
|
|
CorridorIntersection,
|
|
EvidenceBasis,
|
|
EvidenceCurrentness,
|
|
GridCell,
|
|
HistorySample,
|
|
LocalObstacleMap,
|
|
MetricGeometry,
|
|
ModalityOutcome,
|
|
ModalityStatus,
|
|
MotionState,
|
|
ObjectProposal2D,
|
|
ObstacleObservation,
|
|
PerceptionContractError,
|
|
QualificationState,
|
|
SourceAccounting,
|
|
SourceEnvelope,
|
|
TemporalObstacle,
|
|
TemporalState,
|
|
ThreatAssessment,
|
|
ThreatDecision,
|
|
TimestampBundle,
|
|
validate_exclusive_point_ownership,
|
|
)
|
|
from k1link.perception.providers import (
|
|
GraphAuthority,
|
|
ProviderContractError,
|
|
ProviderPin,
|
|
ProviderRole,
|
|
QueuePolicy,
|
|
ReferencePerceptionGraphConfig,
|
|
)
|
|
|
|
|
|
def _status(outcome: ModalityOutcome = ModalityOutcome.AVAILABLE) -> ModalityStatus:
|
|
return ModalityStatus(
|
|
available=outcome is ModalityOutcome.AVAILABLE,
|
|
outcome=outcome,
|
|
reason=outcome.value,
|
|
)
|
|
|
|
|
|
def _source(*, lidar: ModalityOutcome = ModalityOutcome.AVAILABLE) -> SourceEnvelope:
|
|
return SourceEnvelope(
|
|
source_id="RAVNOVES00",
|
|
session_id="20260720T065719Z_viewer_live",
|
|
frame_id="frame-000001",
|
|
sequence=1,
|
|
timestamps=TimestampBundle(
|
|
utc_ns=1_786_000_000_000_000_000,
|
|
monotonic_ns=1_000_000,
|
|
source_ns=35_421_857_292,
|
|
clock_basis=ClockBasis.RECORDED_HOST,
|
|
),
|
|
source_age_ns=0,
|
|
binding_reason="exact-recorded-source",
|
|
calibration_id="camera-1-kb4-05f3ad9b",
|
|
representation_id="registered-map-increment-v1",
|
|
image=_status(),
|
|
registered_point_increment=_status(lidar),
|
|
pose=_status(),
|
|
)
|
|
|
|
|
|
def _proposal(*, semantic_hint: str | None = None) -> ObjectProposal2D:
|
|
return ObjectProposal2D(
|
|
proposal_id="proposal-1",
|
|
source_id="RAVNOVES00",
|
|
frame_id="frame-000001",
|
|
region=BoundingRegion2D(10.0, 20.0, 80.0, 100.0),
|
|
objectness=0.91,
|
|
provider_id="triton-yolox-s-raw-kb4/v1",
|
|
model_id="yolox_s:1",
|
|
preprocess_id="raw-kb4-valid-fov-letterbox/v1",
|
|
semantic_hint=semantic_hint,
|
|
provider_tracklet="detector-local-7",
|
|
)
|
|
|
|
|
|
def _geometry() -> MetricGeometry:
|
|
return MetricGeometry(
|
|
coordinate_frame="map",
|
|
centroid_xyz_m=(4.0, 1.0, 0.5),
|
|
range_m=4.15,
|
|
covariance_diagonal_m2=(0.04, 0.04, 0.09),
|
|
)
|
|
|
|
|
|
def _observation(*, semantic_hint: str | None = None, point_id: int = 5) -> ObstacleObservation:
|
|
return ObstacleObservation(
|
|
observation_id=f"observation-{point_id}",
|
|
occupancy_key="frame-local-occupied-1",
|
|
source_id="RAVNOVES00",
|
|
frame_id="frame-000001",
|
|
evidence_time_ns=35_421_857_292,
|
|
basis=EvidenceBasis.FUSED,
|
|
currentness=EvidenceCurrentness.CURRENT,
|
|
occupied_support=True,
|
|
source_point_ids=(point_id,),
|
|
metric_geometry=_geometry(),
|
|
proposal_ids=("proposal-1",),
|
|
semantic_hint=semantic_hint,
|
|
reason_codes=("current-qualified-points",),
|
|
)
|
|
|
|
|
|
def _temporal(*, state: TemporalState = TemporalState.CURRENT) -> TemporalObstacle:
|
|
age_ns = 0 if state is TemporalState.CURRENT else 50_000_000
|
|
cells = () if state is TemporalState.EXPIRED else (GridCell(8, 2, 1),)
|
|
return TemporalObstacle(
|
|
component_id=f"component-{state.value}",
|
|
identity_scope="ephemeral",
|
|
state=state,
|
|
ttl_ns=750_000_000,
|
|
last_hit_ns=35_421_857_292,
|
|
age_ns=age_ns,
|
|
association_basis="current-spatial-support",
|
|
history=(
|
|
HistorySample(
|
|
frame_id="frame-000001",
|
|
evidence_time_ns=35_421_857_292,
|
|
centroid_xyz_m=(4.0, 1.0, 0.5),
|
|
),
|
|
),
|
|
cells=cells,
|
|
coordinate_frame=None if state is TemporalState.EXPIRED else "map",
|
|
last_centroid_xyz_m=None if state is TemporalState.EXPIRED else (4.0, 1.0, 0.5),
|
|
motion=MotionState.UNKNOWN,
|
|
motion_confidence=0.0,
|
|
motion_reason="insufficient-history",
|
|
)
|
|
|
|
|
|
def test_six_contracts_round_trip_with_exact_json_shapes() -> None:
|
|
source = _source()
|
|
proposal = _proposal()
|
|
observation = _observation()
|
|
temporal = _temporal()
|
|
obstacle_map = LocalObstacleMap(
|
|
source_id=source.source_id,
|
|
session_id=source.session_id,
|
|
frame_id=source.frame_id,
|
|
graph_id="reference-perception-graph/v1",
|
|
generated_monotonic_ns=1_010_000,
|
|
output_age_ns=10_000,
|
|
occupied=(temporal,),
|
|
unknown=(_temporal(state=TemporalState.HELD),),
|
|
camera_uncertainty=(proposal,),
|
|
accounting=SourceAccounting(1, 1, 0, 0),
|
|
)
|
|
assessment = ThreatAssessment(
|
|
assessment_id="assessment-1",
|
|
component_id=temporal.component_id,
|
|
rig_profile_id="virtual-rig-ravnoves00/v1",
|
|
corridor_profile_id="virtual-corridor-ravnoves00/v1",
|
|
qualification=QualificationState.QUALIFIED,
|
|
relative_speed_mps=-0.2,
|
|
closest_approach_m=3.0,
|
|
ttc_seconds=None,
|
|
corridor_intersection=CorridorIntersection.CLEAR,
|
|
decision=ThreatDecision.NOT_THREAT,
|
|
reason_codes=("qualified-corridor-clear",),
|
|
)
|
|
|
|
values = (
|
|
(SourceEnvelope, source),
|
|
(ObjectProposal2D, proposal),
|
|
(ObstacleObservation, observation),
|
|
(TemporalObstacle, temporal),
|
|
(LocalObstacleMap, obstacle_map),
|
|
(ThreatAssessment, assessment),
|
|
)
|
|
for contract_type, contract in values:
|
|
document = json.loads(json.dumps(contract.to_dict()))
|
|
assert contract_type.from_dict(document) == contract
|
|
incompatible = copy.deepcopy(document)
|
|
incompatible["unexpected"] = True
|
|
with pytest.raises(PerceptionContractError, match="fields are incompatible"):
|
|
contract_type.from_dict(incompatible)
|
|
|
|
|
|
def test_object_proposal_is_valid_without_a_semantic_class() -> None:
|
|
proposal = _proposal(semantic_hint=None)
|
|
assert ObjectProposal2D.from_dict(proposal.to_dict()) == proposal
|
|
assert proposal.semantic_hint is None
|
|
assert not hasattr(proposal, "range_m")
|
|
|
|
|
|
def test_semantic_change_does_not_change_occupancy_identity() -> None:
|
|
before = _observation(semantic_hint="car")
|
|
after = replace(before, semantic_hint="person")
|
|
assert before.occupancy_identity == after.occupancy_identity
|
|
assert before.source_point_ids == after.source_point_ids
|
|
|
|
|
|
def test_geometry_only_obstacle_is_valid_without_class_or_proposal() -> None:
|
|
observation = replace(
|
|
_observation(),
|
|
basis=EvidenceBasis.LIDAR,
|
|
proposal_ids=(),
|
|
semantic_hint=None,
|
|
)
|
|
assert ObstacleObservation.from_dict(observation.to_dict()) == observation
|
|
|
|
|
|
def test_camera_only_observation_remains_non_metric_uncertainty() -> None:
|
|
observation = ObstacleObservation(
|
|
observation_id="camera-observation-1",
|
|
occupancy_key="camera-uncertainty-1",
|
|
source_id="RAVNOVES00",
|
|
frame_id="frame-000001",
|
|
evidence_time_ns=35_421_857_292,
|
|
basis=EvidenceBasis.CAMERA,
|
|
currentness=EvidenceCurrentness.CURRENT,
|
|
occupied_support=False,
|
|
source_point_ids=(),
|
|
metric_geometry=None,
|
|
proposal_ids=("proposal-1",),
|
|
semantic_hint=None,
|
|
reason_codes=("camera-only-no-metric-support",),
|
|
)
|
|
assert observation.metric_geometry is None
|
|
|
|
|
|
def test_range_without_current_qualified_points_is_rejected() -> None:
|
|
with pytest.raises(PerceptionContractError, match="qualified points"):
|
|
replace(_observation(), source_point_ids=())
|
|
with pytest.raises(PerceptionContractError, match="non-current"):
|
|
replace(_observation(), currentness=EvidenceCurrentness.HELD)
|
|
|
|
|
|
def test_duplicate_source_point_ownership_is_rejected_across_observations() -> None:
|
|
first = _observation(point_id=5)
|
|
second = replace(first, observation_id="observation-duplicate")
|
|
with pytest.raises(PerceptionContractError, match="duplicate observation ownership"):
|
|
validate_exclusive_point_ownership((first, second))
|
|
|
|
|
|
def test_missing_lidar_cannot_be_published_as_free_space() -> None:
|
|
source = _source(lidar=ModalityOutcome.UNAVAILABLE)
|
|
assert source.registered_point_increment.available is False
|
|
with pytest.raises(PerceptionContractError, match="implicit free space"):
|
|
LocalObstacleMap(
|
|
source_id=source.source_id,
|
|
session_id=source.session_id,
|
|
frame_id=source.frame_id,
|
|
graph_id="reference-perception-graph/v1",
|
|
generated_monotonic_ns=1,
|
|
output_age_ns=0,
|
|
occupied=(),
|
|
unknown=(),
|
|
camera_uncertainty=(_proposal(),),
|
|
accounting=SourceAccounting(1, 1, 0, 0),
|
|
free_space_claimed=True,
|
|
)
|
|
|
|
|
|
def test_threat_requires_profiles_and_never_grants_physical_authority() -> None:
|
|
with pytest.raises(PerceptionContractError, match="rig profile id"):
|
|
ThreatAssessment(
|
|
assessment_id="assessment-1",
|
|
component_id="component-current",
|
|
rig_profile_id="",
|
|
corridor_profile_id="corridor/v1",
|
|
qualification=QualificationState.UNQUALIFIED,
|
|
relative_speed_mps=None,
|
|
closest_approach_m=None,
|
|
ttc_seconds=None,
|
|
corridor_intersection=CorridorIntersection.UNKNOWN,
|
|
decision=ThreatDecision.UNKNOWN,
|
|
reason_codes=("missing-rig",),
|
|
)
|
|
with pytest.raises(PerceptionContractError, match="collision or actuation"):
|
|
ThreatAssessment(
|
|
assessment_id="assessment-1",
|
|
component_id="component-current",
|
|
rig_profile_id="rig/v1",
|
|
corridor_profile_id="corridor/v1",
|
|
qualification=QualificationState.QUALIFIED,
|
|
relative_speed_mps=1.0,
|
|
closest_approach_m=0.5,
|
|
ttc_seconds=1.0,
|
|
corridor_intersection=CorridorIntersection.INTERSECTS,
|
|
decision=ThreatDecision.THREAT,
|
|
reason_codes=("intersects",),
|
|
actuation_allowed=True,
|
|
)
|
|
|
|
|
|
def test_reference_graph_config_pins_all_roles_and_queue_bounds() -> None:
|
|
config = ReferencePerceptionGraphConfig(
|
|
graph_id="reference-perception-graph/v1",
|
|
source_profile_id="m4-ravnoves00-recorded-realtime/v1",
|
|
providers=tuple(
|
|
ProviderPin(role, f"{role.value}-provider", "v1", "78a3dc2", "a" * 64)
|
|
for role in ProviderRole
|
|
if role is not ProviderRole.ROLLING
|
|
),
|
|
queues=tuple(
|
|
QueuePolicy(stage, 2, 80_000_000, 200_000_000)
|
|
for stage in ("detector", "geometry", "temporal", "threat")
|
|
),
|
|
authority=GraphAuthority(),
|
|
)
|
|
assert ReferencePerceptionGraphConfig.from_dict(config.to_dict()) == config
|
|
with pytest.raises(ProviderContractError, match="each provider role"):
|
|
replace(config, providers=config.providers[:-1])
|
|
with pytest.raises(ProviderContractError, match="physical or command authority"):
|
|
GraphAuthority(commands_enabled=True)
|
|
|
|
|
|
def test_track_geometry_adapter_preserves_exact_point_ownership_without_class() -> None:
|
|
binding = TrackGeometrySourceBinding(
|
|
source_pack_id=(
|
|
"e10-lidar-pack-576c994a6c814e2592dd6240ace3902a5db94843312c759a73ba0c9166157d2b"
|
|
),
|
|
source_session_id="20260720T065719Z_viewer_live",
|
|
representation_profile_id="registered-map-increment-v1",
|
|
e31_qualification_id=(
|
|
"e31-source-qualification-b2460a5eb143688c7eea6821b2277e13aea79868abe81d83f7e78548c119159a"
|
|
),
|
|
calibration_sha256="0" * 64,
|
|
coordinate_frame="map",
|
|
time_basis="recorded-host",
|
|
selected_offset_ms=0,
|
|
)
|
|
slab = PointSlab(
|
|
frame_index=1,
|
|
source_frame_index=1,
|
|
source_point_count=10,
|
|
coordinate_frame="map",
|
|
owner_keys=("geometry-1",),
|
|
source_indices=np.asarray([7, 8], dtype="<i8"),
|
|
points_xyz_m=np.asarray([[4.0, 1.0, 0.5], [4.2, 1.0, 0.5]], dtype="<f4"),
|
|
owner_indices=np.asarray([0, 0], dtype="<u4"),
|
|
)
|
|
frame = TrackGeometryFrame(
|
|
binding=binding,
|
|
frame_index=1,
|
|
source_frame_index=1,
|
|
session_seconds=35.421857292,
|
|
source_available=True,
|
|
point_slab=slab,
|
|
geometries=(
|
|
TrackGeometry(
|
|
owner_key="geometry-1",
|
|
owner_kind=TrackGeometryOwnerKind.GEOMETRY_CLUSTER,
|
|
evidence_state=TrackGeometryEvidenceState.GEOMETRY_ONLY,
|
|
currentness=TrackGeometryCurrentness.CURRENT,
|
|
metric_basis=TrackGeometryMetricBasis.CURRENT_POINTS,
|
|
reason_codes=("current-geometry-cluster",),
|
|
range_m=4.15,
|
|
),
|
|
),
|
|
)
|
|
observations = observations_from_track_geometry(
|
|
frame,
|
|
source_id="RAVNOVES00",
|
|
frame_id="frame-000001",
|
|
evidence_time_ns=35_421_857_292,
|
|
)
|
|
assert len(observations) == 1
|
|
assert observations[0].basis is EvidenceBasis.LIDAR
|
|
assert observations[0].source_point_ids == (7, 8)
|
|
assert observations[0].semantic_hint is None
|
|
assert observations[0].metric_geometry is not None
|
|
|
|
|
|
def test_e34_adapter_marks_ids_ephemeral_and_held_as_unknown_state() -> None:
|
|
component = {
|
|
"temporal_id": 3,
|
|
"last_observed_age_seconds": 0.1,
|
|
"association_reason": "ttl-hold-last-hit",
|
|
"centroid_map_xyz_m": [4.0, 1.0, 0.5],
|
|
"cell_row_start": 0,
|
|
"cell_row_count": 1,
|
|
"history_tail": [
|
|
{
|
|
"frame_index": 10,
|
|
"session_seconds": 36.0,
|
|
"centroid_map_xyz_m": [4.0, 1.0, 0.5],
|
|
}
|
|
],
|
|
"semantic_provenance": {"labels": ["car"]},
|
|
}
|
|
projection = TemporalFrameProjection(
|
|
document={"current": [], "held": [component], "expired": []},
|
|
cell_rows=np.asarray([[8, 2, 1]], dtype="<i4"),
|
|
)
|
|
obstacles = temporal_obstacles_from_e34_projection(
|
|
projection,
|
|
coordinate_frame="map",
|
|
ttl_ns=750_000_000,
|
|
)
|
|
assert obstacles[0].identity_scope == "ephemeral"
|
|
assert obstacles[0].state is TemporalState.HELD
|
|
assert obstacles[0].cells == (GridCell(8, 2, 1),)
|