SILVER/NODEDC_MISSION_CORE
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
DCCONSTRUCTIONS
2026-07-17 00:26:03 +03:00
parent a281faf923
commit 2bda1986bd
33 changed files with 2927 additions and 330 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/k1link/viewer/rerun_bridge.py
+127 -36
View File
@@ -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,
src/k1link/viewer/runtime.py
+33 -11
View File
@@ -28,6 +28,10 @@ RuntimePhase = Literal[
SourceMode = Literal["idle", "live", "replay"]
StateCallback = Callable[[], None]
BridgeFactory = Callable[..., RerunBridge]
# TODO: replace the mixed-modality FIFO with a latest point-cloud slot and a
# bounded pose queue. The compact queue protects acquisition from a slow
# visualizer, but under sustained pressure it can still evict pose messages.
PREVIEW_QUEUE_SIZE = 4
class CanonicalNormalizer(Protocol):
@@ -191,7 +195,13 @@ class VisualizationRuntime:
self._bridge = None
self._rerun_grpc_url = None
if bridge is not None:
bridge.close()
try:
bridge.close()
except BaseException as exc:
self._finish_error(
f"Ошибка завершения визуального моста: {type(exc).__name__}: {exc}"
)
raise
self._notify()
def _start(
@@ -308,7 +318,7 @@ class VisualizationRuntime:
*,
running_phase: RuntimePhase,
) -> None:
messages: queue.Queue[StreamMessage] = queue.Queue(maxsize=32)
messages: queue.Queue[StreamMessage] = queue.Queue(maxsize=PREVIEW_QUEUE_SIZE)
source_done = threading.Event()
publisher_ready = threading.Event()
publisher_aborted = threading.Event()
@@ -334,23 +344,28 @@ class VisualizationRuntime:
def publish() -> None:
bridge: RerunBridge | None = None
try:
bridge = self._bridge
with self._lock:
bridge = self._bridge
if self._closed:
publisher_aborted.set()
if publisher_aborted.is_set():
publisher_ready.set()
return
if bridge is None:
candidate = self._bridge_factory(
grpc_port=self._grpc_port,
metrics=self._metrics,
settings_provider=self._current_scene_settings,
)
bridge = candidate
with self._lock:
if self._closed:
publisher_aborted.set()
else:
self._bridge = candidate
bridge = candidate
if publisher_aborted.is_set():
candidate.close()
publisher_ready.set()
return
if publisher_aborted.is_set():
publisher_ready.set()
return
assert bridge is not None
bridge.begin_session(self._metrics)
with self._lock:
@@ -402,12 +417,19 @@ class VisualizationRuntime:
finally:
if bridge is not None:
with self._lock:
close_bridge = self._closed and self._bridge is bridge
if close_bridge:
close_bridge = self._closed and (
self._bridge is bridge or publisher_aborted.is_set()
)
if close_bridge and self._bridge is bridge:
self._bridge = None
self._rerun_grpc_url = None
if close_bridge:
bridge.close()
try:
bridge.close()
except BaseException as exc:
# Process shutdown must not become a false successful
# idle state when the native bridge failed to close.
publisher_error.append(exc)
def join_publisher() -> None:
# Never orphan a publisher: the session thread remains its owner.