992 lines
37 KiB
Python
992 lines
37 KiB
Python
"""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": 8,
|
|
"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])
|