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

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

Browse Source
This commit is contained in:
DCCONSTRUCTIONS
2026-07-16 19:44:06 +03:00
parent 19ab973110
commit e6f7648b84
45 changed files with 6401 additions and 440 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/k1link/viewer/rerun_bridge.py
+43 -90
View File
@@ -11,19 +11,12 @@ 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.data_plane import (
DecodedDataPlaneView,
DecodedPointCloudView,
DecodedPoseView,
)
from k1link.viewer.foxglove_bridge import BridgeMetrics
from k1link.viewer.messages import StreamMessage
from k1link.viewer.metrics import BridgeMetrics
PointColorMode = Literal["intensity", "height", "distance", "rgb", "class"]
PointPalette = Literal["turbo", "viridis", "plasma", "grayscale", "custom"]
@@ -59,7 +52,7 @@ SettingsProvider = Callable[[], RerunSceneSettings]
class RerunBridge:
"""Decode verified device topics into a self-hosted Rerun recording stream."""
"""Publish transport-neutral canonical envelopes to a Rerun recording."""
def __init__(
self,
@@ -73,9 +66,7 @@ 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"
)
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,
@@ -95,9 +86,7 @@ class RerunBridge:
rr.TransformAxes3D(axis_length=0.45, show_frame=True),
static=True,
)
self._path: deque[tuple[float, float, float]] = deque(
maxlen=MAX_TRAJECTORY_POSES
)
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
@@ -130,33 +119,23 @@ class RerunBridge:
make_default=True,
)
def process(self, message: StreamMessage) -> None:
started_ns = time.monotonic_ns()
self.metrics.received(len(message.payload))
def process(self, envelope: DecodedDataPlaneView) -> None:
self._apply_latest_settings()
self._set_message_time(message)
self._set_message_time(envelope)
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()
if isinstance(envelope, DecodedPointCloudView):
self._publish_points(envelope)
point_frame = True
elif isinstance(envelope, DecodedPoseView):
self._publish_pose(envelope)
point_frame = False
else:
return
published_ns = time.monotonic_ns()
decode_publish_ms = (published_ns - started_ns) / 1_000_000
decode_publish_ms = (
published_ns - envelope.context.processing_started_monotonic_ns
) / 1_000_000
if point_frame:
self.metrics.published_pcl(
self._last_point_count,
@@ -169,10 +148,9 @@ class RerunBridge:
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
)
context = envelope.context
if context.live and context.received_monotonic_ns is not None:
self.metrics.record_latency((published_ns - context.received_monotonic_ns) / 1_000_000)
def close(self) -> None:
if self._closed:
@@ -187,13 +165,14 @@ class RerunBridge:
# instead of waiting for the publisher thread frame to be collected.
del self._recording
def _set_message_time(self, message: StreamMessage) -> None:
def _set_message_time(self, envelope: DecodedDataPlaneView) -> None:
context = envelope.context
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,
timestamp=context.captured_at_epoch_ns / 1_000_000_000,
)
self._recording.set_time("message_sequence", sequence=message.sequence)
self._recording.set_time("message_sequence", sequence=context.sequence)
def _apply_latest_settings(self) -> None:
settings = self._settings_provider()
@@ -202,34 +181,17 @@ class RerunBridge:
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:
def _publish_points(self, frame: DecodedPointCloudView) -> None:
positions = np.asarray(frame.positions_xyz, dtype=np.float32).reshape((-1, 3))
if frame.intensities is None:
intensities = np.full(frame.point_count, 255, dtype=np.uint8)
else:
intensities = np.frombuffer(frame.intensities, dtype=np.uint8)
rgb = (
None
if frame.colors_rgb is None
else np.frombuffer(frame.colors_rgb, dtype=np.uint8).reshape((-1, 3))
)
self._last_point_count = int(positions.shape[0])
if not self._settings.show_points:
self._recording.log("/world/points", rr.Clear(recursive=False))
@@ -244,25 +206,15 @@ class RerunBridge:
),
)
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:
def _publish_pose(self, frame: DecodedPoseView) -> None:
self._recording.log(
"/world/sensor_pose",
rr.Transform3D(
translation=position_xyz,
quaternion=rr.Quaternion(xyzw=orientation_xyzw),
translation=frame.position_xyz,
quaternion=rr.Quaternion(xyzw=frame.orientation_xyzw),
),
)
self._path.append(position_xyz)
self._path.append(frame.position_xyz)
if not self._settings.show_trajectory:
self._recording.log("/world/trajectory", rr.Clear(recursive=False))
return
@@ -283,6 +235,7 @@ class RerunBridge:
),
)
def _blueprint(settings: RerunSceneSettings) -> rrb.Blueprint:
accumulation = max(0.0, settings.accumulation_seconds)
time_range = rr.VisibleTimeRange(