feat(device-plugins): add profiled K1 lifecycle and canonical data plane

src/k1link/viewer/metrics.py

@@ -0,0 +1,134 @@
+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
+
+
+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
+
+    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 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)
+            latencies = list(self._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,
+            }
+
+
+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)