feat(perception): add bounded reference graph
This commit is contained in:
DCCONSTRUCTIONS
@@ -0,0 +1,544 @@
|
||||
from __future__ import annotations
|
||||
|
||||
import json
|
||||
import threading
|
||||
from collections.abc import Iterator
|
||||
from dataclasses import replace
|
||||
from pathlib import Path
|
||||
from threading import Event
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from k1link.compute.pipeline_telemetry import PipelineTelemetryIdentity
|
||||
from k1link.perception.baseline import BASELINE_PROFILE_ID
|
||||
from k1link.perception.contracts import (
|
||||
BoundingRegion2D,
|
||||
ClockBasis,
|
||||
CorridorIntersection,
|
||||
EvidenceBasis,
|
||||
EvidenceCurrentness,
|
||||
GridCell,
|
||||
HistorySample,
|
||||
LocalObstacleMap,
|
||||
MetricGeometry,
|
||||
ModalityOutcome,
|
||||
ModalityStatus,
|
||||
MotionState,
|
||||
ObjectProposal2D,
|
||||
ObstacleObservation,
|
||||
QualificationState,
|
||||
SourceEnvelope,
|
||||
TemporalObstacle,
|
||||
TemporalState,
|
||||
ThreatAssessment,
|
||||
ThreatDecision,
|
||||
TimestampBundle,
|
||||
)
|
||||
from k1link.perception.graph import (
|
||||
GraphRunResult,
|
||||
GraphState,
|
||||
ReferencePerceptionGraphV1,
|
||||
TerminalOutcomeType,
|
||||
)
|
||||
from k1link.perception.providers import (
|
||||
GraphAuthority,
|
||||
ProviderPin,
|
||||
ProviderRole,
|
||||
QueuePolicy,
|
||||
ReferencePerceptionGraphConfig,
|
||||
SourcePacket,
|
||||
)
|
||||
from k1link.perception.recorded_source import (
|
||||
RecordedRavnoves00Source,
|
||||
RecordedSourceError,
|
||||
ReplayPacing,
|
||||
)
|
||||
|
||||
|
||||
def _status(available: bool = True) -> ModalityStatus:
|
||||
return ModalityStatus(
|
||||
available=available,
|
||||
outcome=ModalityOutcome.AVAILABLE if available else ModalityOutcome.UNAVAILABLE,
|
||||
reason="test-available" if available else "test-unavailable",
|
||||
)
|
||||
|
||||
|
||||
def _packet(
|
||||
sequence: int,
|
||||
*,
|
||||
image: bool = True,
|
||||
lidar: bool = True,
|
||||
source_age_ns: int = 0,
|
||||
) -> SourcePacket:
|
||||
envelope = SourceEnvelope(
|
||||
source_id="RAVNOVES00",
|
||||
session_id="20260720T065719Z_viewer_live",
|
||||
frame_id=f"frame-{sequence:06d}",
|
||||
sequence=sequence,
|
||||
timestamps=TimestampBundle(
|
||||
utc_ns=1_786_000_000_000_000_000 + sequence,
|
||||
monotonic_ns=1_000 + sequence,
|
||||
source_ns=35_000_000_000 + sequence * 100_000_000,
|
||||
clock_basis=ClockBasis.RECORDED_HOST,
|
||||
),
|
||||
source_age_ns=source_age_ns,
|
||||
binding_reason="test-recorded-source",
|
||||
calibration_id="camera-1-kb4-test",
|
||||
representation_id="registered-map-increment-v1",
|
||||
image=_status(image),
|
||||
registered_point_increment=_status(lidar),
|
||||
pose=_status(lidar),
|
||||
)
|
||||
return SourcePacket(
|
||||
envelope=envelope,
|
||||
image_payload=("image", sequence) if image else None,
|
||||
registered_point_increment_payload=("points", sequence) if lidar else None,
|
||||
pose_payload=("pose", sequence) if lidar else None,
|
||||
)
|
||||
|
||||
|
||||
class _Source:
|
||||
provider_id = "test-source/v1"
|
||||
|
||||
def __init__(self, packets: tuple[SourcePacket, ...]) -> None:
|
||||
self.values = packets
|
||||
|
||||
def packets(self, stop_event: Event) -> Iterator[SourcePacket]:
|
||||
for packet in self.values:
|
||||
if stop_event.is_set():
|
||||
return
|
||||
yield packet
|
||||
|
||||
|
||||
class _Detector:
|
||||
provider_id = "test-detector/v1"
|
||||
|
||||
def detect(self, packet: SourcePacket) -> tuple[ObjectProposal2D, ...]:
|
||||
sequence = packet.envelope.sequence
|
||||
return (
|
||||
ObjectProposal2D(
|
||||
proposal_id=f"proposal-{sequence}",
|
||||
source_id=packet.envelope.source_id,
|
||||
frame_id=packet.envelope.frame_id,
|
||||
region=BoundingRegion2D(1.0, 2.0, 20.0, 30.0),
|
||||
objectness=0.9,
|
||||
provider_id=self.provider_id,
|
||||
model_id="objectness-test/v1",
|
||||
preprocess_id="kb4-test/v1",
|
||||
),
|
||||
)
|
||||
|
||||
|
||||
class _Geometry:
|
||||
provider_id = "test-geometry/v1"
|
||||
|
||||
def associate(
|
||||
self,
|
||||
packet: SourcePacket,
|
||||
proposals: tuple[ObjectProposal2D, ...],
|
||||
) -> tuple[ObstacleObservation, ...]:
|
||||
sequence = packet.envelope.sequence
|
||||
if not packet.envelope.registered_point_increment.available:
|
||||
return (
|
||||
ObstacleObservation(
|
||||
observation_id=f"camera-observation-{sequence}",
|
||||
occupancy_key=f"camera-uncertainty-{sequence}",
|
||||
source_id=packet.envelope.source_id,
|
||||
frame_id=packet.envelope.frame_id,
|
||||
evidence_time_ns=packet.envelope.timestamps.source_ns,
|
||||
basis=EvidenceBasis.CAMERA,
|
||||
currentness=EvidenceCurrentness.CURRENT,
|
||||
occupied_support=False,
|
||||
source_point_ids=(),
|
||||
metric_geometry=None,
|
||||
proposal_ids=(proposals[0].proposal_id,),
|
||||
semantic_hint=None,
|
||||
reason_codes=("camera-only",),
|
||||
),
|
||||
)
|
||||
return (
|
||||
ObstacleObservation(
|
||||
observation_id=f"observation-{sequence}",
|
||||
occupancy_key=f"occupancy-{sequence}",
|
||||
source_id=packet.envelope.source_id,
|
||||
frame_id=packet.envelope.frame_id,
|
||||
evidence_time_ns=packet.envelope.timestamps.source_ns,
|
||||
basis=EvidenceBasis.FUSED,
|
||||
currentness=EvidenceCurrentness.CURRENT,
|
||||
occupied_support=True,
|
||||
source_point_ids=(sequence,),
|
||||
metric_geometry=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),
|
||||
),
|
||||
proposal_ids=(proposals[0].proposal_id,),
|
||||
semantic_hint=None,
|
||||
reason_codes=("current-qualified-points",),
|
||||
),
|
||||
)
|
||||
|
||||
|
||||
class _Temporal:
|
||||
provider_id = "test-temporal/v1"
|
||||
|
||||
def update(
|
||||
self,
|
||||
packet: SourcePacket,
|
||||
observations: tuple[ObstacleObservation, ...],
|
||||
) -> tuple[TemporalObstacle, ...]:
|
||||
if not packet.envelope.registered_point_increment.available:
|
||||
return ()
|
||||
sequence = packet.envelope.sequence
|
||||
return (
|
||||
TemporalObstacle(
|
||||
component_id=f"component-{sequence}",
|
||||
identity_scope="ephemeral",
|
||||
state=TemporalState.CURRENT,
|
||||
ttl_ns=750_000_000,
|
||||
last_hit_ns=packet.envelope.timestamps.source_ns,
|
||||
age_ns=0,
|
||||
association_basis="current-spatial-support",
|
||||
history=(
|
||||
HistorySample(
|
||||
frame_id=packet.envelope.frame_id,
|
||||
evidence_time_ns=packet.envelope.timestamps.source_ns,
|
||||
centroid_xyz_m=(4.0, 1.0, 0.5),
|
||||
),
|
||||
),
|
||||
cells=(GridCell(sequence, 0, 0),),
|
||||
coordinate_frame="map",
|
||||
last_centroid_xyz_m=(4.0, 1.0, 0.5),
|
||||
motion=MotionState.UNKNOWN,
|
||||
motion_confidence=0.0,
|
||||
motion_reason="insufficient-history",
|
||||
),
|
||||
)
|
||||
|
||||
|
||||
class _Motion:
|
||||
provider_id = "test-motion/v1"
|
||||
|
||||
def estimate(
|
||||
self,
|
||||
packet: SourcePacket,
|
||||
obstacles: tuple[TemporalObstacle, ...],
|
||||
) -> tuple[TemporalObstacle, ...]:
|
||||
return obstacles
|
||||
|
||||
|
||||
class _Threat:
|
||||
provider_id = "test-threat/v1"
|
||||
|
||||
def assess(self, obstacle_map: LocalObstacleMap) -> tuple[ThreatAssessment, ...]:
|
||||
occupied = obstacle_map.occupied
|
||||
return tuple(
|
||||
ThreatAssessment(
|
||||
assessment_id=f"assessment-{item.component_id}",
|
||||
component_id=item.component_id,
|
||||
rig_profile_id="ravnoves00-virtual-rig/v1",
|
||||
corridor_profile_id="ravnoves00-virtual-corridor/v1",
|
||||
qualification=QualificationState.QUALIFIED,
|
||||
relative_speed_mps=0.0,
|
||||
closest_approach_m=4.0,
|
||||
ttc_seconds=None,
|
||||
corridor_intersection=CorridorIntersection.CLEAR,
|
||||
decision=ThreatDecision.NOT_THREAT,
|
||||
reason_codes=("qualified-corridor-clear",),
|
||||
)
|
||||
for item in occupied
|
||||
)
|
||||
|
||||
|
||||
def _config(
|
||||
capacity: int = 8,
|
||||
terminal_timeout_ns: int = 500_000_000,
|
||||
) -> ReferencePerceptionGraphConfig:
|
||||
ids = {
|
||||
ProviderRole.SOURCE: _Source.provider_id,
|
||||
ProviderRole.DETECTOR: _Detector.provider_id,
|
||||
ProviderRole.GEOMETRY: _Geometry.provider_id,
|
||||
ProviderRole.TEMPORAL: _Temporal.provider_id,
|
||||
ProviderRole.MOTION: _Motion.provider_id,
|
||||
ProviderRole.THREAT: _Threat.provider_id,
|
||||
}
|
||||
return ReferencePerceptionGraphConfig(
|
||||
graph_id="reference-perception-graph/v1",
|
||||
source_profile_id=BASELINE_PROFILE_ID,
|
||||
providers=tuple(
|
||||
ProviderPin(role, provider_id, "v1", "test-revision", "a" * 64)
|
||||
for role, provider_id in ids.items()
|
||||
),
|
||||
queues=tuple(
|
||||
QueuePolicy(stage, capacity, 80_000_000, terminal_timeout_ns)
|
||||
for stage in ("detector", "geometry", "temporal", "threat")
|
||||
),
|
||||
authority=GraphAuthority(),
|
||||
)
|
||||
|
||||
|
||||
class _MemoryTelemetry:
|
||||
def __init__(self) -> None:
|
||||
self.records: list[dict[str, object]] = []
|
||||
self._lock = threading.Lock()
|
||||
|
||||
def publish(self, topic: str, payload: bytes) -> None:
|
||||
with self._lock:
|
||||
self.records.append({"topic": topic, "payload": json.loads(payload)})
|
||||
|
||||
|
||||
def _graph(
|
||||
source: object,
|
||||
*,
|
||||
detector: object | None = None,
|
||||
capacity: int = 8,
|
||||
terminal_timeout_ns: int = 500_000_000,
|
||||
telemetry: _MemoryTelemetry | None = None,
|
||||
) -> ReferencePerceptionGraphV1:
|
||||
identity = (
|
||||
PipelineTelemetryIdentity(
|
||||
contour_id="worker-006",
|
||||
agent_id="mission-core-worker",
|
||||
node_id="DESKTOP-OPJ8J04",
|
||||
lab_id="M4",
|
||||
run_id="m4-unit-run",
|
||||
source_id="RAVNOVES00",
|
||||
source_package_id="ravnoves00-recorded-source/v1",
|
||||
method_id="reference-perception-graph/v1",
|
||||
)
|
||||
if telemetry is not None
|
||||
else None
|
||||
)
|
||||
return ReferencePerceptionGraphV1(
|
||||
config=_config(capacity, terminal_timeout_ns),
|
||||
source=source,
|
||||
detector=detector or _Detector(),
|
||||
geometry=_Geometry(),
|
||||
temporal=_Temporal(),
|
||||
motion=_Motion(),
|
||||
threat=_Threat(),
|
||||
telemetry_identity=identity,
|
||||
telemetry_sink=telemetry,
|
||||
clock_ns=lambda: 10_000,
|
||||
)
|
||||
|
||||
|
||||
def test_reference_graph_closes_accounting_telemetry_and_deterministic_digest() -> None:
|
||||
telemetry = _MemoryTelemetry()
|
||||
graph = _graph(_Source((_packet(0), _packet(1))), telemetry=telemetry)
|
||||
|
||||
first = graph.run()
|
||||
second = graph.run()
|
||||
|
||||
assert first.state is GraphState.STOPPED
|
||||
assert first.admitted_count == 2
|
||||
assert [item.outcome for item in first.terminal_outcomes] == [
|
||||
TerminalOutcomeType.DELIVERED,
|
||||
TerminalOutcomeType.DELIVERED,
|
||||
]
|
||||
assert all(
|
||||
delivery.obstacle_map.accounting.source_envelopes == 1
|
||||
for delivery in first.deliveries
|
||||
)
|
||||
assert all(delivery.obstacle_map.free_space_claimed is False for delivery in first.deliveries)
|
||||
assert first.canonical_payload_sha256 == second.canonical_payload_sha256
|
||||
assert graph.worker_threads_alive == 0
|
||||
stage_ids = {
|
||||
record["payload"].get("stage_id")
|
||||
for record in telemetry.records
|
||||
if isinstance(record["payload"], dict)
|
||||
}
|
||||
assert {"detector", "geometry", "temporal", "threat"}.issubset(stage_ids)
|
||||
|
||||
|
||||
def test_reference_graph_marks_unavailable_stale_and_provider_failure() -> None:
|
||||
class FailingDetector(_Detector):
|
||||
def detect(self, packet: SourcePacket) -> tuple[ObjectProposal2D, ...]:
|
||||
if packet.envelope.sequence == 2:
|
||||
raise RuntimeError("deliberate provider failure")
|
||||
return super().detect(packet)
|
||||
|
||||
FailingDetector.provider_id = _Detector.provider_id
|
||||
packets = (
|
||||
_packet(0, image=False),
|
||||
_packet(1, source_age_ns=80_000_001),
|
||||
_packet(2),
|
||||
_packet(3),
|
||||
)
|
||||
result = _graph(_Source(packets), detector=FailingDetector()).run()
|
||||
|
||||
assert [item.outcome for item in result.terminal_outcomes] == [
|
||||
TerminalOutcomeType.UNAVAILABLE,
|
||||
TerminalOutcomeType.STALE,
|
||||
TerminalOutcomeType.FAILED,
|
||||
TerminalOutcomeType.DELIVERED,
|
||||
]
|
||||
assert len(result.deliveries) == 1
|
||||
assert result.deliveries[0].sequence == 3
|
||||
|
||||
|
||||
def test_bounded_detector_queue_has_explicit_latest_wins_supersession() -> None:
|
||||
detector_started = Event()
|
||||
source_emitted = Event()
|
||||
release_detector = Event()
|
||||
|
||||
class BlockingDetector(_Detector):
|
||||
def detect(self, packet: SourcePacket) -> tuple[ObjectProposal2D, ...]:
|
||||
if packet.envelope.sequence == 0:
|
||||
detector_started.set()
|
||||
assert release_detector.wait(2)
|
||||
return super().detect(packet)
|
||||
|
||||
BlockingDetector.provider_id = _Detector.provider_id
|
||||
|
||||
class CoordinatedSource(_Source):
|
||||
def packets(self, stop_event: Event) -> Iterator[SourcePacket]:
|
||||
yield self.values[0]
|
||||
assert detector_started.wait(2)
|
||||
yield from self.values[1:]
|
||||
source_emitted.set()
|
||||
|
||||
source = CoordinatedSource(tuple(_packet(index) for index in range(4)))
|
||||
graph = _graph(
|
||||
source,
|
||||
detector=BlockingDetector(),
|
||||
capacity=1,
|
||||
terminal_timeout_ns=1_000_000_000,
|
||||
)
|
||||
holder: list[GraphRunResult] = []
|
||||
runner = threading.Thread(target=lambda: holder.append(graph.run()))
|
||||
runner.start()
|
||||
assert source_emitted.wait(2)
|
||||
release_detector.set()
|
||||
runner.join(3)
|
||||
|
||||
assert not runner.is_alive()
|
||||
outcomes = {item.sequence: item.outcome for item in holder[0].terminal_outcomes}
|
||||
assert outcomes[1] is TerminalOutcomeType.SUPERSEDED
|
||||
assert outcomes[2] is TerminalOutcomeType.SUPERSEDED
|
||||
assert outcomes[3] is TerminalOutcomeType.DELIVERED
|
||||
assert set(outcomes) == {0, 1, 2, 3}
|
||||
assert graph.worker_threads_alive == 0
|
||||
|
||||
|
||||
def test_cancelled_graph_closes_threads_and_can_restart() -> None:
|
||||
source_waiting = Event()
|
||||
|
||||
class RestartableSource(_Source):
|
||||
block = True
|
||||
|
||||
def packets(self, stop_event: Event) -> Iterator[SourcePacket]:
|
||||
yield self.values[0]
|
||||
if self.block:
|
||||
source_waiting.set()
|
||||
stop_event.wait(2)
|
||||
|
||||
source = RestartableSource((_packet(0),))
|
||||
graph = _graph(source)
|
||||
holder: list[GraphRunResult] = []
|
||||
runner = threading.Thread(target=lambda: holder.append(graph.run()))
|
||||
runner.start()
|
||||
assert source_waiting.wait(2)
|
||||
graph.cancel()
|
||||
runner.join(3)
|
||||
|
||||
assert holder[0].state is GraphState.CANCELLED
|
||||
assert graph.worker_threads_alive == 0
|
||||
source.block = False
|
||||
restarted = graph.run()
|
||||
assert restarted.state is GraphState.STOPPED
|
||||
assert restarted.terminal_outcomes[0].outcome is TerminalOutcomeType.DELIVERED
|
||||
assert graph.worker_threads_alive == 0
|
||||
|
||||
|
||||
def _write_recorded_fixture(root: Path, *, mismatched: bool = False) -> tuple[Path, Path]:
|
||||
camera_path = root / "camera.jsonl"
|
||||
timeline_path = root / "source-pack.npz"
|
||||
camera_rows = [
|
||||
{
|
||||
"schema_version": "missioncore.camera-recording-index/v1",
|
||||
"kind": "media",
|
||||
"sequence": index + 1,
|
||||
"host_epoch_ns": 100 + index,
|
||||
"host_monotonic_ns": 200 + index,
|
||||
"sha256": f"{index + 1:064x}",
|
||||
}
|
||||
for index in range(2)
|
||||
]
|
||||
camera_path.write_text("\n".join(json.dumps(row) for row in camera_rows) + "\n", "utf-8")
|
||||
np.savez(
|
||||
timeline_path,
|
||||
frame_indices=np.asarray([0, 2 if mismatched else 1], dtype=np.int64),
|
||||
source_frame_indices=np.asarray([0, 1], dtype=np.int64),
|
||||
session_seconds=np.asarray([10.0, 10.1], dtype=np.float64),
|
||||
sample_available=np.asarray([True, False], dtype=np.bool_),
|
||||
)
|
||||
return camera_path, timeline_path
|
||||
|
||||
|
||||
def test_recorded_source_reuses_one_timeline_for_1x_and_uncapped(tmp_path: Path) -> None:
|
||||
camera_path, timeline_path = _write_recorded_fixture(tmp_path)
|
||||
now = [1_000_000_000]
|
||||
waits: list[float] = []
|
||||
|
||||
def wait(stop_event: Event, seconds: float) -> bool:
|
||||
waits.append(seconds)
|
||||
now[0] += round(seconds * 1_000_000_000)
|
||||
return stop_event.is_set()
|
||||
|
||||
source = RecordedRavnoves00Source(
|
||||
camera_index_path=camera_path,
|
||||
source_pack_path=timeline_path,
|
||||
pacing=ReplayPacing.ONE_X,
|
||||
expected_frame_count=2,
|
||||
expected_source_pack_sha256=None,
|
||||
clock_ns=lambda: now[0],
|
||||
wait=wait,
|
||||
)
|
||||
one_x = list(source.packets(Event()))
|
||||
uncapped = list(
|
||||
RecordedRavnoves00Source(
|
||||
camera_index_path=camera_path,
|
||||
source_pack_path=timeline_path,
|
||||
pacing=ReplayPacing.UNCAPPED,
|
||||
expected_frame_count=2,
|
||||
expected_source_pack_sha256=None,
|
||||
wait=lambda _event, _seconds: pytest.fail("uncapped replay waited"),
|
||||
).packets(Event())
|
||||
)
|
||||
|
||||
assert [packet.envelope.to_dict() for packet in one_x] == [
|
||||
packet.envelope.to_dict() for packet in uncapped
|
||||
]
|
||||
assert waits == pytest.approx([0.1])
|
||||
assert one_x[1].envelope.registered_point_increment.available is False
|
||||
assert one_x[1].registered_point_increment_payload is None
|
||||
|
||||
|
||||
def test_recorded_source_rejects_timeline_mismatch(tmp_path: Path) -> None:
|
||||
camera_path, timeline_path = _write_recorded_fixture(tmp_path, mismatched=True)
|
||||
source = RecordedRavnoves00Source(
|
||||
camera_index_path=camera_path,
|
||||
source_pack_path=timeline_path,
|
||||
expected_frame_count=2,
|
||||
expected_source_pack_sha256=None,
|
||||
)
|
||||
with pytest.raises(RecordedSourceError, match="timeline frame index"):
|
||||
list(source.packets(Event()))
|
||||
|
||||
|
||||
def test_camera_only_path_never_invents_metric_occupancy_or_free_space() -> None:
|
||||
result = _graph(_Source((_packet(0, lidar=False),))).run()
|
||||
delivery = result.deliveries[0]
|
||||
assert delivery.obstacle_map.occupied == ()
|
||||
assert delivery.obstacle_map.unknown == ()
|
||||
assert delivery.obstacle_map.free_space_claimed is False
|
||||
assert len(delivery.obstacle_map.camera_uncertainty) == 1
|
||||
|
||||
|
||||
def test_graph_config_rejects_unbounded_or_missing_stage_policy() -> None:
|
||||
config = _config()
|
||||
with pytest.raises(ValueError, match="each reference stage"):
|
||||
replace(config, queues=config.queues[:-1])
|
||||
Reference in New Issue
Block a user