DCCONSTRUCTIONS
b53d6d5a45
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.
229 lines
8.9 KiB
Python
229 lines
8.9 KiB
Python
from __future__ import annotations
|
|
|
|
import math
|
|
import statistics
|
|
import threading
|
|
import time
|
|
from collections import deque
|
|
from typing import TypedDict
|
|
|
|
|
|
class MetricsSnapshot(TypedDict):
|
|
messages_received: int
|
|
payload_bytes: int
|
|
pcl_frames: int
|
|
pose_frames: int
|
|
points_published: int
|
|
last_point_count: int
|
|
decode_errors: int
|
|
preview_dropped: int
|
|
pcl_fps: float
|
|
pose_fps: float
|
|
mqtt_to_publish_ms: float | None
|
|
mqtt_to_publish_p50_ms: float | None
|
|
mqtt_to_publish_p95_ms: float | None
|
|
decode_publish_ms: float | None
|
|
trajectory_poses: int
|
|
device_elapsed_seconds: float | None
|
|
device_route_distance_meters: float | None
|
|
device_speed_meters_per_second: float | None
|
|
device_pgo_progress: int | None
|
|
modeling_reports: int
|
|
modeling_decode_errors: int
|
|
perception_frames: int
|
|
perception_dropped: int
|
|
perception_fps: float
|
|
perception_end_to_end_ms: float | None
|
|
perception_end_to_end_p95_ms: float | None
|
|
perception_stale_ms: float | None
|
|
|
|
|
|
class BridgeMetrics:
|
|
"""Thread-safe counters shared by canonical visualization consumers."""
|
|
|
|
def __init__(self) -> None:
|
|
self._lock = threading.Lock()
|
|
self._messages_received = 0
|
|
self._payload_bytes = 0
|
|
self._pcl_frames = 0
|
|
self._pose_frames = 0
|
|
self._points_published = 0
|
|
self._last_point_count = 0
|
|
self._decode_errors = 0
|
|
self._preview_dropped = 0
|
|
self._trajectory_poses = 0
|
|
self._pcl_times: deque[int] = deque()
|
|
self._pose_times: deque[int] = deque()
|
|
self._latencies_ms: deque[float] = deque(maxlen=512)
|
|
self._decode_publish_ms: float | None = None
|
|
self._device_elapsed_seconds: float | None = None
|
|
self._device_route_distance_meters: float | None = None
|
|
self._device_speed_meters_per_second: float | None = None
|
|
self._device_pgo_progress: int | None = None
|
|
self._modeling_reports = 0
|
|
self._modeling_decode_errors = 0
|
|
self._perception_frames = 0
|
|
self._perception_dropped = 0
|
|
self._perception_times: deque[int] = deque()
|
|
self._perception_latencies_ms: deque[float] = deque(maxlen=512)
|
|
self._perception_last_publish_monotonic_ns: int | None = None
|
|
|
|
def received(self, payload_bytes: int) -> None:
|
|
with self._lock:
|
|
self._messages_received += 1
|
|
self._payload_bytes += payload_bytes
|
|
|
|
def published_pcl(self, point_count: int, now_ns: int, decode_publish_ms: float) -> None:
|
|
with self._lock:
|
|
self._pcl_frames += 1
|
|
self._points_published += point_count
|
|
self._last_point_count = point_count
|
|
self._decode_publish_ms = decode_publish_ms
|
|
self._pcl_times.append(now_ns)
|
|
_trim_rate_window(self._pcl_times, now_ns)
|
|
|
|
def published_pose(self, now_ns: int, decode_publish_ms: float, path_size: int) -> None:
|
|
with self._lock:
|
|
self._pose_frames += 1
|
|
self._decode_publish_ms = decode_publish_ms
|
|
self._trajectory_poses = path_size
|
|
self._pose_times.append(now_ns)
|
|
_trim_rate_window(self._pose_times, now_ns)
|
|
|
|
def record_latency(self, milliseconds: float) -> None:
|
|
if not math.isfinite(milliseconds) or milliseconds < 0:
|
|
return
|
|
with self._lock:
|
|
self._latencies_ms.append(milliseconds)
|
|
|
|
def decode_error(self) -> None:
|
|
with self._lock:
|
|
self._decode_errors += 1
|
|
|
|
def preview_dropped(self) -> None:
|
|
with self._lock:
|
|
self._preview_dropped += 1
|
|
|
|
def perception_dropped(self) -> None:
|
|
with self._lock:
|
|
self._perception_dropped += 1
|
|
|
|
def published_perception(
|
|
self,
|
|
*,
|
|
captured_at_epoch_ns: int,
|
|
published_at_epoch_ns: int,
|
|
published_monotonic_ns: int,
|
|
) -> None:
|
|
latency_ms = (published_at_epoch_ns - captured_at_epoch_ns) / 1_000_000
|
|
with self._lock:
|
|
self._perception_frames += 1
|
|
self._perception_times.append(published_monotonic_ns)
|
|
_trim_rate_window(self._perception_times, published_monotonic_ns)
|
|
self._perception_last_publish_monotonic_ns = published_monotonic_ns
|
|
if math.isfinite(latency_ms) and latency_ms >= 0:
|
|
self._perception_latencies_ms.append(latency_ms)
|
|
|
|
def acquisition_telemetry(
|
|
self,
|
|
*,
|
|
elapsed_seconds: float,
|
|
route_distance_meters: float,
|
|
speed_meters_per_second: float,
|
|
pgo_progress: int,
|
|
) -> None:
|
|
values = (elapsed_seconds, route_distance_meters, speed_meters_per_second)
|
|
if any(not math.isfinite(value) or value < 0 for value in values):
|
|
return
|
|
if pgo_progress < 0:
|
|
return
|
|
with self._lock:
|
|
self._device_elapsed_seconds = elapsed_seconds
|
|
# This is the device's current scan generation. FW 3.0.2 resets
|
|
# ScanTime and MoveDistance together; carrying an earlier project's
|
|
# route across that boundary would be false precision.
|
|
self._device_route_distance_meters = route_distance_meters
|
|
self._device_speed_meters_per_second = speed_meters_per_second
|
|
self._device_pgo_progress = pgo_progress
|
|
self._modeling_reports += 1
|
|
|
|
def modeling_decode_error(self) -> None:
|
|
with self._lock:
|
|
self._modeling_decode_errors += 1
|
|
|
|
def snapshot(self) -> MetricsSnapshot:
|
|
now_ns = time.monotonic_ns()
|
|
with self._lock:
|
|
_trim_rate_window(self._pcl_times, now_ns)
|
|
_trim_rate_window(self._pose_times, now_ns)
|
|
_trim_rate_window(self._perception_times, now_ns)
|
|
latencies = list(self._latencies_ms)
|
|
perception_latencies = list(self._perception_latencies_ms)
|
|
last_latency = latencies[-1] if latencies else None
|
|
p50 = statistics.median(latencies) if latencies else None
|
|
p95 = _percentile(latencies, 0.95) if latencies else None
|
|
return {
|
|
"messages_received": self._messages_received,
|
|
"payload_bytes": self._payload_bytes,
|
|
"pcl_frames": self._pcl_frames,
|
|
"pose_frames": self._pose_frames,
|
|
"points_published": self._points_published,
|
|
"last_point_count": self._last_point_count,
|
|
"decode_errors": self._decode_errors,
|
|
"preview_dropped": self._preview_dropped,
|
|
"pcl_fps": _window_rate(self._pcl_times),
|
|
"pose_fps": _window_rate(self._pose_times),
|
|
"mqtt_to_publish_ms": _rounded(last_latency),
|
|
"mqtt_to_publish_p50_ms": _rounded(p50),
|
|
"mqtt_to_publish_p95_ms": _rounded(p95),
|
|
"decode_publish_ms": _rounded(self._decode_publish_ms),
|
|
"trajectory_poses": self._trajectory_poses,
|
|
"device_elapsed_seconds": _rounded(self._device_elapsed_seconds),
|
|
"device_route_distance_meters": _rounded(self._device_route_distance_meters),
|
|
"device_speed_meters_per_second": _rounded(self._device_speed_meters_per_second),
|
|
"device_pgo_progress": self._device_pgo_progress,
|
|
"modeling_reports": self._modeling_reports,
|
|
"modeling_decode_errors": self._modeling_decode_errors,
|
|
"perception_frames": self._perception_frames,
|
|
"perception_dropped": self._perception_dropped,
|
|
"perception_fps": _window_rate(self._perception_times),
|
|
"perception_end_to_end_ms": _rounded(
|
|
perception_latencies[-1] if perception_latencies else None
|
|
),
|
|
"perception_end_to_end_p95_ms": _rounded(
|
|
_percentile(perception_latencies, 0.95)
|
|
if perception_latencies
|
|
else None
|
|
),
|
|
"perception_stale_ms": _rounded(
|
|
None
|
|
if self._perception_last_publish_monotonic_ns is None
|
|
else (now_ns - self._perception_last_publish_monotonic_ns) / 1_000_000
|
|
),
|
|
}
|
|
|
|
|
|
def _trim_rate_window(samples: deque[int], now_ns: int) -> None:
|
|
cutoff_ns = now_ns - 1_000_000_000
|
|
while samples and samples[0] < cutoff_ns:
|
|
samples.popleft()
|
|
|
|
|
|
def _window_rate(samples: deque[int]) -> float:
|
|
if len(samples) < 2:
|
|
return float(len(samples))
|
|
elapsed = (samples[-1] - samples[0]) / 1_000_000_000
|
|
return len(samples) / max(elapsed, 1.0)
|
|
|
|
|
|
def _percentile(values: list[float], quantile: float) -> float:
|
|
if not values:
|
|
raise ValueError("values must not be empty")
|
|
ordered = sorted(values)
|
|
index = max(0, min(len(ordered) - 1, math.ceil(len(ordered) * quantile) - 1))
|
|
return ordered[index]
|
|
|
|
|
|
def _rounded(value: float | None) -> float | None:
|
|
return None if value is None else round(value, 3)
|