Ivan/feat/go2zenoh

dimos/core/transport_factory.py

@@ -57,9 +57,9 @@ def transport_topic(name: str, g: GlobalConfig = global_config) -> str:
 # High-rate sensor streams: drop stale frames under congestion, never stall the
 # publisher. Matched by message type since that is what makes them high-rate.
 _LATEST_WINS_TYPES = ("sensor_msgs.Image", "sensor_msgs.PointCloud2")
-# Agent/human conversation channels: low-rate, and a dropped message loses a
-# whole turn of conversation.
-_NEVER_DROP_CHANNELS = ("human_input", "agent", "agent_idle")
+# Low-rate channels where a drop loses something that never comes back: a whole
+# turn of agent/human conversation, or a one-shot robot action verb.
+_NEVER_DROP_CHANNELS = ("human_input", "agent", "agent_idle", "command")
 
 
 def default_zenoh_qos(name: str, msg_type: type | None = None) -> ZenohQoS | None:

dimos/mapping/ray_tracing/rust/flake.nix

@@ -34,7 +34,7 @@
           cargoRoot = "dimos/mapping/ray_tracing/rust";
           buildAndTestSubdir = "dimos/mapping/ray_tracing/rust";
 
-          cargoHash = "sha256-0d0dlNDvDplA7oWTyUWOCOlS74Zie8uMQ+ps6lXntOI=";
+          cargoHash = "sha256-6a8GHRSKI6mjg9HNbrestCud8xZtF8HaD0bWVMbl7N8=";
 
           meta.mainProgram = "voxel_ray_tracing";
         };

dimos/msgs/foxglove_msgs/CompressedVideo.py

@@ -0,0 +1,107 @@
+# Copyright 2026 Dimensional Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+"""foxglove_msgs::msg::CompressedVideo — one encoded video packet.
+
+Just the encoded bytes + codec name. Inter-frame codecs (h264) can't be decoded
+one packet at a time — feed the ordered stream through
+:class:`~dimos.robot.unitree.go2.dds.video.H264Decoder` to get ``Image`` frames,
+or let the rerun bridge log it as a ``VideoStream`` and decode in-viewer.
+"""
+
+from __future__ import annotations
+
+import time
+from typing import TYPE_CHECKING
+
+from dimos_lcm.foxglove_msgs import CompressedVideo as LCMCompressedVideo
+import numpy as np
+
+from dimos.types.timestamped import Timestamped
+
+if TYPE_CHECKING:
+    from dimos.visualization.rerun.bridge import RerunData
+
+
+class CompressedVideo(Timestamped):
+    """One encoded video packet (Annex-B for h264)."""
+
+    msg_name = "foxglove_msgs.CompressedVideo"
+
+    def __init__(
+        self,
+        data: np.ndarray | bytes,
+        format: str = "h264",
+        frame_id: str = "",
+        ts: float | None = None,
+    ) -> None:
+        """Initialize a CompressedVideo.
+
+        Args:
+            data: The encoded packet, u8
+            format: Codec name, e.g. "h264"
+            frame_id: Frame ID
+            ts: Timestamp in seconds
+        """
+        self.data = data if isinstance(data, np.ndarray) else np.frombuffer(data, dtype=np.uint8)
+        self.format = format
+        self.frame_id = frame_id
+        self.ts = ts if ts is not None else time.time()
+
+    def lcm_encode(self) -> bytes:
+        msg = LCMCompressedVideo()
+        msg.timestamp.sec = int(self.ts)
+        msg.timestamp.nanosec = int((self.ts - int(self.ts)) * 1e9)
+        msg.frame_id = self.frame_id
+        msg.data = self.data.tobytes()
+        msg.data_length = len(msg.data)
+        msg.format = self.format
+        return msg.lcm_encode()  # type: ignore[no-any-return]
+
+    @classmethod
+    def lcm_decode(cls, data: bytes) -> CompressedVideo:
+        msg = LCMCompressedVideo.lcm_decode(data)
+        return cls(
+            data=bytes(msg.data),
+            format=msg.format,
+            frame_id=msg.frame_id,
+            ts=msg.timestamp.sec + msg.timestamp.nanosec * 1e-9,
+        )
+
+    def to_rerun(self) -> RerunData:
+        """Log the encoded packet as a rerun ``VideoStream`` sample (viewer decodes).
+
+        rerun decodes the stream in-viewer, so this stays per-packet and cheap —
+        no server-side decode, and the .rrd holds the compressed bytes. Iterate
+        from the start (or a keyframe) so the first sample the viewer sees is one.
+        """
+        import rerun as rr
+
+        codecs = {
+            "h264": rr.VideoCodec.H264,
+            "h265": rr.VideoCodec.H265,
+            "av1": rr.VideoCodec.AV1,
+            "vp8": rr.VideoCodec.VP8,
+            "vp9": rr.VideoCodec.VP9,
+        }
+        codec = codecs.get(self.format.lower())
+        if codec is None:
+            raise ValueError(f"no rerun VideoCodec for format {self.format!r}")
+        return rr.VideoStream(codec, sample=self.data.tobytes())
+
+    def __repr__(self) -> str:
+        return (
+            f"CompressedVideo(format='{self.format}', bytes={self.data.size}, "
+            f"frame_id='{self.frame_id}', ts={self.ts})"
+        )

dimos/msgs/foxglove_msgs/test_CompressedVideo.py

@@ -0,0 +1,48 @@
+# Copyright 2026 Dimensional Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import pytest
+import rerun as rr
+
+from dimos.msgs.foxglove_msgs.CompressedVideo import CompressedVideo
+from dimos.msgs.helpers import resolve_msg_type
+
+PACKET = b"\x00\x00\x00\x01\x65payload"
+
+
+def test_lcm_encode_decode() -> None:
+    original = CompressedVideo(data=PACKET, format="h264", frame_id="front_camera", ts=1234567890.5)
+    decoded = CompressedVideo.lcm_decode(original.lcm_encode())
+
+    assert decoded.data.tobytes() == PACKET
+    assert decoded.format == "h264"
+    assert decoded.frame_id == "front_camera"
+    assert abs(decoded.ts - original.ts) < 1e-6
+
+
+def test_resolves_over_the_raw_lcm_type() -> None:
+    """The rerun bridge decodes by type name — it must land on the to_rerun() class."""
+    resolved = resolve_msg_type("foxglove_msgs.CompressedVideo")
+    assert resolved is CompressedVideo
+
+
+def test_to_rerun_video_stream() -> None:
+    stream = CompressedVideo(data=PACKET, format="h264").to_rerun()
+    assert isinstance(stream, rr.VideoStream)
+    assert bytes(stream.sample.as_arrow_array().to_pylist()[0]) == PACKET  # type: ignore[union-attr]
+
+
+def test_to_rerun_unknown_codec() -> None:
+    with pytest.raises(ValueError, match="mjpeg"):
+        CompressedVideo(data=PACKET, format="mjpeg").to_rerun()

dimos/msgs/std_msgs/String.py

@@ -0,0 +1,30 @@
+#!/usr/bin/env python3
+# Copyright 2025-2026 Dimensional Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+"""String message type."""
+
+from typing import ClassVar
+
+from dimos_lcm.std_msgs import String as LCMString
+
+
+class String(LCMString):  # type: ignore[misc]
+    """ROS-compatible String message."""
+
+    msg_name: ClassVar[str] = "std_msgs.String"
+
+    def __init__(self, data: str = "") -> None:
+        """Initialize String with data value."""
+        self.data = data

dimos/navigation/nav_3d/evaluator/blueprints.py

@@ -22,25 +22,26 @@
 
 from __future__ import annotations
 
-import numpy as np
 import rerun as rr
 from rerun._baseclasses import Archetype
 
 from dimos.core.coordination.blueprints import autoconnect
 from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped
-from dimos.msgs.nav_msgs.LineSegments3D import LineSegments3D
 from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2
 from dimos.navigation.cmu_nav.modules.click_start_goal_router.click_start_goal_router import (
     ClickStartGoalRouter,
 )
 from dimos.navigation.nav_3d.evaluator.evaluator import Evaluator
 from dimos.navigation.nav_3d.mls_planner.mls_planner_native import MLSPlannerNative
+from dimos.navigation.nav_3d.mls_planner.viz import (
+    render_node_edges,
+    render_nodes,
+    render_surface_map,
+)
 from dimos.visualization.rerun.bridge import RerunBridgeModule
 from dimos.visualization.rerun.websocket_server import RerunWebSocketServer
 
 _POSE_MARKER_RADIUS = 0.4
-# Small lift so graph artifacts render visibly above the surface points instead of z-fighting.
-_GRAPH_Z_LIFT = 0.05
 
 
 def _render_start_pose(msg: PoseStamped) -> Archetype:
@@ -63,42 +64,6 @@ def _render_global_map(msg: PointCloud2) -> Archetype:
     return msg.to_rerun(voxel_size=0.03, colors=[128, 128, 128])
 
 
-def _render_surface_map(msg: PointCloud2) -> Archetype:
-    return msg.to_rerun(voxel_size=0.1, colors=[40, 75, 130])
-
-
-def _render_nodes(msg: PointCloud2) -> Archetype:
-    pts, _ = msg.as_numpy()
-    if pts is None or len(pts) == 0:
-        return rr.Points3D([])
-    pts = pts.copy()
-    pts[:, 2] += _GRAPH_Z_LIFT
-    return rr.Points3D(positions=pts, colors=[[75, 156, 211]], radii=[0.15])
-
-
-def _render_node_edges(msg: LineSegments3D) -> Archetype:
-    """Color each segment by its safe-adj weight on a log-scale green->red gradient."""
-    if not msg._segments:
-        return rr.LineStrips3D([])
-    weights = np.asarray(msg._traversability, dtype=np.float64)
-    log_w = np.log10(np.maximum(weights, 1e-6))
-    lo, hi = float(log_w.min()), float(log_w.max())
-    norm = (log_w - lo) / (hi - lo) if hi > lo else np.zeros_like(log_w)
-    r = (255 * norm).astype(np.uint8)
-    g = (255 * (1.0 - norm)).astype(np.uint8)
-    b = np.full_like(r, 60)
-    a = np.full_like(r, 220)
-    colors = np.column_stack([r, g, b, a])
-    strips = [
-        [
-            [p1[0], p1[1], p1[2] + _GRAPH_Z_LIFT],
-            [p2[0], p2[1], p2[2] + _GRAPH_Z_LIFT],
-        ]
-        for p1, p2 in msg._segments
-    ]
-    return rr.LineStrips3D(strips, colors=colors, radii=[0.04] * len(strips))
-
-
 path_planner_eval = autoconnect(
     Evaluator.blueprint(),
     MLSPlannerNative.blueprint(),
@@ -109,9 +74,10 @@ def _render_node_edges(msg: LineSegments3D) -> Archetype:
             "world/start_pose": _render_start_pose,
             "world/goal_pose": _render_goal_pose,
             "world/global_map": _render_global_map,
-            "world/surface_map": _render_surface_map,
-            "world/nodes": _render_nodes,
-            "world/node_edges": _render_node_edges,
+            # The evaluator exists to inspect the planner, so these are always on.
+            "world/surface_map": render_surface_map,
+            "world/nodes": render_nodes,
+            "world/node_edges": render_node_edges,
         }
     ),
 )

dimos/navigation/nav_3d/mls_planner/rust/src/main.rs

@@ -28,6 +28,7 @@ use tracing::debug;
 
 /// A point in the planner's world frame.
 type Xyz = (f32, f32, f32);
+type Xyzi = (f32, f32, f32, f32);
 
 /// State shared between the handle loop and the worker.
 type Shared<T> = Arc<Mutex<Option<T>>>;
@@ -296,11 +297,15 @@ impl Worker {
         let frame = &self.config.world_frame;
         let graph = planner.graph();
 
-        let surface_points: Vec<Xyz> = planner
-            .surface()
-            .map(|(ix, iy, iz)| surface_point_xyz(ix, iy, iz, voxel_size))
+        let surface_points: Vec<Xyzi> = planner
+            .surface_clearance()
+            .into_iter()
+            .map(|((ix, iy, iz), clearance)| {
+                let (x, y, z) = surface_point_xyz(ix, iy, iz, voxel_size);
+                (x, y, z, clearance)
+            })
             .collect();
-        let surface = build_pc2_xyz(&surface_points, frame, now());
+        let surface = build_pc2_xyzi(&surface_points, frame, now());
 
         let node_points: Vec<Xyz> = graph.nodes.iter().map(|n| n.pos).collect();
         let node_cloud = build_pc2_xyz(&node_points, frame, now());
@@ -458,6 +463,40 @@ fn build_segments_path(plg: &PlannerGraph, voxel_size: f32, frame_id: &str, stam
     }
 }
 
+/// Like `build_pc2_xyz` plus an `intensity` float carrying the cell's wall clearance.
+fn build_pc2_xyzi(points: &[Xyzi], frame_id: &str, stamp: Time) -> PointCloud2 {
+    let n = points.len() as i32;
+    let mut data = Vec::with_capacity(points.len() * 16);
+    for &(x, y, z, i) in points {
+        data.extend_from_slice(&x.to_le_bytes());
+        data.extend_from_slice(&y.to_le_bytes());
+        data.extend_from_slice(&z.to_le_bytes());
+        data.extend_from_slice(&i.to_le_bytes());
+    }
+    let make_field = |name: &str, off: i32| PointField {
+        name: name.into(),
+        offset: off,
+        datatype: PointField::FLOAT32 as u8,
+        count: 1,
+    };
+    PointCloud2 {
+        header: header(frame_id, stamp),
+        height: 1,
+        width: n,
+        fields: vec![
+            make_field("x", 0),
+            make_field("y", 4),
+            make_field("z", 8),
+            make_field("intensity", 12),
+        ],
+        is_bigendian: false,
+        point_step: 16,
+        row_step: 16 * n,
+        data,
+        is_dense: true,
+    }
+}
+
 fn build_pc2_xyz(points: &[(f32, f32, f32)], frame_id: &str, stamp: Time) -> PointCloud2 {
     let n = points.len() as i32;
     let mut data = Vec::with_capacity(points.len() * 12);