feat(perception): integrate calibrated operator pipeline
Add calibrated K1 projection, recorded and near-live perception qualification, unified Rerun operator layers, bounded replay admission, audited viewer controls, worker experiments, and lab evidence.
This commit is contained in:
DCCONSTRUCTIONS
@@ -0,0 +1,991 @@
|
||||
"""Bounded latest-wins primitives for derived live perception."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import json
|
||||
import math
|
||||
import statistics
|
||||
import struct
|
||||
import threading
|
||||
import time
|
||||
import zlib
|
||||
from collections import deque
|
||||
from collections.abc import Mapping, Sequence
|
||||
from dataclasses import dataclass
|
||||
from hashlib import sha256
|
||||
from typing import Any, Final, Literal
|
||||
|
||||
import numpy as np
|
||||
|
||||
from k1link.data_plane import DecodedPointCloudView, DecodedPoseView
|
||||
|
||||
WORLD_STATE_SCHEMA = "missioncore.live-perception-world-state/v1"
|
||||
TELEMETRY_SCHEMA = "missioncore.live-perception-telemetry/v1"
|
||||
|
||||
HealthState = Literal["healthy", "degraded", "stale", "unavailable"]
|
||||
LiveIngressModality = Literal[
|
||||
"control",
|
||||
"camera-init",
|
||||
"camera-frame",
|
||||
"lidar",
|
||||
"pose",
|
||||
]
|
||||
|
||||
LIVE_INGRESS_SCHEMA: Final = "missioncore.live-perception-ingress/v1"
|
||||
LIVE_INGRESS_WIRE_SCHEMA: Final = "missioncore.live-perception-wire/v1"
|
||||
LIVE_RESULT_WIRE_SCHEMA: Final = "missioncore.live-perception-result-wire/v1"
|
||||
LIVE_RESULT_MAGIC: Final = b"MCPR"
|
||||
LIVE_RESULT_MAX_HEADER_BYTES: Final = 256 * 1024
|
||||
LIVE_RESULT_MAX_PAYLOAD_BYTES: Final = 2 * 1024 * 1024
|
||||
|
||||
|
||||
@dataclass(frozen=True, slots=True)
|
||||
class LivePerceptionResultFrame:
|
||||
frame_index: int
|
||||
source_frame_index: int
|
||||
session_seconds: float
|
||||
captured_at_epoch_ns: int
|
||||
image_jpeg: bytes
|
||||
segmentation_mask: np.ndarray[Any, np.dtype[np.uint8]] | None
|
||||
objects: tuple[dict[str, Any], ...]
|
||||
delivery: dict[str, Any]
|
||||
|
||||
|
||||
def encode_live_perception_result(
|
||||
*,
|
||||
frame_index: int,
|
||||
source_frame_index: int,
|
||||
session_seconds: float,
|
||||
captured_at_epoch_ns: int,
|
||||
image_jpeg: bytes,
|
||||
segmentation_mask: np.ndarray[Any, Any] | None,
|
||||
objects: Sequence[Mapping[str, Any]],
|
||||
delivery: Mapping[str, Any],
|
||||
) -> bytes:
|
||||
"""Encode one bounded, non-authoritative worker-to-viewer result frame."""
|
||||
|
||||
if (
|
||||
frame_index < 0
|
||||
or source_frame_index < 0
|
||||
or captured_at_epoch_ns < 0
|
||||
or not math.isfinite(session_seconds)
|
||||
or session_seconds < 0
|
||||
or not 4 <= len(image_jpeg) <= 1024 * 1024
|
||||
or not image_jpeg.startswith(b"\xff\xd8")
|
||||
or not image_jpeg.endswith(b"\xff\xd9")
|
||||
):
|
||||
raise ValueError("live perception result identity or image is invalid")
|
||||
normalized_objects = tuple(_normalize_live_result_object(value) for value in objects)
|
||||
if len(normalized_objects) > 128:
|
||||
raise ValueError("live perception result object count exceeds the bound")
|
||||
mask_payload = b""
|
||||
mask_shape: list[int] | None = None
|
||||
if segmentation_mask is not None:
|
||||
mask = np.asarray(segmentation_mask, dtype=np.uint8)
|
||||
if mask.shape != (600, 800):
|
||||
raise ValueError("live perception segmentation shape is invalid")
|
||||
mask_payload = zlib.compress(mask.tobytes(order="C"), level=1)
|
||||
mask_shape = [600, 800]
|
||||
payload = bytes(image_jpeg) + mask_payload
|
||||
if len(payload) > LIVE_RESULT_MAX_PAYLOAD_BYTES:
|
||||
raise ValueError("live perception result payload exceeds the bound")
|
||||
header = {
|
||||
"schema_version": LIVE_RESULT_WIRE_SCHEMA,
|
||||
"frame_index": frame_index,
|
||||
"source_frame_index": source_frame_index,
|
||||
"session_seconds": session_seconds,
|
||||
"captured_at_epoch_ns": captured_at_epoch_ns,
|
||||
"image": {"codec": "jpeg", "byte_length": len(image_jpeg)},
|
||||
"segmentation": (
|
||||
None
|
||||
if mask_shape is None
|
||||
else {
|
||||
"codec": "zlib-uint8-c1",
|
||||
"shape": mask_shape,
|
||||
"byte_length": len(mask_payload),
|
||||
}
|
||||
),
|
||||
"objects": normalized_objects,
|
||||
"delivery": dict(delivery),
|
||||
"payload_bytes": len(payload),
|
||||
"payload_sha256": sha256(payload).hexdigest(),
|
||||
"authority": "shadow-diagnostic-only",
|
||||
"commands_enabled": False,
|
||||
"navigation_or_safety_accepted": False,
|
||||
}
|
||||
encoded_header = json.dumps(
|
||||
header,
|
||||
sort_keys=True,
|
||||
separators=(",", ":"),
|
||||
allow_nan=False,
|
||||
).encode("utf-8")
|
||||
if len(encoded_header) > LIVE_RESULT_MAX_HEADER_BYTES:
|
||||
raise ValueError("live perception result header exceeds the bound")
|
||||
return LIVE_RESULT_MAGIC + struct.pack("!I", len(encoded_header)) + encoded_header + payload
|
||||
|
||||
|
||||
def decode_live_perception_result(encoded: bytes) -> LivePerceptionResultFrame:
|
||||
"""Validate and decode one result frame before it reaches the Rerun bridge."""
|
||||
|
||||
if len(encoded) < 10 or not encoded.startswith(LIVE_RESULT_MAGIC):
|
||||
raise ValueError("live perception result frame is truncated")
|
||||
header_length = struct.unpack("!I", encoded[4:8])[0]
|
||||
if not 2 <= header_length <= LIVE_RESULT_MAX_HEADER_BYTES:
|
||||
raise ValueError("live perception result header length is invalid")
|
||||
boundary = 8 + header_length
|
||||
if boundary > len(encoded):
|
||||
raise ValueError("live perception result header is truncated")
|
||||
try:
|
||||
header = json.loads(encoded[8:boundary])
|
||||
except (UnicodeDecodeError, json.JSONDecodeError) as exc:
|
||||
raise ValueError("live perception result header is invalid") from exc
|
||||
payload = encoded[boundary:]
|
||||
if (
|
||||
not isinstance(header, dict)
|
||||
or header.get("schema_version") != LIVE_RESULT_WIRE_SCHEMA
|
||||
or header.get("authority") != "shadow-diagnostic-only"
|
||||
or header.get("commands_enabled") is not False
|
||||
or header.get("navigation_or_safety_accepted") is not False
|
||||
or header.get("payload_bytes") != len(payload)
|
||||
or len(payload) > LIVE_RESULT_MAX_PAYLOAD_BYTES
|
||||
or header.get("payload_sha256") != sha256(payload).hexdigest()
|
||||
):
|
||||
raise ValueError("live perception result contract is invalid")
|
||||
image = header.get("image")
|
||||
segmentation = header.get("segmentation")
|
||||
objects = header.get("objects")
|
||||
delivery = header.get("delivery")
|
||||
if (
|
||||
not isinstance(image, dict)
|
||||
or image.get("codec") != "jpeg"
|
||||
or not isinstance(image.get("byte_length"), int)
|
||||
or not isinstance(objects, list)
|
||||
or len(objects) > 128
|
||||
or not isinstance(delivery, dict)
|
||||
):
|
||||
raise ValueError("live perception result content descriptor is invalid")
|
||||
image_length = image["byte_length"]
|
||||
if not 4 <= image_length <= min(len(payload), 1024 * 1024):
|
||||
raise ValueError("live perception result image length is invalid")
|
||||
image_jpeg = payload[:image_length]
|
||||
if not image_jpeg.startswith(b"\xff\xd8") or not image_jpeg.endswith(b"\xff\xd9"):
|
||||
raise ValueError("live perception result JPEG is invalid")
|
||||
mask: np.ndarray[Any, np.dtype[np.uint8]] | None = None
|
||||
if segmentation is None:
|
||||
if len(payload) != image_length:
|
||||
raise ValueError("live perception result has an undescribed payload tail")
|
||||
else:
|
||||
if (
|
||||
not isinstance(segmentation, dict)
|
||||
or segmentation.get("codec") != "zlib-uint8-c1"
|
||||
or segmentation.get("shape") != [600, 800]
|
||||
or not isinstance(segmentation.get("byte_length"), int)
|
||||
or segmentation["byte_length"] != len(payload) - image_length
|
||||
):
|
||||
raise ValueError("live perception segmentation descriptor is invalid")
|
||||
try:
|
||||
raw_mask = zlib.decompress(payload[image_length:])
|
||||
except zlib.error as exc:
|
||||
raise ValueError("live perception segmentation payload is invalid") from exc
|
||||
if len(raw_mask) != 600 * 800:
|
||||
raise ValueError("live perception segmentation byte length is invalid")
|
||||
mask = np.frombuffer(raw_mask, dtype=np.uint8).reshape((600, 800)).copy()
|
||||
normalized_objects = tuple(_normalize_live_result_object(value) for value in objects)
|
||||
frame_index = header.get("frame_index")
|
||||
source_frame_index = header.get("source_frame_index")
|
||||
session_seconds = header.get("session_seconds")
|
||||
captured_at_epoch_ns = header.get("captured_at_epoch_ns")
|
||||
if (
|
||||
not isinstance(frame_index, int)
|
||||
or isinstance(frame_index, bool)
|
||||
or frame_index < 0
|
||||
or not isinstance(source_frame_index, int)
|
||||
or isinstance(source_frame_index, bool)
|
||||
or source_frame_index < 0
|
||||
or not isinstance(captured_at_epoch_ns, int)
|
||||
or isinstance(captured_at_epoch_ns, bool)
|
||||
or captured_at_epoch_ns < 0
|
||||
or not isinstance(session_seconds, (int, float))
|
||||
or isinstance(session_seconds, bool)
|
||||
or not math.isfinite(float(session_seconds))
|
||||
or float(session_seconds) < 0
|
||||
):
|
||||
raise ValueError("live perception result time identity is invalid")
|
||||
return LivePerceptionResultFrame(
|
||||
frame_index=frame_index,
|
||||
source_frame_index=source_frame_index,
|
||||
session_seconds=float(session_seconds),
|
||||
captured_at_epoch_ns=captured_at_epoch_ns,
|
||||
image_jpeg=image_jpeg,
|
||||
segmentation_mask=mask,
|
||||
objects=normalized_objects,
|
||||
delivery=dict(delivery),
|
||||
)
|
||||
|
||||
|
||||
def _normalize_live_result_object(value: Mapping[str, Any]) -> dict[str, Any]:
|
||||
if not isinstance(value, Mapping):
|
||||
raise ValueError("live perception result object is invalid")
|
||||
track_id = value.get("track_id")
|
||||
label = value.get("label")
|
||||
score = value.get("score")
|
||||
bbox = value.get("bbox_xyxy")
|
||||
if (
|
||||
not isinstance(track_id, int)
|
||||
or isinstance(track_id, bool)
|
||||
or track_id < 0
|
||||
or not isinstance(label, str)
|
||||
or not 1 <= len(label) <= 64
|
||||
or not isinstance(score, (int, float))
|
||||
or isinstance(score, bool)
|
||||
or not math.isfinite(float(score))
|
||||
or not isinstance(bbox, Sequence)
|
||||
or isinstance(bbox, (str, bytes))
|
||||
or len(bbox) != 4
|
||||
):
|
||||
raise ValueError("live perception result object identity is invalid")
|
||||
bbox_values = [float(item) for item in bbox]
|
||||
if not all(math.isfinite(item) for item in bbox_values):
|
||||
raise ValueError("live perception result 2D box is invalid")
|
||||
normalized: dict[str, Any] = {
|
||||
"track_id": track_id,
|
||||
"label": label,
|
||||
"score": float(score),
|
||||
"bbox_xyxy": bbox_values,
|
||||
}
|
||||
distance = value.get("distance_smoothed_m", value.get("distance_median_m"))
|
||||
if distance is not None:
|
||||
if (
|
||||
not isinstance(distance, (int, float))
|
||||
or isinstance(distance, bool)
|
||||
or not math.isfinite(float(distance))
|
||||
or float(distance) < 0
|
||||
):
|
||||
raise ValueError("live perception result distance is invalid")
|
||||
normalized["distance_m"] = float(distance)
|
||||
else:
|
||||
normalized["distance_m"] = None
|
||||
cuboid_fields = (
|
||||
("cuboid_center_map", 3),
|
||||
("cuboid_half_size", 3),
|
||||
("cuboid_quaternion_xyzw", 4),
|
||||
)
|
||||
present = [value.get(name) is not None for name, _ in cuboid_fields]
|
||||
if any(present) and not all(present):
|
||||
raise ValueError("live perception result cuboid is incomplete")
|
||||
for name, length in cuboid_fields:
|
||||
candidate = value.get(name)
|
||||
if candidate is None:
|
||||
normalized[name] = None
|
||||
continue
|
||||
if (
|
||||
not isinstance(candidate, Sequence)
|
||||
or isinstance(candidate, (str, bytes))
|
||||
or len(candidate) != length
|
||||
):
|
||||
raise ValueError("live perception result cuboid geometry is invalid")
|
||||
values = [float(item) for item in candidate]
|
||||
if not all(math.isfinite(item) for item in values):
|
||||
raise ValueError("live perception result cuboid contains non-finite values")
|
||||
normalized[name] = values
|
||||
return normalized
|
||||
|
||||
|
||||
@dataclass(frozen=True, slots=True)
|
||||
class LiveIngressEvent:
|
||||
"""One raw-first, derived-only event admitted to the shadow transport."""
|
||||
|
||||
ingress_sequence: int
|
||||
session_id: str
|
||||
modality: LiveIngressModality
|
||||
source_id: str
|
||||
source_sequence: int
|
||||
captured_at_epoch_ns: int
|
||||
received_monotonic_ns: int
|
||||
payload: bytes
|
||||
|
||||
def wire_bytes(self) -> bytes:
|
||||
header = json.dumps(
|
||||
{
|
||||
"schema_version": LIVE_INGRESS_WIRE_SCHEMA,
|
||||
"ingress_sequence": self.ingress_sequence,
|
||||
"session_id": self.session_id,
|
||||
"modality": self.modality,
|
||||
"source_id": self.source_id,
|
||||
"source_sequence": self.source_sequence,
|
||||
"captured_at_epoch_ns": self.captured_at_epoch_ns,
|
||||
"received_monotonic_ns": self.received_monotonic_ns,
|
||||
"payload_bytes": len(self.payload),
|
||||
"payload_sha256": sha256(self.payload).hexdigest(),
|
||||
"authority": "shadow-diagnostic-only",
|
||||
"commands_enabled": False,
|
||||
"navigation_or_safety_accepted": False,
|
||||
},
|
||||
ensure_ascii=True,
|
||||
separators=(",", ":"),
|
||||
sort_keys=True,
|
||||
).encode("utf-8")
|
||||
return struct.pack("!I", len(header)) + header + self.payload
|
||||
|
||||
|
||||
@dataclass(frozen=True, slots=True)
|
||||
class LiveIngressQueueSnapshot:
|
||||
capacity: int
|
||||
depth: int
|
||||
maximum_depth: int
|
||||
published: int
|
||||
consumed: int
|
||||
dropped_overflow: int
|
||||
rejected_oversize: int
|
||||
|
||||
|
||||
@dataclass(slots=True)
|
||||
class _LiveIngressQueue:
|
||||
capacity: int
|
||||
items: deque[LiveIngressEvent]
|
||||
maximum_depth: int = 0
|
||||
published: int = 0
|
||||
consumed: int = 0
|
||||
dropped_overflow: int = 0
|
||||
rejected_oversize: int = 0
|
||||
|
||||
|
||||
class LivePerceptionIngress:
|
||||
"""Exclusive, bounded fan-out from committed K1 evidence to one AI worker.
|
||||
|
||||
The ingress is deliberately not an acquisition source and has no command
|
||||
surface. Camera and MQTT producers call it only after their raw evidence
|
||||
commit has completed. Separate modality queues prevent camera bursts from
|
||||
evicting pose or LiDAR observations.
|
||||
"""
|
||||
|
||||
_CAPACITIES: Final[dict[LiveIngressModality, int]] = {
|
||||
"control": 4,
|
||||
"camera-init": 1,
|
||||
"camera-frame": 2,
|
||||
"lidar": 2,
|
||||
"pose": 16,
|
||||
}
|
||||
_MAX_PAYLOAD_BYTES: Final[dict[LiveIngressModality, int]] = {
|
||||
"control": 16 * 1024,
|
||||
"camera-init": 1024 * 1024,
|
||||
"camera-frame": 1024 * 1024,
|
||||
"lidar": 2 * 1024 * 1024,
|
||||
"pose": 2 * 1024 * 1024,
|
||||
}
|
||||
|
||||
def __init__(self) -> None:
|
||||
self._condition = threading.Condition()
|
||||
self._queues = {
|
||||
modality: _LiveIngressQueue(capacity, deque())
|
||||
for modality, capacity in self._CAPACITIES.items()
|
||||
}
|
||||
self._ingress_sequence = 0
|
||||
self._session_id: str | None = None
|
||||
self._active = False
|
||||
self._closed = False
|
||||
self._consumer_id: str | None = None
|
||||
|
||||
def begin_session(self, session_id: str) -> None:
|
||||
if not session_id or len(session_id) > 160:
|
||||
raise ValueError("live perception session id is invalid")
|
||||
with self._condition:
|
||||
if self._closed:
|
||||
raise RuntimeError("live perception ingress is closed")
|
||||
if self._active:
|
||||
if self._session_id == session_id:
|
||||
return
|
||||
raise RuntimeError("another live perception session is active")
|
||||
self._session_id = session_id
|
||||
self._active = True
|
||||
self._publish_locked(
|
||||
modality="control",
|
||||
source_id="mission-core",
|
||||
source_sequence=0,
|
||||
captured_at_epoch_ns=time.time_ns(),
|
||||
received_monotonic_ns=time.monotonic_ns(),
|
||||
payload=b'{"event":"session-start"}',
|
||||
)
|
||||
|
||||
def end_session(self, session_id: str) -> None:
|
||||
with self._condition:
|
||||
if not self._active or self._session_id != session_id:
|
||||
return
|
||||
self._publish_locked(
|
||||
modality="control",
|
||||
source_id="mission-core",
|
||||
source_sequence=0,
|
||||
captured_at_epoch_ns=time.time_ns(),
|
||||
received_monotonic_ns=time.monotonic_ns(),
|
||||
payload=b'{"event":"session-end"}',
|
||||
)
|
||||
self._active = False
|
||||
|
||||
def publish(
|
||||
self,
|
||||
*,
|
||||
modality: LiveIngressModality,
|
||||
source_id: str,
|
||||
source_sequence: int,
|
||||
captured_at_epoch_ns: int,
|
||||
received_monotonic_ns: int,
|
||||
payload: bytes,
|
||||
) -> bool:
|
||||
if modality == "control":
|
||||
raise ValueError("control events are owned by the ingress lifecycle")
|
||||
if not source_id or source_sequence < 0:
|
||||
raise ValueError("live perception source identity is invalid")
|
||||
if captured_at_epoch_ns < 0 or received_monotonic_ns < 0:
|
||||
raise ValueError("live perception timestamps must be non-negative")
|
||||
with self._condition:
|
||||
if self._closed or not self._active:
|
||||
return False
|
||||
return self._publish_locked(
|
||||
modality=modality,
|
||||
source_id=source_id,
|
||||
source_sequence=source_sequence,
|
||||
captured_at_epoch_ns=captured_at_epoch_ns,
|
||||
received_monotonic_ns=received_monotonic_ns,
|
||||
payload=payload,
|
||||
)
|
||||
|
||||
def open_consumer(self, consumer_id: str) -> None:
|
||||
if not consumer_id or len(consumer_id) > 128:
|
||||
raise ValueError("live perception consumer id is invalid")
|
||||
with self._condition:
|
||||
if self._closed:
|
||||
raise RuntimeError("live perception ingress is closed")
|
||||
if self._consumer_id is not None and self._consumer_id != consumer_id:
|
||||
raise RuntimeError("live perception ingress already has a consumer")
|
||||
self._consumer_id = consumer_id
|
||||
|
||||
def close_consumer(self, consumer_id: str) -> None:
|
||||
with self._condition:
|
||||
if self._consumer_id == consumer_id:
|
||||
self._consumer_id = None
|
||||
self._condition.notify_all()
|
||||
|
||||
def take_next(
|
||||
self,
|
||||
consumer_id: str,
|
||||
*,
|
||||
timeout: float | None = None,
|
||||
) -> LiveIngressEvent | None:
|
||||
with self._condition:
|
||||
if self._consumer_id != consumer_id:
|
||||
raise RuntimeError("live perception consumer lease is not active")
|
||||
ready = self._condition.wait_for(
|
||||
lambda: any(queue.items for queue in self._queues.values()) or self._closed,
|
||||
timeout=timeout,
|
||||
)
|
||||
if not ready:
|
||||
return None
|
||||
candidates = [
|
||||
(queue.items[0].ingress_sequence, modality, queue)
|
||||
for modality, queue in self._queues.items()
|
||||
if queue.items
|
||||
]
|
||||
if not candidates:
|
||||
return None
|
||||
_, _, selected = min(candidates, key=lambda item: item[0])
|
||||
selected.consumed += 1
|
||||
return selected.items.popleft()
|
||||
|
||||
def close(self) -> None:
|
||||
with self._condition:
|
||||
self._closed = True
|
||||
self._active = False
|
||||
self._condition.notify_all()
|
||||
|
||||
def snapshot(self) -> dict[str, Any]:
|
||||
with self._condition:
|
||||
return {
|
||||
"schema_version": LIVE_INGRESS_SCHEMA,
|
||||
"mode": "shadow-diagnostic-only",
|
||||
"active": self._active,
|
||||
"session_id": self._session_id,
|
||||
"consumer_connected": self._consumer_id is not None,
|
||||
"commands_enabled": False,
|
||||
"navigation_or_safety_accepted": False,
|
||||
"closed": self._closed,
|
||||
"queues": {
|
||||
modality: {
|
||||
"capacity": queue.capacity,
|
||||
"depth": len(queue.items),
|
||||
"maximum_depth": queue.maximum_depth,
|
||||
"published": queue.published,
|
||||
"consumed": queue.consumed,
|
||||
"dropped_overflow": queue.dropped_overflow,
|
||||
"rejected_oversize": queue.rejected_oversize,
|
||||
}
|
||||
for modality, queue in self._queues.items()
|
||||
},
|
||||
}
|
||||
|
||||
def _publish_locked(
|
||||
self,
|
||||
*,
|
||||
modality: LiveIngressModality,
|
||||
source_id: str,
|
||||
source_sequence: int,
|
||||
captured_at_epoch_ns: int,
|
||||
received_monotonic_ns: int,
|
||||
payload: bytes,
|
||||
) -> bool:
|
||||
queue = self._queues[modality]
|
||||
if len(payload) > self._MAX_PAYLOAD_BYTES[modality]:
|
||||
queue.rejected_oversize += 1
|
||||
return False
|
||||
session_id = self._session_id
|
||||
if session_id is None:
|
||||
return False
|
||||
self._ingress_sequence += 1
|
||||
event = LiveIngressEvent(
|
||||
ingress_sequence=self._ingress_sequence,
|
||||
session_id=session_id,
|
||||
modality=modality,
|
||||
source_id=source_id,
|
||||
source_sequence=source_sequence,
|
||||
captured_at_epoch_ns=captured_at_epoch_ns,
|
||||
received_monotonic_ns=received_monotonic_ns,
|
||||
payload=bytes(payload),
|
||||
)
|
||||
if len(queue.items) == queue.capacity:
|
||||
queue.items.popleft()
|
||||
queue.dropped_overflow += 1
|
||||
queue.items.append(event)
|
||||
queue.published += 1
|
||||
queue.maximum_depth = max(queue.maximum_depth, len(queue.items))
|
||||
self._condition.notify_all()
|
||||
return True
|
||||
|
||||
|
||||
@dataclass(frozen=True, slots=True)
|
||||
class QueueSnapshot:
|
||||
capacity: int
|
||||
depth: int
|
||||
maximum_depth: int
|
||||
published: int
|
||||
consumed: int
|
||||
dropped_overflow: int
|
||||
dropped_superseded: int
|
||||
closed: bool
|
||||
|
||||
@property
|
||||
def dropped_total(self) -> int:
|
||||
return self.dropped_overflow + self.dropped_superseded
|
||||
|
||||
|
||||
class LatestWinsQueue[T]:
|
||||
"""A bounded derived-data queue that never lets old preview work accumulate."""
|
||||
|
||||
def __init__(self, capacity: int) -> None:
|
||||
if capacity < 1:
|
||||
raise ValueError("latest-wins queue capacity must be positive")
|
||||
self._capacity = capacity
|
||||
self._items: deque[T] = deque()
|
||||
self._condition = threading.Condition()
|
||||
self._maximum_depth = 0
|
||||
self._published = 0
|
||||
self._consumed = 0
|
||||
self._dropped_overflow = 0
|
||||
self._dropped_superseded = 0
|
||||
self._closed = False
|
||||
|
||||
def publish(self, item: T) -> None:
|
||||
with self._condition:
|
||||
if self._closed:
|
||||
raise RuntimeError("cannot publish to a closed latest-wins queue")
|
||||
if len(self._items) == self._capacity:
|
||||
self._items.popleft()
|
||||
self._dropped_overflow += 1
|
||||
self._items.append(item)
|
||||
self._published += 1
|
||||
self._maximum_depth = max(self._maximum_depth, len(self._items))
|
||||
self._condition.notify()
|
||||
|
||||
def take_next(self, timeout: float | None = None) -> T | None:
|
||||
with self._condition:
|
||||
ready = self._condition.wait_for(
|
||||
lambda: bool(self._items) or self._closed,
|
||||
timeout=timeout,
|
||||
)
|
||||
if not ready or not self._items:
|
||||
return None
|
||||
item = self._items.popleft()
|
||||
self._consumed += 1
|
||||
return item
|
||||
|
||||
def close(self) -> None:
|
||||
with self._condition:
|
||||
self._closed = True
|
||||
self._condition.notify_all()
|
||||
|
||||
def snapshot(self) -> QueueSnapshot:
|
||||
with self._condition:
|
||||
return QueueSnapshot(
|
||||
capacity=self._capacity,
|
||||
depth=len(self._items),
|
||||
maximum_depth=self._maximum_depth,
|
||||
published=self._published,
|
||||
consumed=self._consumed,
|
||||
dropped_overflow=self._dropped_overflow,
|
||||
dropped_superseded=self._dropped_superseded,
|
||||
closed=self._closed,
|
||||
)
|
||||
|
||||
|
||||
@dataclass(frozen=True, slots=True)
|
||||
class LiveSensorBinding:
|
||||
"""One bounded camera→LiDAR→pose match for live diagnostic fusion."""
|
||||
|
||||
state: Literal[
|
||||
"fused-ready",
|
||||
"lidar-unavailable",
|
||||
"lidar-camera-delta-exceeded",
|
||||
"pose-unavailable",
|
||||
"pose-point-delta-exceeded",
|
||||
]
|
||||
point_cloud: DecodedPointCloudView | None
|
||||
pose: DecodedPoseView | None
|
||||
lidar_camera_delta_ms: float | None
|
||||
pose_point_delta_ms: float | None
|
||||
|
||||
|
||||
class LiveSensorSynchronizer:
|
||||
"""Keep a small arrival-time window and bind sensors without back-pressure.
|
||||
|
||||
The synchronizer deliberately uses the already-recorded host arrival clock
|
||||
carried by the shadow wire contract. It does not claim hardware-clock
|
||||
synchronization. A short wait budget lets a LiDAR or pose event that is
|
||||
already in flight reach the receiver while keeping the detector/world-state
|
||||
latency bounded.
|
||||
"""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
*,
|
||||
maximum_lidar_camera_delta_ms: float,
|
||||
maximum_pose_point_delta_ms: float,
|
||||
capacity_per_modality: int = 32,
|
||||
retention_seconds: float = 3.0,
|
||||
) -> None:
|
||||
if (
|
||||
maximum_lidar_camera_delta_ms <= 0
|
||||
or maximum_pose_point_delta_ms <= 0
|
||||
or capacity_per_modality < 2
|
||||
or retention_seconds <= 0
|
||||
):
|
||||
raise ValueError("live sensor synchronizer bounds are invalid")
|
||||
self._maximum_lidar_camera_delta_ns = round(
|
||||
maximum_lidar_camera_delta_ms * 1_000_000
|
||||
)
|
||||
self._maximum_pose_point_delta_ns = round(maximum_pose_point_delta_ms * 1_000_000)
|
||||
self._capacity = capacity_per_modality
|
||||
self._retention_ns = round(retention_seconds * 1_000_000_000)
|
||||
self._condition = threading.Condition()
|
||||
self._points: deque[DecodedPointCloudView] = deque()
|
||||
self._poses: deque[DecodedPoseView] = deque()
|
||||
self._published_points = 0
|
||||
self._published_poses = 0
|
||||
self._evicted_points = 0
|
||||
self._evicted_poses = 0
|
||||
self._maximum_point_depth = 0
|
||||
self._maximum_pose_depth = 0
|
||||
|
||||
def publish_point_cloud(self, value: DecodedPointCloudView) -> None:
|
||||
with self._condition:
|
||||
self._points.append(value)
|
||||
self._published_points += 1
|
||||
self._evicted_points += self._prune(self._points)
|
||||
self._maximum_point_depth = max(self._maximum_point_depth, len(self._points))
|
||||
self._condition.notify_all()
|
||||
|
||||
def publish_pose(self, value: DecodedPoseView) -> None:
|
||||
with self._condition:
|
||||
self._poses.append(value)
|
||||
self._published_poses += 1
|
||||
self._evicted_poses += self._prune(self._poses)
|
||||
self._maximum_pose_depth = max(self._maximum_pose_depth, len(self._poses))
|
||||
self._condition.notify_all()
|
||||
|
||||
def bind_camera(
|
||||
self,
|
||||
captured_at_epoch_ns: int,
|
||||
*,
|
||||
wait_seconds: float = 0.0,
|
||||
) -> LiveSensorBinding:
|
||||
if captured_at_epoch_ns < 0 or wait_seconds < 0 or not math.isfinite(wait_seconds):
|
||||
raise ValueError("camera synchronization input is invalid")
|
||||
deadline = time.monotonic() + wait_seconds
|
||||
with self._condition:
|
||||
while True:
|
||||
binding = self._binding(captured_at_epoch_ns)
|
||||
if binding.state == "fused-ready" or wait_seconds == 0:
|
||||
return binding
|
||||
remaining = deadline - time.monotonic()
|
||||
if remaining <= 0:
|
||||
return binding
|
||||
self._condition.wait(timeout=remaining)
|
||||
|
||||
def snapshot(self) -> dict[str, int | float]:
|
||||
with self._condition:
|
||||
return {
|
||||
"capacity_per_modality": self._capacity,
|
||||
"retention_seconds": self._retention_ns / 1_000_000_000,
|
||||
"point_depth": len(self._points),
|
||||
"pose_depth": len(self._poses),
|
||||
"maximum_point_depth": self._maximum_point_depth,
|
||||
"maximum_pose_depth": self._maximum_pose_depth,
|
||||
"published_points": self._published_points,
|
||||
"published_poses": self._published_poses,
|
||||
"evicted_points": self._evicted_points,
|
||||
"evicted_poses": self._evicted_poses,
|
||||
}
|
||||
|
||||
def _binding(self, captured_at_epoch_ns: int) -> LiveSensorBinding:
|
||||
if not self._points:
|
||||
return LiveSensorBinding("lidar-unavailable", None, None, None, None)
|
||||
point = min(
|
||||
self._points,
|
||||
key=lambda value: abs(value.context.captured_at_epoch_ns - captured_at_epoch_ns),
|
||||
)
|
||||
lidar_delta_ns = point.context.captured_at_epoch_ns - captured_at_epoch_ns
|
||||
lidar_delta_ms = lidar_delta_ns / 1_000_000
|
||||
if abs(lidar_delta_ns) > self._maximum_lidar_camera_delta_ns:
|
||||
return LiveSensorBinding(
|
||||
"lidar-camera-delta-exceeded",
|
||||
point,
|
||||
None,
|
||||
lidar_delta_ms,
|
||||
None,
|
||||
)
|
||||
if not self._poses:
|
||||
return LiveSensorBinding(
|
||||
"pose-unavailable",
|
||||
point,
|
||||
None,
|
||||
lidar_delta_ms,
|
||||
None,
|
||||
)
|
||||
pose = min(
|
||||
self._poses,
|
||||
key=lambda value: abs(
|
||||
value.context.captured_at_epoch_ns - point.context.captured_at_epoch_ns
|
||||
),
|
||||
)
|
||||
pose_delta_ns = pose.context.captured_at_epoch_ns - point.context.captured_at_epoch_ns
|
||||
pose_delta_ms = pose_delta_ns / 1_000_000
|
||||
if abs(pose_delta_ns) > self._maximum_pose_point_delta_ns:
|
||||
return LiveSensorBinding(
|
||||
"pose-point-delta-exceeded",
|
||||
point,
|
||||
pose,
|
||||
lidar_delta_ms,
|
||||
pose_delta_ms,
|
||||
)
|
||||
return LiveSensorBinding(
|
||||
"fused-ready",
|
||||
point,
|
||||
pose,
|
||||
lidar_delta_ms,
|
||||
pose_delta_ms,
|
||||
)
|
||||
|
||||
def _prune(self, values: deque[DecodedPointCloudView] | deque[DecodedPoseView]) -> int:
|
||||
removed = 0
|
||||
newest = values[-1].context.captured_at_epoch_ns
|
||||
oldest_allowed = newest - self._retention_ns
|
||||
while values and (
|
||||
len(values) > self._capacity
|
||||
or values[0].context.captured_at_epoch_ns < oldest_allowed
|
||||
):
|
||||
values.popleft()
|
||||
removed += 1
|
||||
return removed
|
||||
|
||||
|
||||
def classify_health(
|
||||
*,
|
||||
source_available: bool,
|
||||
fusion_state: str,
|
||||
result_age_ms: float,
|
||||
stale_after_ms: float,
|
||||
unavailable_after_ms: float,
|
||||
) -> tuple[HealthState, tuple[str, ...]]:
|
||||
"""Classify freshness separately from whether depth was available."""
|
||||
|
||||
if stale_after_ms <= 0 or unavailable_after_ms <= stale_after_ms:
|
||||
raise ValueError("health thresholds are invalid")
|
||||
if not source_available or result_age_ms >= unavailable_after_ms:
|
||||
return "unavailable", ("source-unavailable",)
|
||||
if result_age_ms >= stale_after_ms:
|
||||
return "stale", ("result-age-exceeded",)
|
||||
if fusion_state != "fused":
|
||||
return "degraded", (fusion_state,)
|
||||
return "healthy", ()
|
||||
|
||||
|
||||
class WorldStateProjector:
|
||||
"""Project accepted E6 observations into a control-facing, timestamped state."""
|
||||
|
||||
def __init__(self, *, velocity_history_limit_s: float = 1.0) -> None:
|
||||
if velocity_history_limit_s <= 0:
|
||||
raise ValueError("velocity history limit must be positive")
|
||||
self._velocity_history_limit_s = velocity_history_limit_s
|
||||
self._track_history: dict[
|
||||
int, deque[tuple[float, tuple[float, float, float]]]
|
||||
] = {}
|
||||
|
||||
def project(
|
||||
self,
|
||||
*,
|
||||
frame: Mapping[str, Any],
|
||||
lidar_positions: Mapping[int, Sequence[float]],
|
||||
clearance: Mapping[str, Any],
|
||||
) -> dict[str, Any]:
|
||||
session_seconds = float(frame["session_seconds"])
|
||||
objects: list[dict[str, Any]] = []
|
||||
for raw in frame.get("objects", []):
|
||||
if not str(raw.get("cuboid_status", "")).startswith("accepted-"):
|
||||
continue
|
||||
track_id = int(raw["track_id"])
|
||||
center_map = _vector3(raw["cuboid_center_map"], "cuboid center")
|
||||
half_size = _vector3(raw["cuboid_half_size"], "cuboid half-size")
|
||||
quaternion = _vector4(raw["cuboid_quaternion_xyzw"], "cuboid quaternion")
|
||||
velocity, velocity_status, velocity_residual = self._velocity(
|
||||
track_id, session_seconds, center_map
|
||||
)
|
||||
speed = None
|
||||
if velocity is not None:
|
||||
speed = math.sqrt(sum(component * component for component in velocity))
|
||||
lidar = lidar_positions.get(track_id)
|
||||
objects.append(
|
||||
{
|
||||
"track_id": track_id,
|
||||
"class": str(raw["association_group"]),
|
||||
"detector_label": str(raw["label"]),
|
||||
"confidence": float(raw["score"]),
|
||||
"position_map_m": list(center_map),
|
||||
"position_lidar_m": (
|
||||
None if lidar is None else [float(value) for value in lidar]
|
||||
),
|
||||
"orientation_map_xyzw": list(quaternion),
|
||||
"size_m": [2.0 * value for value in half_size],
|
||||
"range_m": float(raw["distance_smoothed_m"]),
|
||||
"velocity_map_mps": None if velocity is None else list(velocity),
|
||||
"speed_mps": speed,
|
||||
"velocity_status": velocity_status,
|
||||
"velocity_residual_m": velocity_residual,
|
||||
"support_points": int(raw["clustered_points"]),
|
||||
"geometry": "point-supported-visible-surface-envelope",
|
||||
}
|
||||
)
|
||||
self._prune(session_seconds)
|
||||
return {
|
||||
"schema_version": WORLD_STATE_SCHEMA,
|
||||
"frame_index": int(frame["frame_index"]),
|
||||
"source_frame_index": int(frame["source_frame_index"]),
|
||||
"session_seconds": session_seconds,
|
||||
"coordinate_frames": {
|
||||
"world": "k1-map",
|
||||
"sensor_relative": "k1-lidar",
|
||||
"vehicle_body": "unavailable-no-rig-to-vehicle-transform",
|
||||
},
|
||||
"fusion_state": str(frame["state"]),
|
||||
"objects": objects,
|
||||
"object_count": len(objects),
|
||||
"clearance": dict(clearance),
|
||||
}
|
||||
|
||||
def _velocity(
|
||||
self,
|
||||
track_id: int,
|
||||
session_seconds: float,
|
||||
center_map: tuple[float, float, float],
|
||||
) -> tuple[tuple[float, float, float] | None, str, float | None]:
|
||||
history = self._track_history.setdefault(track_id, deque(maxlen=32))
|
||||
if history and session_seconds <= history[-1][0]:
|
||||
return None, "unavailable-nonmonotonic-time", None
|
||||
history.append((session_seconds, center_map))
|
||||
oldest = session_seconds - self._velocity_history_limit_s
|
||||
while history and history[0][0] < oldest:
|
||||
history.popleft()
|
||||
if len(history) < 4 or history[-1][0] - history[0][0] < 0.4:
|
||||
return None, "unavailable-insufficient-history", None
|
||||
slopes: list[tuple[float, float, float]] = []
|
||||
values = list(history)
|
||||
for left, (left_time, left_center) in enumerate(values):
|
||||
for right_time, right_center in values[left + 1 :]:
|
||||
delta = right_time - left_time
|
||||
if delta < 0.2:
|
||||
continue
|
||||
slopes.append(
|
||||
(
|
||||
(right_center[0] - left_center[0]) / delta,
|
||||
(right_center[1] - left_center[1]) / delta,
|
||||
(right_center[2] - left_center[2]) / delta,
|
||||
)
|
||||
)
|
||||
if not slopes:
|
||||
return None, "unavailable-insufficient-baseline", None
|
||||
velocity = (
|
||||
statistics.median(item[0] for item in slopes),
|
||||
statistics.median(item[1] for item in slopes),
|
||||
statistics.median(item[2] for item in slopes),
|
||||
)
|
||||
speed = math.sqrt(sum(component * component for component in velocity))
|
||||
latest_time, latest_center = values[-1]
|
||||
residuals = []
|
||||
for observed_time, observed_center in values:
|
||||
predicted = tuple(
|
||||
latest - component * (latest_time - observed_time)
|
||||
for latest, component in zip(latest_center, velocity, strict=True)
|
||||
)
|
||||
residuals.append(
|
||||
math.sqrt(
|
||||
sum(
|
||||
(observed - expected) ** 2
|
||||
for observed, expected in zip(
|
||||
observed_center, predicted, strict=True
|
||||
)
|
||||
)
|
||||
)
|
||||
)
|
||||
residual = statistics.median(residuals)
|
||||
if speed > 20.0:
|
||||
return None, "rejected-speed-bound", residual
|
||||
if residual > 0.75:
|
||||
return None, "rejected-position-residual", residual
|
||||
return velocity, "diagnostic-robust-history", residual
|
||||
|
||||
def _prune(self, session_seconds: float) -> None:
|
||||
oldest = session_seconds - self._velocity_history_limit_s
|
||||
expired = [
|
||||
track_id
|
||||
for track_id, history in self._track_history.items()
|
||||
if not history or history[-1][0] < oldest
|
||||
]
|
||||
for track_id in expired:
|
||||
del self._track_history[track_id]
|
||||
|
||||
|
||||
def wait_until(deadline: float) -> float:
|
||||
"""Wait for a replay deadline and return non-negative scheduling lag seconds."""
|
||||
|
||||
remaining = deadline - time.perf_counter()
|
||||
if remaining > 0:
|
||||
time.sleep(remaining)
|
||||
return max(0.0, time.perf_counter() - deadline)
|
||||
|
||||
|
||||
def _vector3(value: Sequence[Any], label: str) -> tuple[float, float, float]:
|
||||
if len(value) != 3:
|
||||
raise ValueError(f"{label} must contain three values")
|
||||
return float(value[0]), float(value[1]), float(value[2])
|
||||
|
||||
|
||||
def _vector4(value: Sequence[Any], label: str) -> tuple[float, float, float, float]:
|
||||
if len(value) != 4:
|
||||
raise ValueError(f"{label} must contain four values")
|
||||
return float(value[0]), float(value[1]), float(value[2]), float(value[3])
|
||||
Reference in New Issue
Block a user