569 lines
19 KiB
Python
569 lines
19 KiB
Python
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 (
|
|
DecodedRecordedSource,
|
|
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_decoded_recorded_source_attaches_images_without_detector_logic(tmp_path: Path) -> None:
|
|
camera_path, timeline_path = _write_recorded_fixture(tmp_path)
|
|
source = RecordedRavnoves00Source(
|
|
camera_index_path=camera_path,
|
|
source_pack_path=timeline_path,
|
|
expected_frame_count=2,
|
|
expected_source_pack_sha256=None,
|
|
)
|
|
|
|
class Decoder:
|
|
def frames(self, stop_event: Event) -> Iterator[np.ndarray]:
|
|
for value in (3, 7):
|
|
if stop_event.is_set():
|
|
return
|
|
yield np.full((600, 800, 3), value, dtype=np.uint8)
|
|
|
|
packets = list(DecodedRecordedSource(source=source, decoder=Decoder()).packets(Event()))
|
|
assert len(packets) == 2
|
|
assert isinstance(packets[0].image_payload, np.ndarray)
|
|
assert int(packets[0].image_payload[0, 0, 0]) == 3
|
|
assert int(packets[1].image_payload[0, 0, 0]) == 7
|
|
|
|
|
|
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])
|