feat(k1): add live cameras and reliable spatial following

src/k1link/viewer/rerun_bridge.py

@@ -2,10 +2,11 @@ from __future__ import annotations
 
 import math
 import time
-from collections import deque
 from collections.abc import Callable
+from contextlib import suppress
 from dataclasses import asdict, dataclass
 from typing import Literal
+from uuid import uuid4
 
 import numpy as np
 import rerun as rr
@@ -21,7 +22,12 @@ from k1link.viewer.metrics import BridgeMetrics
 PointColorMode = Literal["intensity", "height", "distance", "rgb", "class"]
 PointPalette = Literal["turbo", "viridis", "plasma", "grayscale", "custom"]
 
-MAX_TRAJECTORY_POSES = 20_000
+MAX_TRAJECTORY_POSES = 2_000
+TRAJECTORY_APPEND_INTERVAL_NS = 500_000_000
+TRAJECTORY_FORCE_APPEND_NS = 2_000_000_000
+TRAJECTORY_MIN_DISTANCE_METERS = 0.02
+TRAJECTORY_PUBLISH_INTERVAL_NS = 500_000_000
+LIVE_GRPC_BUFFER_LIMIT = "32MiB"
 DEFAULT_GRPC_PORT = 9876
 DEFAULT_CORS_ORIGINS = (
     "http://127.0.0.1:5173",
@@ -66,27 +72,52 @@ class RerunBridge:
         self.metrics = metrics or BridgeMetrics()
         self._settings_provider = settings_provider or RerunSceneSettings
         self._settings = self._settings_provider()
-        self._recording = (recording_factory or rr.RecordingStream)("nodedc_mission_core_spatial")
-        blueprint = _blueprint(self._settings)
-        self._url = self._recording.serve_grpc(
-            grpc_port=grpc_port,
-            default_blueprint=blueprint,
-            server_memory_limit="512MiB",
-            newest_first=True,
-            cors_allow_origin=list(cors_allow_origin),
-        )
-        self._recording.send_blueprint(
-            blueprint,
-            make_active=True,
-            make_default=True,
-        )
-        self._recording.log("/world", rr.ViewCoordinates.RIGHT_HAND_Z_UP, static=True)
-        self._recording.log(
-            "/world/sensor_pose",
-            rr.TransformAxes3D(axis_length=0.45, show_frame=True),
-            static=True,
-        )
-        self._path: deque[tuple[float, float, float]] = deque(maxlen=MAX_TRAJECTORY_POSES)
+        if recording_factory is None:
+            recording = rr.RecordingStream(
+                "nodedc_mission_core_spatial",
+                recording_id=uuid4(),
+            )
+        else:
+            recording = recording_factory("nodedc_mission_core_spatial")
+        try:
+            blueprint = _blueprint(self._settings)
+            url = recording.serve_grpc(
+                grpc_port=grpc_port,
+                default_blueprint=blueprint,
+                # This is a reconnect cushion for the live preview, not the source
+                # of record. Raw MQTT evidence is persisted independently. A large
+                # late-client backlog can block the native SDK and freeze preview.
+                server_memory_limit=LIVE_GRPC_BUFFER_LIMIT,
+                # Rerun 0.34.1 can replay ActivateStore before StoreInfo when an
+                # evicted buffer is served newest-first, leaving late viewers on the
+                # welcome screen. Preserve protocol order within the bounded cache.
+                newest_first=False,
+                cors_allow_origin=list(cors_allow_origin),
+            )
+            recording.send_blueprint(
+                blueprint,
+                make_active=True,
+                make_default=True,
+            )
+            recording.log("/world", rr.ViewCoordinates.RIGHT_HAND_Z_UP, static=True)
+            recording.log(
+                "/world/sensor_pose",
+                rr.TransformAxes3D(axis_length=0.45, show_frame=True),
+                static=True,
+            )
+            # Do not expose a URL whose StoreInfo, blueprint and static scene are
+            # still waiting in the SDK micro-batcher.
+            recording.flush(timeout_sec=5.0)
+        except BaseException:
+            # Preserve the construction failure while still making a best
+            # effort to release a partially started native listener.
+            with suppress(BaseException):
+                recording.disconnect()
+            raise
+        self._recording = recording
+        self._url = url
+        self._path: list[tuple[float, float, float]] = []
+        self._last_path_append_source_ns = 0
         self._last_trajectory_publish_ns = 0
         self._last_point_count = 0
         self._closed = False
@@ -96,13 +127,14 @@ class RerunBridge:
         return self._url
 
     def begin_session(self, metrics: BridgeMetrics | None = None) -> None:
-        """Reset session-local state while keeping the process-wide server alive."""
+        """Reset session state while keeping the process-lifetime server alive."""
         if self._closed:
             raise RuntimeError("Rerun bridge is already closed")
         if metrics is not None:
             self.metrics = metrics
         self._settings = self._settings_provider()
         self._path.clear()
+        self._last_path_append_source_ns = 0
         self._last_trajectory_publish_ns = 0
         self._last_point_count = 0
         self._recording.set_time("stream_time", timestamp=time.time())
@@ -118,6 +150,10 @@ class RerunBridge:
             make_active=True,
             make_default=True,
         )
+        # VisualizationRuntime publishes grpc_url only after this method
+        # returns, so a late subscriber cannot race the initial StoreInfo and
+        # blueprint through the SDK micro-batcher.
+        self._recording.flush(timeout_sec=5.0)
 
     def process(self, envelope: DecodedDataPlaneView) -> None:
         self._apply_latest_settings()
@@ -158,12 +194,14 @@ class RerunBridge:
         self._closed = True
         recording = self._recording
         try:
-            recording.flush(timeout_sec=5.0)
+            try:
+                recording.flush(timeout_sec=5.0)
+            finally:
+                recording.disconnect()
         finally:
-            recording.disconnect()
-        # The Python wrapper owns the native gRPC server. Release it immediately
-        # instead of waiting for the publisher thread frame to be collected.
-        del self._recording
+            # The Python wrapper owns the native gRPC server. Release it even if
+            # flush or disconnect fails instead of waiting for garbage collection.
+            del self._recording
 
     def _set_message_time(self, envelope: DecodedDataPlaneView) -> None:
         context = envelope.context
@@ -207,25 +245,35 @@ class RerunBridge:
         )
 
     def _publish_pose(self, frame: DecodedPoseView) -> None:
+        publish_now_ns = time.monotonic_ns()
+        position = (
+            float(frame.position_xyz[0]),
+            float(frame.position_xyz[1]),
+            float(frame.position_xyz[2]),
+        )
         self._recording.log(
             "/world/sensor_pose",
             rr.Transform3D(
-                translation=frame.position_xyz,
+                translation=position,
                 quaternion=rr.Quaternion(xyzw=frame.orientation_xyzw),
             ),
         )
-        self._path.append(frame.position_xyz)
+
+        appended = self._append_trajectory_pose(
+            position,
+            frame.context.captured_at_epoch_ns,
+        )
         if not self._settings.show_trajectory:
             self._recording.log("/world/trajectory", rr.Clear(recursive=False))
             return
-        now_ns = time.monotonic_ns()
+        if not appended:
+            return
         if (
-            len(self._path) > 2
-            and len(self._path) % 20 != 0
-            and now_ns - self._last_trajectory_publish_ns < 200_000_000
+            publish_now_ns - self._last_trajectory_publish_ns
+            < TRAJECTORY_PUBLISH_INTERVAL_NS
         ):
             return
-        self._last_trajectory_publish_ns = now_ns
+        self._last_trajectory_publish_ns = publish_now_ns
         self._recording.log(
             "/world/trajectory",
             rr.LineStrips3D(
@@ -235,6 +283,39 @@ class RerunBridge:
             ),
         )
 
+    def _append_trajectory_pose(
+        self,
+        position: tuple[float, float, float],
+        source_time_ns: int,
+    ) -> bool:
+        if not self._path:
+            self._path.append(position)
+            self._last_path_append_source_ns = source_time_ns
+            return True
+
+        elapsed_ns = source_time_ns - self._last_path_append_source_ns
+        if elapsed_ns < 0:
+            # A source clock discontinuity must not freeze trajectory sampling
+            # until the timestamp catches up again. Preserve message order and
+            # start a new sampling interval at the discontinuity.
+            elapsed_ns = TRAJECTORY_FORCE_APPEND_NS
+        if elapsed_ns < TRAJECTORY_APPEND_INTERVAL_NS:
+            return False
+        if (
+            math.dist(self._path[-1], position) < TRAJECTORY_MIN_DISTANCE_METERS
+            and elapsed_ns < TRAJECTORY_FORCE_APPEND_NS
+        ):
+            return False
+
+        self._path.append(position)
+        self._last_path_append_source_ns = source_time_ns
+        if len(self._path) > MAX_TRAJECTORY_POSES:
+            last = self._path[-1]
+            self._path = self._path[::2]
+            if self._path[-1] != last:
+                self._path.append(last)
+        return True
+
 
 def _blueprint(settings: RerunSceneSettings) -> rrb.Blueprint:
     accumulation = max(0.0, settings.accumulation_seconds)
@@ -255,12 +336,22 @@ def _blueprint(settings: RerunSceneSettings) -> rrb.Blueprint:
             ),
             time_ranges=[time_range],
         ),
+        _live_time_panel(),
         auto_layout=False,
         auto_views=False,
         collapse_panels=True,
     )
 
 
+def _live_time_panel() -> rrb.TimePanel:
+    """Keep the hidden vendor timeline on its native live edge."""
+    return rrb.TimePanel(
+        timeline="stream_time",
+        play_state="following",
+        state="hidden",
+    )
+
+
 def _point_colors(
     positions: np.ndarray,
     intensities: np.ndarray,