feat: activate self-hosted lidar spatial scene
This commit is contained in:
@@ -0,0 +1,386 @@
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
import time
|
||||
from collections import deque
|
||||
from collections.abc import Callable
|
||||
from dataclasses import asdict, dataclass
|
||||
from typing import Literal
|
||||
|
||||
import numpy as np
|
||||
import rerun as rr
|
||||
from rerun import blueprint as rrb
|
||||
|
||||
from k1link.protocol.streams import (
|
||||
LegacyPointCloudFrame,
|
||||
LegacyPoseFrame,
|
||||
LioPointCloudFrame,
|
||||
LioPoseFrame,
|
||||
StreamDecodeError,
|
||||
decode_legacy_pointcloud,
|
||||
decode_legacy_pose,
|
||||
decode_lio_pcl,
|
||||
decode_lio_pose,
|
||||
)
|
||||
from k1link.viewer.foxglove_bridge import BridgeMetrics
|
||||
from k1link.viewer.messages import StreamMessage
|
||||
|
||||
PointColorMode = Literal["intensity", "height", "distance", "rgb", "class"]
|
||||
PointPalette = Literal["turbo", "viridis", "plasma", "grayscale", "custom"]
|
||||
|
||||
MAX_TRAJECTORY_POSES = 20_000
|
||||
DEFAULT_GRPC_PORT = 9876
|
||||
DEFAULT_CORS_ORIGINS = (
|
||||
"http://127.0.0.1:5173",
|
||||
"http://localhost:5173",
|
||||
"http://127.0.0.1:4173",
|
||||
"http://localhost:4173",
|
||||
"http://127.0.0.1:8000",
|
||||
"http://localhost:8000",
|
||||
)
|
||||
|
||||
|
||||
@dataclass(frozen=True, slots=True)
|
||||
class RerunSceneSettings:
|
||||
point_size: float = 2.5
|
||||
color_mode: PointColorMode = "intensity"
|
||||
palette: PointPalette = "turbo"
|
||||
custom_color: str = "#f7f8f4"
|
||||
accumulation_seconds: float = 12.0
|
||||
show_points: bool = True
|
||||
show_trajectory: bool = True
|
||||
show_grid: bool = True
|
||||
|
||||
def as_dict(self) -> dict[str, object]:
|
||||
return asdict(self)
|
||||
|
||||
|
||||
SettingsProvider = Callable[[], RerunSceneSettings]
|
||||
|
||||
|
||||
class RerunBridge:
|
||||
"""Decode verified device topics into a self-hosted Rerun recording stream."""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
*,
|
||||
grpc_port: int = DEFAULT_GRPC_PORT,
|
||||
metrics: BridgeMetrics | None = None,
|
||||
settings_provider: SettingsProvider | None = None,
|
||||
cors_allow_origin: tuple[str, ...] = DEFAULT_CORS_ORIGINS,
|
||||
recording_factory: Callable[[str], rr.RecordingStream] | None = None,
|
||||
) -> None:
|
||||
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_device_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
|
||||
)
|
||||
self._last_trajectory_publish_ns = 0
|
||||
self._last_point_count = 0
|
||||
self._closed = False
|
||||
|
||||
@property
|
||||
def grpc_url(self) -> str:
|
||||
return self._url
|
||||
|
||||
def begin_session(self, metrics: BridgeMetrics | None = None) -> None:
|
||||
"""Reset session-local state while keeping the process-wide 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_trajectory_publish_ns = 0
|
||||
self._last_point_count = 0
|
||||
self._recording.set_time("stream_time", timestamp=time.time())
|
||||
self._recording.log("/world", rr.Clear(recursive=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._recording.send_blueprint(
|
||||
_blueprint(self._settings),
|
||||
make_active=True,
|
||||
make_default=True,
|
||||
)
|
||||
|
||||
def process(self, message: StreamMessage) -> None:
|
||||
started_ns = time.monotonic_ns()
|
||||
self.metrics.received(len(message.payload))
|
||||
self._apply_latest_settings()
|
||||
self._set_message_time(message)
|
||||
|
||||
try:
|
||||
if message.topic.endswith("/lio_pcl"):
|
||||
self._publish_lio_pcl(decode_lio_pcl(message.payload))
|
||||
point_frame = True
|
||||
elif message.topic == "RealtimePointcloud":
|
||||
self._publish_legacy_pcl(decode_legacy_pointcloud(message.payload))
|
||||
point_frame = True
|
||||
elif message.topic.endswith("/lio_pose"):
|
||||
self._publish_lio_pose(decode_lio_pose(message.payload))
|
||||
point_frame = False
|
||||
elif message.topic == "RealtimePath":
|
||||
self._publish_legacy_pose(decode_legacy_pose(message.payload))
|
||||
point_frame = False
|
||||
else:
|
||||
return
|
||||
except StreamDecodeError:
|
||||
self.metrics.decode_error()
|
||||
return
|
||||
|
||||
published_ns = time.monotonic_ns()
|
||||
decode_publish_ms = (published_ns - started_ns) / 1_000_000
|
||||
if point_frame:
|
||||
self.metrics.published_pcl(
|
||||
self._last_point_count,
|
||||
published_ns,
|
||||
decode_publish_ms,
|
||||
)
|
||||
else:
|
||||
self.metrics.published_pose(
|
||||
published_ns,
|
||||
decode_publish_ms,
|
||||
len(self._path),
|
||||
)
|
||||
if message.source == "live_mqtt" and message.received_monotonic_ns is not None:
|
||||
self.metrics.record_latency(
|
||||
(published_ns - message.received_monotonic_ns) / 1_000_000
|
||||
)
|
||||
|
||||
def close(self) -> None:
|
||||
if self._closed:
|
||||
return
|
||||
self._closed = True
|
||||
recording = self._recording
|
||||
try:
|
||||
recording.flush(timeout_sec=5.0)
|
||||
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
|
||||
|
||||
def _set_message_time(self, message: StreamMessage) -> None:
|
||||
self._recording.set_time("stream_time", timestamp=time.time())
|
||||
self._recording.set_time(
|
||||
"capture_time",
|
||||
timestamp=message.received_at_epoch_ns / 1_000_000_000,
|
||||
)
|
||||
self._recording.set_time("message_sequence", sequence=message.sequence)
|
||||
|
||||
def _apply_latest_settings(self) -> None:
|
||||
settings = self._settings_provider()
|
||||
if settings == self._settings:
|
||||
return
|
||||
self._settings = settings
|
||||
self._recording.send_blueprint(_blueprint(settings))
|
||||
|
||||
def _publish_lio_pcl(self, frame: LioPointCloudFrame) -> None:
|
||||
count = len(frame.points)
|
||||
positions = np.empty((count, 3), dtype=np.float32)
|
||||
intensities = np.empty(count, dtype=np.uint8)
|
||||
scaler = frame.header.scaler
|
||||
for index, point in enumerate(frame.points):
|
||||
positions[index] = point.scaled_xyz(scaler)
|
||||
intensities[index] = point.intensity
|
||||
self._publish_points(positions, intensities, rgb=None)
|
||||
|
||||
def _publish_legacy_pcl(self, frame: LegacyPointCloudFrame) -> None:
|
||||
count = len(frame.points)
|
||||
positions = np.empty((count, 3), dtype=np.float32)
|
||||
intensities = np.empty(count, dtype=np.uint8)
|
||||
rgb = np.empty((count, 3), dtype=np.uint8)
|
||||
for index, point in enumerate(frame.points):
|
||||
positions[index] = (point.x, point.y, point.z)
|
||||
intensities[index] = point.intensity
|
||||
rgb[index] = (point.r, point.g, point.b)
|
||||
self._publish_points(positions, intensities, rgb=rgb)
|
||||
|
||||
def _publish_points(
|
||||
self,
|
||||
positions: np.ndarray,
|
||||
intensities: np.ndarray,
|
||||
*,
|
||||
rgb: np.ndarray | None,
|
||||
) -> None:
|
||||
self._last_point_count = int(positions.shape[0])
|
||||
if not self._settings.show_points:
|
||||
self._recording.log("/world/points", rr.Clear(recursive=False))
|
||||
return
|
||||
colors = _point_colors(positions, intensities, rgb, self._settings)
|
||||
self._recording.log(
|
||||
"/world/points",
|
||||
rr.Points3D(
|
||||
positions,
|
||||
colors=colors,
|
||||
radii=rr.Radius.ui_points(self._settings.point_size),
|
||||
),
|
||||
)
|
||||
|
||||
def _publish_lio_pose(self, frame: LioPoseFrame) -> None:
|
||||
self._publish_pose(frame.position_xyz, frame.orientation_xyzw)
|
||||
|
||||
def _publish_legacy_pose(self, frame: LegacyPoseFrame) -> None:
|
||||
self._publish_pose(frame.position_xyz, frame.orientation_xyzw)
|
||||
|
||||
def _publish_pose(
|
||||
self,
|
||||
position_xyz: tuple[float, float, float],
|
||||
orientation_xyzw: tuple[float, float, float, float],
|
||||
) -> None:
|
||||
self._recording.log(
|
||||
"/world/sensor_pose",
|
||||
rr.Transform3D(
|
||||
translation=position_xyz,
|
||||
quaternion=rr.Quaternion(xyzw=orientation_xyzw),
|
||||
),
|
||||
)
|
||||
self._path.append(position_xyz)
|
||||
if not self._settings.show_trajectory:
|
||||
self._recording.log("/world/trajectory", rr.Clear(recursive=False))
|
||||
return
|
||||
now_ns = time.monotonic_ns()
|
||||
if (
|
||||
len(self._path) > 2
|
||||
and len(self._path) % 20 != 0
|
||||
and now_ns - self._last_trajectory_publish_ns < 200_000_000
|
||||
):
|
||||
return
|
||||
self._last_trajectory_publish_ns = now_ns
|
||||
self._recording.log(
|
||||
"/world/trajectory",
|
||||
rr.LineStrips3D(
|
||||
[list(self._path)],
|
||||
colors=[247, 248, 244, 255],
|
||||
radii=rr.Radius.ui_points(2.0),
|
||||
),
|
||||
)
|
||||
|
||||
def _blueprint(settings: RerunSceneSettings) -> rrb.Blueprint:
|
||||
accumulation = max(0.0, settings.accumulation_seconds)
|
||||
time_range = rr.VisibleTimeRange(
|
||||
"stream_time",
|
||||
start=rr.TimeRangeBoundary.cursor_relative(seconds=-accumulation),
|
||||
end=rr.TimeRangeBoundary.cursor_relative(),
|
||||
)
|
||||
return rrb.Blueprint(
|
||||
rrb.Spatial3DView(
|
||||
origin="/world",
|
||||
name="Пространственная сцена",
|
||||
background=[7, 8, 10, 255],
|
||||
line_grid=rrb.LineGrid3D(
|
||||
visible=settings.show_grid,
|
||||
color=[86, 91, 99, 110],
|
||||
stroke_width=0.75,
|
||||
),
|
||||
time_ranges=[time_range],
|
||||
),
|
||||
auto_layout=False,
|
||||
auto_views=False,
|
||||
collapse_panels=True,
|
||||
)
|
||||
|
||||
|
||||
def _point_colors(
|
||||
positions: np.ndarray,
|
||||
intensities: np.ndarray,
|
||||
rgb: np.ndarray | None,
|
||||
settings: RerunSceneSettings,
|
||||
) -> np.ndarray:
|
||||
if settings.color_mode == "rgb" and rgb is not None:
|
||||
return rgb
|
||||
if settings.color_mode == "class":
|
||||
color = _parse_hex_color(settings.custom_color)
|
||||
return np.tile(np.asarray(color, dtype=np.uint8), (positions.shape[0], 1))
|
||||
if settings.color_mode == "height":
|
||||
values = _normalize(positions[:, 2])
|
||||
elif settings.color_mode == "distance":
|
||||
values = _normalize(np.linalg.norm(positions, axis=1))
|
||||
else:
|
||||
values = intensities.astype(np.float32) / 255.0
|
||||
return _apply_palette(values, settings.palette, settings.custom_color)
|
||||
|
||||
|
||||
def _normalize(values: np.ndarray) -> np.ndarray:
|
||||
if values.size == 0:
|
||||
return values.astype(np.float32)
|
||||
minimum = float(np.min(values))
|
||||
maximum = float(np.max(values))
|
||||
if not math.isfinite(minimum) or not math.isfinite(maximum) or maximum <= minimum:
|
||||
return np.full(values.shape, 0.5, dtype=np.float32)
|
||||
return ((values - minimum) / (maximum - minimum)).astype(np.float32)
|
||||
|
||||
|
||||
def _apply_palette(values: np.ndarray, palette: PointPalette, custom: str) -> np.ndarray:
|
||||
clipped = np.clip(values, 0.0, 1.0)
|
||||
if palette == "custom":
|
||||
color = _parse_hex_color(custom)
|
||||
return np.tile(np.asarray(color, dtype=np.uint8), (values.size, 1))
|
||||
if palette == "grayscale":
|
||||
channel = np.rint(clipped * 255).astype(np.uint8)
|
||||
return np.column_stack((channel, channel, channel))
|
||||
|
||||
stops = {
|
||||
"turbo": (
|
||||
(0.00, (48, 18, 59)),
|
||||
(0.25, (40, 126, 231)),
|
||||
(0.50, (42, 240, 154)),
|
||||
(0.75, (246, 214, 55)),
|
||||
(1.00, (180, 4, 38)),
|
||||
),
|
||||
"viridis": (
|
||||
(0.00, (68, 1, 84)),
|
||||
(0.25, (59, 82, 139)),
|
||||
(0.50, (33, 145, 140)),
|
||||
(0.75, (94, 201, 98)),
|
||||
(1.00, (253, 231, 37)),
|
||||
),
|
||||
"plasma": (
|
||||
(0.00, (13, 8, 135)),
|
||||
(0.25, (126, 3, 168)),
|
||||
(0.50, (204, 71, 120)),
|
||||
(0.75, (248, 149, 64)),
|
||||
(1.00, (240, 249, 33)),
|
||||
),
|
||||
}[palette]
|
||||
positions = np.asarray([item[0] for item in stops], dtype=np.float32)
|
||||
colors = np.asarray([item[1] for item in stops], dtype=np.float32)
|
||||
channels = [np.interp(clipped, positions, colors[:, index]) for index in range(3)]
|
||||
return np.rint(np.column_stack(channels)).astype(np.uint8)
|
||||
|
||||
|
||||
def _parse_hex_color(value: str) -> tuple[int, int, int]:
|
||||
candidate = value.removeprefix("#")
|
||||
if len(candidate) != 6:
|
||||
return 247, 248, 244
|
||||
try:
|
||||
return tuple(int(candidate[index : index + 2], 16) for index in (0, 2, 4)) # type: ignore[return-value]
|
||||
except ValueError:
|
||||
return 247, 248, 244
|
||||
+121
-18
@@ -11,9 +11,10 @@ from typing import Literal, TypedDict
|
||||
|
||||
from k1link.artifacts import utc_now_iso, write_json_atomic
|
||||
from k1link.mqtt import CapturedMqttMessage, CaptureError, capture_mqtt
|
||||
from k1link.viewer.foxglove_bridge import BridgeMetrics, FoxgloveBridge, MetricsSnapshot
|
||||
from k1link.viewer.foxglove_bridge import BridgeMetrics, MetricsSnapshot
|
||||
from k1link.viewer.messages import StreamMessage
|
||||
from k1link.viewer.replay import iter_replay_messages
|
||||
from k1link.viewer.rerun_bridge import DEFAULT_GRPC_PORT, RerunBridge, RerunSceneSettings
|
||||
|
||||
RuntimePhase = Literal[
|
||||
"idle",
|
||||
@@ -25,6 +26,7 @@ RuntimePhase = Literal[
|
||||
]
|
||||
SourceMode = Literal["idle", "live", "replay"]
|
||||
StateCallback = Callable[[], None]
|
||||
BridgeFactory = Callable[..., RerunBridge]
|
||||
|
||||
|
||||
class RuntimeSnapshot(TypedDict):
|
||||
@@ -33,13 +35,21 @@ class RuntimeSnapshot(TypedDict):
|
||||
source_mode: SourceMode
|
||||
foxglove_ws_url: str | None
|
||||
foxglove_viewer_url: str | None
|
||||
rerun_grpc_url: str | None
|
||||
viewer_settings: dict[str, object]
|
||||
metrics: MetricsSnapshot
|
||||
|
||||
|
||||
class VisualizationRuntime:
|
||||
"""Own one bounded live/replay source and one deterministic publisher thread."""
|
||||
|
||||
def __init__(self, *, on_state_change: StateCallback | None = None) -> None:
|
||||
def __init__(
|
||||
self,
|
||||
*,
|
||||
on_state_change: StateCallback | None = None,
|
||||
grpc_port: int = DEFAULT_GRPC_PORT,
|
||||
bridge_factory: BridgeFactory | None = None,
|
||||
) -> None:
|
||||
self._lock = threading.Lock()
|
||||
self._on_state_change = on_state_change
|
||||
self._thread: threading.Thread | None = None
|
||||
@@ -49,6 +59,12 @@ class VisualizationRuntime:
|
||||
self._source_mode: SourceMode = "idle"
|
||||
self._foxglove_ws_url: str | None = None
|
||||
self._foxglove_viewer_url: str | None = None
|
||||
self._rerun_grpc_url: str | None = None
|
||||
self._grpc_port = grpc_port
|
||||
self._bridge_factory = bridge_factory or RerunBridge
|
||||
self._bridge: RerunBridge | None = None
|
||||
self._closed = False
|
||||
self._scene_settings = RerunSceneSettings()
|
||||
self._metrics = BridgeMetrics()
|
||||
|
||||
def snapshot(self) -> RuntimeSnapshot:
|
||||
@@ -59,9 +75,17 @@ class VisualizationRuntime:
|
||||
"source_mode": self._source_mode,
|
||||
"foxglove_ws_url": self._foxglove_ws_url,
|
||||
"foxglove_viewer_url": self._foxglove_viewer_url,
|
||||
"rerun_grpc_url": self._rerun_grpc_url,
|
||||
"viewer_settings": self._scene_settings.as_dict(),
|
||||
"metrics": self._metrics.snapshot(),
|
||||
}
|
||||
|
||||
def update_scene_settings(self, settings: RerunSceneSettings) -> RuntimeSnapshot:
|
||||
with self._lock:
|
||||
self._scene_settings = settings
|
||||
self._notify()
|
||||
return self.snapshot()
|
||||
|
||||
def start_replay(self, path: Path, *, speed: float = 1.0, loop: bool = False) -> None:
|
||||
resolved = path.expanduser().resolve()
|
||||
if not resolved.is_file():
|
||||
@@ -123,6 +147,34 @@ class VisualizationRuntime:
|
||||
assert thread is not None
|
||||
thread.join(timeout=wait_seconds)
|
||||
|
||||
def close(self, *, wait_seconds: float = 5.0) -> None:
|
||||
"""Stop the active source and release the process-wide visual bridge."""
|
||||
with self._lock:
|
||||
self._closed = True
|
||||
thread = self._thread
|
||||
if thread is not None and thread.is_alive():
|
||||
self._phase = "stopping"
|
||||
self._message = "Завершаем локальный поток и визуальный мост."
|
||||
self._stop_event.set()
|
||||
else:
|
||||
self._phase = "idle"
|
||||
self._source_mode = "idle"
|
||||
self._message = "Локальный поток завершён."
|
||||
self._notify()
|
||||
|
||||
if thread is not None and thread.is_alive():
|
||||
thread.join(timeout=wait_seconds)
|
||||
|
||||
with self._lock:
|
||||
thread_alive = self._thread is not None and self._thread.is_alive()
|
||||
bridge = None if thread_alive else self._bridge
|
||||
if not thread_alive:
|
||||
self._bridge = None
|
||||
self._rerun_grpc_url = None
|
||||
if bridge is not None:
|
||||
bridge.close()
|
||||
self._notify()
|
||||
|
||||
def _start(
|
||||
self,
|
||||
*,
|
||||
@@ -132,6 +184,8 @@ class VisualizationRuntime:
|
||||
target: Callable[[], None],
|
||||
) -> None:
|
||||
with self._lock:
|
||||
if self._closed:
|
||||
raise RuntimeError("runtime завершён; перезапустите локальный сервер")
|
||||
if self._thread is not None and self._thread.is_alive():
|
||||
raise RuntimeError("поток уже запущен; сначала остановите текущую сессию")
|
||||
self._stop_event = threading.Event()
|
||||
@@ -221,6 +275,7 @@ class VisualizationRuntime:
|
||||
messages: queue.Queue[StreamMessage] = queue.Queue(maxsize=32)
|
||||
source_done = threading.Event()
|
||||
publisher_ready = threading.Event()
|
||||
publisher_aborted = threading.Event()
|
||||
publisher_error: list[BaseException] = []
|
||||
|
||||
def enqueue(message: StreamMessage) -> None:
|
||||
@@ -241,19 +296,41 @@ class VisualizationRuntime:
|
||||
self._metrics.preview_dropped()
|
||||
|
||||
def publish() -> None:
|
||||
bridge: FoxgloveBridge | None = None
|
||||
bridge: RerunBridge | None = None
|
||||
try:
|
||||
bridge = FoxgloveBridge(metrics=self._metrics)
|
||||
bridge = self._bridge
|
||||
if bridge is None:
|
||||
candidate = self._bridge_factory(
|
||||
grpc_port=self._grpc_port,
|
||||
metrics=self._metrics,
|
||||
settings_provider=self._current_scene_settings,
|
||||
)
|
||||
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
|
||||
assert bridge is not None
|
||||
bridge.begin_session(self._metrics)
|
||||
with self._lock:
|
||||
self._foxglove_ws_url = bridge.websocket_url
|
||||
self._foxglove_viewer_url = bridge.viewer_url
|
||||
if self._phase != "stopping":
|
||||
if self._closed:
|
||||
publisher_aborted.set()
|
||||
else:
|
||||
self._rerun_grpc_url = bridge.grpc_url
|
||||
if not self._closed and self._phase != "stopping":
|
||||
self._phase = running_phase
|
||||
self._message = (
|
||||
"Локальный мост визуализации готов; источник данных запущен."
|
||||
"Локальный Rerun-мост готов; источник данных запущен."
|
||||
)
|
||||
publisher_ready.set()
|
||||
self._notify()
|
||||
if publisher_aborted.is_set():
|
||||
return
|
||||
while not source_done.is_set() or not messages.empty():
|
||||
if self._stop_event.is_set() and source_done.is_set() and messages.empty():
|
||||
break
|
||||
@@ -268,19 +345,43 @@ class VisualizationRuntime:
|
||||
if self._metrics.snapshot()["messages_received"] % 10 == 0:
|
||||
self._notify()
|
||||
except BaseException as exc:
|
||||
publisher_error.append(exc)
|
||||
with self._lock:
|
||||
closed = self._closed
|
||||
if not closed:
|
||||
publisher_error.append(exc)
|
||||
else:
|
||||
publisher_aborted.set()
|
||||
publisher_ready.set()
|
||||
self._stop_event.set()
|
||||
finally:
|
||||
if bridge is not None:
|
||||
bridge.close()
|
||||
with self._lock:
|
||||
close_bridge = self._closed and self._bridge is bridge
|
||||
if close_bridge:
|
||||
self._bridge = None
|
||||
self._rerun_grpc_url = None
|
||||
if close_bridge:
|
||||
bridge.close()
|
||||
|
||||
publisher = threading.Thread(target=publish, name="k1-foxglove-publisher", daemon=True)
|
||||
def join_publisher() -> None:
|
||||
# Never orphan a publisher: the session thread remains its owner.
|
||||
# On process shutdown both are daemon threads, so an irrecoverably
|
||||
# blocked native call cannot prevent the operating system from exit.
|
||||
while publisher.is_alive():
|
||||
publisher.join(timeout=0.25)
|
||||
|
||||
publisher = threading.Thread(target=publish, name="k1-rerun-publisher", daemon=True)
|
||||
publisher.start()
|
||||
if not publisher_ready.wait(timeout=15.0):
|
||||
self._finish_error("Локальный мост визуализации не запустился за 15 секунд.")
|
||||
source_done.set()
|
||||
self._stop_event.set()
|
||||
join_publisher()
|
||||
return
|
||||
if publisher_aborted.is_set():
|
||||
source_done.set()
|
||||
self._stop_event.set()
|
||||
join_publisher()
|
||||
return
|
||||
if publisher_error:
|
||||
self._finish_error(
|
||||
@@ -288,6 +389,7 @@ class VisualizationRuntime:
|
||||
f"{type(publisher_error[0]).__name__}: {publisher_error[0]}"
|
||||
)
|
||||
source_done.set()
|
||||
join_publisher()
|
||||
return
|
||||
|
||||
final_message = "Поток остановлен."
|
||||
@@ -299,11 +401,8 @@ class VisualizationRuntime:
|
||||
self._finish_error(f"Ошибка источника: {type(exc).__name__}: {exc}")
|
||||
finally:
|
||||
source_done.set()
|
||||
publisher.join(timeout=15.0)
|
||||
if publisher.is_alive():
|
||||
self._stop_event.set()
|
||||
self._finish_error("Очередь публикации не завершилась за 15 секунд.")
|
||||
elif publisher_error:
|
||||
join_publisher()
|
||||
if publisher_error:
|
||||
self._finish_error(
|
||||
f"Ошибка публикации: {type(publisher_error[0]).__name__}: {publisher_error[0]}"
|
||||
)
|
||||
@@ -334,6 +433,10 @@ class VisualizationRuntime:
|
||||
self._foxglove_viewer_url = None
|
||||
self._notify()
|
||||
|
||||
def _current_scene_settings(self) -> RerunSceneSettings:
|
||||
with self._lock:
|
||||
return self._scene_settings
|
||||
|
||||
def _notify(self) -> None:
|
||||
callback = self._on_state_change
|
||||
if callback is not None:
|
||||
@@ -360,7 +463,7 @@ def _write_live_session_preamble(out_dir: Path, host: str, duration_seconds: flo
|
||||
"schema_version": 1,
|
||||
"started_at_utc": started_at_utc,
|
||||
"started_monotonic_ns": time.monotonic_ns(),
|
||||
"operation": "k1_live_mqtt_to_foxglove",
|
||||
"operation": "k1_live_mqtt_to_rerun",
|
||||
"target": "owner-controlled K1 at redacted RFC1918 address",
|
||||
"requested_duration_seconds": duration_seconds,
|
||||
"raw_capture": "captures/mqtt_live/mqtt.raw.k1mqtt",
|
||||
@@ -370,7 +473,7 @@ def _write_live_session_preamble(out_dir: Path, host: str, duration_seconds: flo
|
||||
notes = (
|
||||
"# K1 live visualization session\n\n"
|
||||
f"Started UTC: {started_at_utc}\n\n"
|
||||
"The local control API started a read-only MQTT subscription and Foxglove "
|
||||
"The local control API started a read-only MQTT subscription and Rerun "
|
||||
"preview. Raw MQTT evidence is written before preview decoding. The target "
|
||||
f"was a validated private IPv4 address ({host.rsplit('.', 1)[0]}.x).\n"
|
||||
)
|
||||
|
||||
+48
-2
@@ -2,10 +2,11 @@ from __future__ import annotations
|
||||
|
||||
import asyncio
|
||||
import threading
|
||||
from collections.abc import Mapping
|
||||
from collections.abc import AsyncIterator, Mapping
|
||||
from contextlib import asynccontextmanager
|
||||
from datetime import UTC, datetime
|
||||
from pathlib import Path
|
||||
from typing import Any
|
||||
from typing import Any, Literal
|
||||
|
||||
from bleak.exc import BleakError
|
||||
from fastapi import FastAPI, HTTPException, WebSocket, WebSocketDisconnect
|
||||
@@ -17,6 +18,7 @@ from k1link.artifacts import write_json_atomic
|
||||
from k1link.ble.scanner import scan
|
||||
from k1link.ble.wifi_provisioning import AP_FALLBACK_IPV4, provision_wifi_once
|
||||
from k1link.mqtt import validate_private_ipv4
|
||||
from k1link.viewer.rerun_bridge import RerunSceneSettings
|
||||
from k1link.viewer.runtime import VisualizationRuntime, new_live_session_dir
|
||||
|
||||
REPOSITORY_ROOT = Path(__file__).resolve().parents[3]
|
||||
@@ -43,6 +45,17 @@ class ReplayRequest(BaseModel):
|
||||
loop: bool = False
|
||||
|
||||
|
||||
class ViewerSettingsRequest(BaseModel):
|
||||
point_size: float = Field(default=2.5, ge=0.5, le=12.0)
|
||||
color_mode: Literal["intensity", "height", "distance", "rgb", "class"] = "intensity"
|
||||
palette: Literal["turbo", "viridis", "plasma", "grayscale", "custom"] = "turbo"
|
||||
custom_color: str = Field(default="#f7f8f4", pattern=r"^#[0-9A-Fa-f]{6}$")
|
||||
accumulation_seconds: float = Field(default=12.0, ge=0.0, le=120.0)
|
||||
show_points: bool = True
|
||||
show_trajectory: bool = True
|
||||
show_grid: bool = True
|
||||
|
||||
|
||||
class ConsoleService:
|
||||
def __init__(self, repository_root: Path) -> None:
|
||||
self.repository_root = repository_root.resolve()
|
||||
@@ -100,6 +113,8 @@ class ConsoleService:
|
||||
"k1_ip": k1_ip,
|
||||
"foxglove_ws_url": runtime["foxglove_ws_url"],
|
||||
"foxglove_viewer_url": runtime["foxglove_viewer_url"],
|
||||
"rerun_grpc_url": runtime["rerun_grpc_url"],
|
||||
"viewer_settings": runtime["viewer_settings"],
|
||||
"source_mode": runtime["source_mode"],
|
||||
"metrics": {
|
||||
"pipeline_ms": metrics["mqtt_to_publish_ms"],
|
||||
@@ -213,6 +228,21 @@ class ConsoleService:
|
||||
self.runtime.stop()
|
||||
return self.state()
|
||||
|
||||
def update_viewer_settings(self, request: ViewerSettingsRequest) -> dict[str, Any]:
|
||||
self.runtime.update_scene_settings(
|
||||
RerunSceneSettings(
|
||||
point_size=request.point_size,
|
||||
color_mode=request.color_mode,
|
||||
palette=request.palette,
|
||||
custom_color=request.custom_color,
|
||||
accumulation_seconds=request.accumulation_seconds,
|
||||
show_points=request.show_points,
|
||||
show_trajectory=request.show_trajectory,
|
||||
show_grid=request.show_grid,
|
||||
)
|
||||
)
|
||||
return self.state()
|
||||
|
||||
def _set_operation(self, phase: str, message: str) -> None:
|
||||
with self._lock:
|
||||
self._operation_phase = phase
|
||||
@@ -220,12 +250,23 @@ class ConsoleService:
|
||||
|
||||
|
||||
service = ConsoleService(REPOSITORY_ROOT)
|
||||
|
||||
|
||||
@asynccontextmanager
|
||||
async def app_lifespan(_: FastAPI) -> AsyncIterator[None]:
|
||||
try:
|
||||
yield
|
||||
finally:
|
||||
service.runtime.close()
|
||||
|
||||
|
||||
app = FastAPI(
|
||||
title="NODE.DC Device Control API",
|
||||
version=__version__,
|
||||
docs_url="/api/docs",
|
||||
redoc_url=None,
|
||||
openapi_url="/api/openapi.json",
|
||||
lifespan=app_lifespan,
|
||||
)
|
||||
|
||||
|
||||
@@ -284,6 +325,11 @@ def stop_session() -> dict[str, Any]:
|
||||
return service.stop()
|
||||
|
||||
|
||||
@app.post("/api/viewer/settings")
|
||||
def update_viewer_settings(request: ViewerSettingsRequest) -> dict[str, Any]:
|
||||
return service.update_viewer_settings(request)
|
||||
|
||||
|
||||
@app.websocket("/api/events")
|
||||
async def events(websocket: WebSocket) -> None:
|
||||
await websocket.accept()
|
||||
|
||||
Reference in New Issue
Block a user