From c6ea15ff90e8fcd173ceb6bd36f376437e2264d5 Mon Sep 17 00:00:00 2001 From: Ivan Nikolic Date: Wed, 22 Jul 2026 18:00:51 +0300 Subject: [PATCH 01/16] quick zenoh go2 implementation --- dimos/robot/all_blueprints.py | 2 + dimos/robot/unitree/go2/zenoh/blueprints.py | 153 ++++++++++++++++++ .../unitree/go2/zenoh/zenohconnection.py | 135 ++++++++++++++++ 3 files changed, 290 insertions(+) create mode 100644 dimos/robot/unitree/go2/zenoh/blueprints.py create mode 100644 dimos/robot/unitree/go2/zenoh/zenohconnection.py diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 613917a7bf4..ca1733f2de9 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -134,6 +134,7 @@ "unitree-go2-multi": "dimos.robot.unitree.go2.blueprints.basic.unitree_go2_multi:unitree_go2_multi", "unitree-go2-multi-teleop": "dimos.robot.unitree.go2.blueprints.basic.unitree_go2_multi_teleop:unitree_go2_multi_teleop", "unitree-go2-nav-3d": "dimos.robot.unitree.go2.blueprints.navigation.unitree_go2_nav_3d:unitree_go2_nav_3d", + "unitree-go2-nav-3d-zenoh": "dimos.robot.unitree.go2.zenoh.blueprints:unitree_go2_nav_3d_zenoh", "unitree-go2-relocalization": "dimos.robot.unitree.go2.blueprints.smart.unitree_go2:unitree_go2_relocalization", "unitree-go2-ros": "dimos.robot.unitree.go2.blueprints.smart.unitree_go2_ros:unitree_go2_ros", "unitree-go2-security": "dimos.robot.unitree.go2.blueprints.agentic.unitree_go2_security:unitree_go2_security", @@ -201,6 +202,7 @@ "go2-mid360-recorder": "dimos.robot.unitree.go2.go2_mid360_recorder.Go2Mid360Recorder", "go2-mid360-static-tf": "dimos.robot.unitree.go2.go2_mid360_static_transforms.Go2Mid360StaticTf", "go2-teleop-module": "dimos.teleop.quest.quest_extensions.Go2TeleopModule", + "go2-zenoh": "dimos.robot.unitree.go2.zenoh.zenohconnection.GO2Zenoh", "goal-relay": "dimos.navigation.nav_3d.mls_planner.goal_relay.GoalRelay", "google-maps-skill-container": "dimos.agents.skills.google_maps_skill_container.GoogleMapsSkillContainer", "gps-nav-skill-container": "dimos.agents.skills.gps_nav_skill.GpsNavSkillContainer", diff --git a/dimos/robot/unitree/go2/zenoh/blueprints.py b/dimos/robot/unitree/go2/zenoh/blueprints.py new file mode 100644 index 00000000000..fa3d03a2fa3 --- /dev/null +++ b/dimos/robot/unitree/go2/zenoh/blueprints.py @@ -0,0 +1,153 @@ +#!/usr/bin/env python3 +# 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. + +"""3d navigation on a Go2 that runs the go2web zenoh bridge. + +The same nav stack as ``unitree_go2_nav_3d``, minus the two modules the robot now runs +itself: no WebRTC ``GO2Connection`` and no local ``PointLio`` — odom, lidar and video +arrive over zenoh already (see :class:`GO2Zenoh`), which is why the graph runs on the +zenoh transport. +""" + +import math +from typing import Any + +from dimos.core.coordination.blueprints import autoconnect +from dimos.core.global_config import global_config +from dimos.mapping.ray_tracing.module import RayTracingVoxelMap +from dimos.msgs.geometry_msgs.Quaternion import Quaternion +from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.navigation.movement_manager.movement_manager import MovementManager +from dimos.robot.unitree.go2.zenoh.zenohconnection import GO2Zenoh +from dimos.visualization.vis_module import vis_module + +voxel_size = 0.08 + +# The lidar mount rotation on this rig (fixed-axis rpy, degrees), feeding both the static +# tf GO2Zenoh publishes and the rotation that levels its odometry — they must agree or nav +# steers off-heading. 60 deg of tilt, carried on roll because the lidar sits yawed 90 deg +# on its bracket; verified against Point-LIO's own attitude on the standing robot. +MID360_MOUNT_RPY_DEG = (-60.0, 0.0, -90.0) + + +def _mount_rotation() -> list[float]: + """base_link <- lidar rotation, so nav reads odometry in the level body frame. + + base_link -> front_camera carries no rotation, so the composed base_link -> mid360_link + rotation is just the mount rpy above. + """ + rpy = Vector3(*(math.radians(d) for d in MID360_MOUNT_RPY_DEG)) + return list(Quaternion.from_euler(rpy).to_tuple()) + + +def _camera_info_to_pinhole(camera_info: Any) -> Any: + """Log the pinhole onto the video's entity instead of camera_info's own. + + The rerun bridge names entities after topics, so camera_info and video land on + sibling paths — and a Pinhole only projects the entity it lives on and that entity's + children, which is why the frustum draws but stays empty. No ``optical_frame`` here: + the video carries frame_id ``camera_optical``, so the bridge already anchors that + entity to the tf frame, and a second parent would be rejected. + """ + return camera_info.to_rerun(image_topic="world/video") + + +def _rerun_blueprint() -> Any: + """Split layout: camera feed + 3D world, as the WebRTC go2 blueprint has. + + The 2D view sits on ``world/video`` rather than ``world/color_image`` — over zenoh the + camera arrives as an H.264 ``CompressedVideo`` on the ``video`` port, decoded in the + viewer, and that is also where the pinhole is logged. + """ + import rerun as rr + import rerun.blueprint as rrb + + return rrb.Blueprint( + rrb.Horizontal( + rrb.Spatial2DView(origin="world/video", name="Camera"), + rrb.Spatial3DView( + origin="world", + name="3D", + background=rrb.Background(kind="SolidColor", color=[0, 0, 0]), + line_grid=rrb.LineGrid3D(plane=rr.components.Plane3D.XY.with_distance(0.5)), + ), + column_shares=[1, 2], + ), + rrb.TimePanel(state="hidden"), + rrb.SelectionPanel(state="hidden"), + ) + + +def _convert_map(grid: PointCloud2) -> Any: + return grid.to_rerun(voxel_size=0.04) + + +def _convert_scan(grid: PointCloud2) -> Any: + return grid.to_rerun(color=[255, 0, 0], voxel_size=0.05) + + +_rerun_config = { + "blueprint": _rerun_blueprint, + "visual_override": { + "world/camera_info": _camera_info_to_pinhole, + "world/pointlio_map": _convert_map, + "world/lidar": _convert_scan, + }, + # The clouds are what costs: the world map runs up to 300k points x 16 B per message + # at ~2 Hz, the per-scan lidar ~10 Hz. Deliberately no entry for world/video — the + # throttle drops whole messages, and dropping H.264 samples breaks decoding until the + # next keyframe, so the camera has to be slowed at the source instead. (0 here would + # mean unlimited, not off.) + "max_hz": { + # "world/lidar": 2.0, + # "world/pointlio_map": 0.5, + }, +} + +unitree_go2_nav_3d_zenoh = autoconnect( + vis_module(viewer_backend=global_config.viewer, rerun_config=_rerun_config), + GO2Zenoh.blueprint(mid360_mount_rpy_deg=MID360_MOUNT_RPY_DEG), + # Level the tilted-sensor odometry into the body frame so the follower steers on a + # true heading. The ray tracer keeps the raw sensor odometry. + # # OdomBodyFrame.blueprint(mount_rotation=_mount_rotation()), + RayTracingVoxelMap.blueprint( + voxel_size=voxel_size, + emit_every=1, + global_emit_every=50, + min_health=-1, + max_health=5, + support_min=4, + ), + # # global_map is remapped off so the planner runs purely on the + # # incremental local_map + region_bounds pair. + # MLSPlannerNative.blueprint( + # world_frame="odom", + # voxel_size=voxel_size, + # robot_height=0.3, + # surface_closing_radius=0.3, + # wall_clearance_m=0.1, + # wall_buffer_m=0.75, + # wall_buffer_weight=100.0, + # step_threshold_m=0.16, + # step_penalty_weight=4.0, + # viz_publish_hz=0.0, + # ).remappings([(MLSPlannerNative, "global_map", "global_map_unused")]), + # # GoalRelay.blueprint(), + # # BasicPathFollower.blueprint(speed=0.5, heading_gain=0.4, max_angular=0.6).remappings( + # # [(BasicPathFollower, "odometry", "body_odometry")] + # # ), + MovementManager.blueprint(), +).global_config(transport="zenoh", n_workers=8, robot_model="unitree_go2") diff --git a/dimos/robot/unitree/go2/zenoh/zenohconnection.py b/dimos/robot/unitree/go2/zenoh/zenohconnection.py new file mode 100644 index 00000000000..d76a813003e --- /dev/null +++ b/dimos/robot/unitree/go2/zenoh/zenohconnection.py @@ -0,0 +1,135 @@ +# 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. + +"""The Go2 as it appears on the graph when it runs the go2web zenoh bridge. + +The bridge (go2web ``src/dimos_zenoh.rs``, on by default via the ``dimos_zenoh`` cargo +feature) publishes Point-LIO odom, the per-scan and world clouds and H.264 video +straight onto dimos's own zenoh keys, and consumes ``cmd_vel``. Nothing +here produces any of it — declaring the ports is what puts the robot's topics on the +graph, the same arrangement as a NativeModule whose binary publishes out of process (see +:class:`PointLio`). The port names are the wire contract: the bridge's keys are +``dimos//``, so renaming a port here silently disconnects it. Remap in +the blueprint instead. + +What the bridge does not send, and this module supplies: the ``odom -> mid360_link`` tf +edge, the static mount tree, and the camera intrinsics. +""" + +from __future__ import annotations + +import asyncio +import math +import time + +from pydantic import Field +from reactivex.disposable import Disposable + +from dimos.core.core import rpc +from dimos.core.stream import In, Out +from dimos.msgs.foxglove_msgs.CompressedVideo import CompressedVideo +from dimos.msgs.geometry_msgs.Quaternion import Quaternion +from dimos.msgs.geometry_msgs.Transform import Transform +from dimos.msgs.geometry_msgs.Twist import Twist +from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.msgs.nav_msgs.Odometry import Odometry +from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo +from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.protocol.tf.static_tf_publisher import StaticTfPublisher, StaticTfPublisherConfig +from dimos.robot.unitree.go2.connection import _camera_info_static + +# Mount geometry, measured on this rig (metres). Kept here rather than shared with +# go2_mid360_static_transforms: that describes the recording rig, which carries the +# lidar at a different angle and hangs its tree off base_link. +CAMERA_XYZ = Vector3(0.32715, -0.00003, 0.04297) # base_link -> front_camera +MID360_XYZ = Vector3(-0.032, 0.0, 0.12) # front_camera -> mid360_link: 3.2cm back, 12cm up +# rpy mapping a sensor frame to its optical frame (x-right, y-down, z-forward) +OPTICAL_RPY = Vector3(-math.pi / 2, 0.0, -math.pi / 2) + + +class GO2ZenohConfig(StaticTfPublisherConfig): + # Mid-360 mount rotation on the front_camera -> mid360_link edge: fixed-axis rpy, + # degrees. The lidar is yawed 90 deg on its bracket, so its 60 deg downward tilt + # lands on roll about the lidar's own x-axis rather than pitch — Point-LIO reports + # (-60.7, -1.4, +0.8) for this rig standing still, the 1.4 being the dog's stance. + # Both yaw signs level the body; they differ by 180 deg of heading, so flip the sign + # if the camera ends up looking backwards. + mid360_mount_rpy_deg: tuple[float, float, float] = (-60.0, 0.0, -90.0) + camera_info_hz: float = Field(default=1.0, gt=0.0) + + +class GO2Zenoh(StaticTfPublisher): + """The go2's zenoh-side streams, plus the static data the robot doesn't send.""" + + config: GO2ZenohConfig + + # Owned by the on-robot bridge, never published here. + cmd_vel: In[Twist] + odometry: Out[Odometry] + lidar: Out[PointCloud2] # per-scan, in the LIO's own sensor frame + pointlio_map: Out[PointCloud2] # accumulated world map, frame `odom` + video: Out[CompressedVideo] # front camera, H.264 annex-B + + # Ours: nothing on the robot emits intrinsics. + camera_info: Out[CameraInfo] + + _camera_info: CameraInfo = _camera_info_static() + + @rpc + def start(self) -> None: + super().start() # static mount tree onto tf + self.register_disposable( + Disposable(self.odometry.transport.subscribe(self._publish_tf, self.odometry)) + ) + self.spawn(self._publish_camera_info()) + + def transforms(self) -> list[Transform]: + """The mount tree, rooted at mid360_link because Point-LIO owns that frame. + + The rig is measured outward from the body, but the only live edge here is + odom -> mid360_link. Publishing the measured direction as-is would leave + mid360_link with two parents (odom and front_camera) and the body would snap + between them at 35 Hz, so the two edges above the lidar are inverted and the + body hangs off the sensor instead. + """ + base_to_camera = Transform( + translation=CAMERA_XYZ, + frame_id="base_link", + child_frame_id="front_camera", + ) + camera_to_mid360 = Transform( + translation=MID360_XYZ, + rotation=Quaternion.from_euler( + Vector3(*(math.radians(d) for d in self.config.mid360_mount_rpy_deg)) + ), + frame_id="front_camera", + child_frame_id="mid360_link", + ) + camera_to_optical = Transform( + rotation=Quaternion.from_euler(OPTICAL_RPY), + frame_id="front_camera", + child_frame_id="camera_optical", + ) + return [-camera_to_mid360, -base_to_camera, camera_to_optical] + + def _publish_tf(self, odom: Odometry) -> None: + """The one moving edge, odom -> mid360_link; the bridge publishes no tf.""" + self.tf.publish(Transform.from_pose(odom.child_frame_id, odom.to_pose_stamped())) + + async def _publish_camera_info(self) -> None: + period = 1.0 / self.config.camera_info_hz + while self._running: + self._camera_info.ts = time.time() + self.camera_info.publish(self._camera_info) + await asyncio.sleep(period) From 33613428922893a6fcc154c363ebe5609265c11d Mon Sep 17 00:00:00 2001 From: Ivan Nikolic Date: Wed, 22 Jul 2026 18:05:56 +0300 Subject: [PATCH 02/16] adds compressedVideo --- dimos/msgs/foxglove_msgs/CompressedVideo.py | 107 ++++++++++++++++++ .../foxglove_msgs/test_CompressedVideo.py | 48 ++++++++ 2 files changed, 155 insertions(+) create mode 100644 dimos/msgs/foxglove_msgs/CompressedVideo.py create mode 100644 dimos/msgs/foxglove_msgs/test_CompressedVideo.py diff --git a/dimos/msgs/foxglove_msgs/CompressedVideo.py b/dimos/msgs/foxglove_msgs/CompressedVideo.py new file mode 100644 index 00000000000..b13089c303b --- /dev/null +++ b/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})" + ) diff --git a/dimos/msgs/foxglove_msgs/test_CompressedVideo.py b/dimos/msgs/foxglove_msgs/test_CompressedVideo.py new file mode 100644 index 00000000000..fcecd05a1cb --- /dev/null +++ b/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() From f12f478142b717e1d8a7514379f1a3a91c12a4bc Mon Sep 17 00:00:00 2001 From: Ivan Nikolic Date: Wed, 22 Jul 2026 18:21:07 +0300 Subject: [PATCH 03/16] go2 zenoh: action verbs over a std_msgs/String topic MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The bridge could only be driven by cmd_vel; sit/stand/hello had no path off the web UI. Adds `dimos/command/std_msgs.String`, whose payload go2web maps to a sport api id and writes to rt/api/sport/request — the same DDS writer the UI's buttons already use, so no second control path. String is the ROS-shaped choice here (ROS has no enum msg, and an action is the RPC this deliberately isn't). The wrapper was the only std_msgs type dimos was missing. QoS is reliable+block, unlike every other robot channel: a dropped verb is a stand-up that silently never happens, where a dropped cmd_vel frame is superseded 30 ms later. --- dimos/core/transport_factory.py | 6 +-- dimos/msgs/std_msgs/String.py | 30 +++++++++++ .../unitree/go2/zenoh/zenohconnection.py | 51 +++++++++++++++++-- 3 files changed, 81 insertions(+), 6 deletions(-) create mode 100644 dimos/msgs/std_msgs/String.py diff --git a/dimos/core/transport_factory.py b/dimos/core/transport_factory.py index f8dac21a8c3..cf71d41c882 100644 --- a/dimos/core/transport_factory.py +++ b/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: diff --git a/dimos/msgs/std_msgs/String.py b/dimos/msgs/std_msgs/String.py new file mode 100644 index 00000000000..ef48233dfd3 --- /dev/null +++ b/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 diff --git a/dimos/robot/unitree/go2/zenoh/zenohconnection.py b/dimos/robot/unitree/go2/zenoh/zenohconnection.py index d76a813003e..fe81d8bb934 100644 --- a/dimos/robot/unitree/go2/zenoh/zenohconnection.py +++ b/dimos/robot/unitree/go2/zenoh/zenohconnection.py @@ -16,8 +16,8 @@ The bridge (go2web ``src/dimos_zenoh.rs``, on by default via the ``dimos_zenoh`` cargo feature) publishes Point-LIO odom, the per-scan and world clouds and H.264 video -straight onto dimos's own zenoh keys, and consumes ``cmd_vel``. Nothing -here produces any of it — declaring the ports is what puts the robot's topics on the +straight onto dimos's own zenoh keys, and consumes ``cmd_vel`` and ``command``. Nothing +here produces the streams — declaring the ports is what puts the robot's topics on the graph, the same arrangement as a NativeModule whose binary publishes out of process (see :class:`PointLio`). The port names are the wire contract: the bridge's keys are ``dimos//``, so renaming a port here silently disconnects it. Remap in @@ -46,6 +46,7 @@ from dimos.msgs.nav_msgs.Odometry import Odometry from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.msgs.std_msgs.String import String from dimos.protocol.tf.static_tf_publisher import StaticTfPublisher, StaticTfPublisherConfig from dimos.robot.unitree.go2.connection import _camera_info_static @@ -74,8 +75,12 @@ class GO2Zenoh(StaticTfPublisher): config: GO2ZenohConfig - # Owned by the on-robot bridge, never published here. + # Consumed by the on-robot bridge, never published here. cmd_vel: In[Twist] + # One-shot action verbs ("sit", "hello", ...) or a bare sport api id as a + # string. Consumed by the bridge too, but nothing else in the graph produces + # them, so the rpcs below publish onto it — no zenoh RPC, just a topic. + command: In[String] odometry: Out[Odometry] lidar: Out[PointCloud2] # per-scan, in the LIO's own sensor frame pointlio_map: Out[PointCloud2] # accumulated world map, frame `odom` @@ -93,6 +98,46 @@ def start(self) -> None: Disposable(self.odometry.transport.subscribe(self._publish_tf, self.odometry)) ) self.spawn(self._publish_camera_info()) + self.standup() + + @rpc + def stop(self) -> None: + self.liedown() + + @rpc + def send_command(self, verb: str) -> None: + """Fire an action verb at the bridge (see go2web ``topics::sport_id``).""" + self.command.transport.publish(String(verb)) + + @rpc + def sport_command(self, api_id: int) -> None: + """Same, by raw sport api id — the bridge parses a numeric verb.""" + self.send_command(str(api_id)) + + # The verbs the WebRTC GO2Connection exposes as rpcs, same names. + @rpc + def standup(self) -> None: + self.send_command("stand-up") + + @rpc + def liedown(self) -> None: + self.send_command("stand-down") + + @rpc + def balance_stand(self) -> None: + self.send_command("balance") + + @rpc + def sit(self) -> None: + self.send_command("sit") + + @rpc + def hello(self) -> None: + self.send_command("hello") + + @rpc + def jump(self) -> None: + self.send_command("jump") def transforms(self) -> list[Transform]: """The mount tree, rooted at mid360_link because Point-LIO owns that frame. From e12db8b165bd6e29a133cf95d92bf6430390cbf3 Mon Sep 17 00:00:00 2001 From: Ivan Nikolic Date: Wed, 22 Jul 2026 18:21:47 +0300 Subject: [PATCH 04/16] nav blueprints --- dimos/robot/all_blueprints.py | 4 +- dimos/robot/unitree/go2/zenoh/blueprints.py | 168 ++++++++++++++------ 2 files changed, 125 insertions(+), 47 deletions(-) diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index ca1733f2de9..05b0fe88984 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -63,6 +63,9 @@ "drone-agentic": "dimos.robot.drone.blueprints.agentic.drone_agentic:drone_agentic", "drone-basic": "dimos.robot.drone.blueprints.basic.drone_basic:drone_basic", "dual-xarm6-planner": "dimos.robot.manipulators.xarm.blueprints.basic:dual_xarm6_planner", + "go2-zenoh-basic": "dimos.robot.unitree.go2.zenoh.blueprints:go2_zenoh_basic", + "go2-zenoh-nav": "dimos.robot.unitree.go2.zenoh.blueprints:go2_zenoh_nav", + "go2-zenoh-raycaster": "dimos.robot.unitree.go2.zenoh.blueprints:go2_zenoh_raycaster", "keyboard-teleop-a1z": "dimos.robot.manipulators.a1z.blueprints.teleop:keyboard_teleop_a1z", "keyboard-teleop-a750": "dimos.robot.manipulators.a750.blueprints.teleop:keyboard_teleop_a750", "keyboard-teleop-openarm": "dimos.robot.manipulators.openarm.blueprints.teleop:keyboard_teleop_openarm", @@ -134,7 +137,6 @@ "unitree-go2-multi": "dimos.robot.unitree.go2.blueprints.basic.unitree_go2_multi:unitree_go2_multi", "unitree-go2-multi-teleop": "dimos.robot.unitree.go2.blueprints.basic.unitree_go2_multi_teleop:unitree_go2_multi_teleop", "unitree-go2-nav-3d": "dimos.robot.unitree.go2.blueprints.navigation.unitree_go2_nav_3d:unitree_go2_nav_3d", - "unitree-go2-nav-3d-zenoh": "dimos.robot.unitree.go2.zenoh.blueprints:unitree_go2_nav_3d_zenoh", "unitree-go2-relocalization": "dimos.robot.unitree.go2.blueprints.smart.unitree_go2:unitree_go2_relocalization", "unitree-go2-ros": "dimos.robot.unitree.go2.blueprints.smart.unitree_go2_ros:unitree_go2_ros", "unitree-go2-security": "dimos.robot.unitree.go2.blueprints.agentic.unitree_go2_security:unitree_go2_security", diff --git a/dimos/robot/unitree/go2/zenoh/blueprints.py b/dimos/robot/unitree/go2/zenoh/blueprints.py index fa3d03a2fa3..2c027adddc9 100644 --- a/dimos/robot/unitree/go2/zenoh/blueprints.py +++ b/dimos/robot/unitree/go2/zenoh/blueprints.py @@ -13,12 +13,21 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""3d navigation on a Go2 that runs the go2web zenoh bridge. +"""Go2 blueprints for a robot running the go2web zenoh bridge. -The same nav stack as ``unitree_go2_nav_3d``, minus the two modules the robot now runs +The same stack as ``unitree_go2_nav_3d``, minus the two modules the robot now runs itself: no WebRTC ``GO2Connection`` and no local ``PointLio`` — odom, lidar and video -arrive over zenoh already (see :class:`GO2Zenoh`), which is why the graph runs on the +arrive over zenoh already (see :class:`GO2Zenoh`), which is why the graphs run on the zenoh transport. + +Three layers, each a superset of the one above it, so a failure can be bisected by +dropping down a level: + +- ``go2-zenoh-basic`` — streams plus teleop. Confirms the bridge, the tf tree and the + camera without any mapping. +- ``go2-zenoh-raycaster`` — adds :class:`RayTracingVoxelMap`, so the voxel maps can be + eyeballed before a planner consumes them. +- ``go2-zenoh-nav`` — the full stack: planner, goal relay and path follower. """ import math @@ -30,7 +39,11 @@ from dimos.msgs.geometry_msgs.Quaternion import Quaternion from dimos.msgs.geometry_msgs.Vector3 import Vector3 from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.navigation.basic_path_follower.module import BasicPathFollower from dimos.navigation.movement_manager.movement_manager import MovementManager +from dimos.navigation.nav_3d.mls_planner.goal_relay import GoalRelay +from dimos.navigation.nav_3d.mls_planner.mls_planner_native import MLSPlannerNative +from dimos.navigation.nav_3d.mls_planner.odom_body_frame import OdomBodyFrame from dimos.robot.unitree.go2.zenoh.zenohconnection import GO2Zenoh from dimos.visualization.vis_module import vis_module @@ -99,55 +112,118 @@ def _convert_scan(grid: PointCloud2) -> Any: return grid.to_rerun(color=[255, 0, 0], voxel_size=0.05) -_rerun_config = { - "blueprint": _rerun_blueprint, - "visual_override": { - "world/camera_info": _camera_info_to_pinhole, - "world/pointlio_map": _convert_map, - "world/lidar": _convert_scan, - }, - # The clouds are what costs: the world map runs up to 300k points x 16 B per message - # at ~2 Hz, the per-scan lidar ~10 Hz. Deliberately no entry for world/video — the - # throttle drops whole messages, and dropping H.264 samples breaks decoding until the - # next keyframe, so the camera has to be slowed at the source instead. (0 here would - # mean unlimited, not off.) - "max_hz": { - # "world/lidar": 2.0, - # "world/pointlio_map": 0.5, - }, +def _render_global_map(msg: Any) -> Any: + return msg.to_rerun() + + +def _render_path(msg: Any) -> Any: + # The planner emits an empty path when it finds no route to the goal. + # Logging those would blank the line, so drop them and keep the last path. + if len(msg.poses) == 0: + return None + return msg + + +# What the bridge itself sends, present in all three blueprints. +_BRIDGE_OVERRIDE = { + "world/camera_info": _camera_info_to_pinhole, + "world/pointlio_map": _convert_map, + "world/lidar": _convert_scan, +} +# The clouds are what costs: the world maps run up to 300k points x 16 B per message at +# ~2 Hz, the per-scan lidar ~10 Hz. Deliberately no entry for world/video — the throttle +# drops whole messages, and dropping H.264 samples breaks decoding until the next +# keyframe, so the camera has to be slowed at the source instead. (0 here would mean +# unlimited, not off.) +_BRIDGE_MAX_HZ = { + "world/lidar": 2.0, + "world/pointlio_map": 0.5, +} + +_MAP_OVERRIDE = {"world/global_map": _render_global_map} +_MAP_MAX_HZ = { + # Already rate-limited at the source by global_emit_every, roughly every 5s. + "world/global_map": 0, + "world/local_map": 0.5, +} + +_NAV_OVERRIDE = { + "world/path": _render_path, + # Planner debug layers, off — viz_publish_hz=0.0 means nothing arrives anyway. + "world/surface_map": None, + "world/nodes": None, + "world/node_edges": None, } -unitree_go2_nav_3d_zenoh = autoconnect( - vis_module(viewer_backend=global_config.viewer, rerun_config=_rerun_config), + +def _rerun_config( + visual_override: dict[str, Any] | None = None, max_hz: dict[str, Any] | None = None +) -> dict[str, Any]: + """The bridge's own view, plus whatever the layer above it adds.""" + return { + "blueprint": _rerun_blueprint, + "visual_override": {**_BRIDGE_OVERRIDE, **(visual_override or {})}, + "max_hz": {**_BRIDGE_MAX_HZ, **(max_hz or {})}, + } + + +# Bridge streams + teleop only: no mapping, no planning. cmd_vel still reaches the robot +# through MovementManager, so this is the layer to drive from when something upstream is +# suspect. +go2_zenoh_basic = autoconnect( + vis_module(viewer_backend=global_config.viewer, rerun_config=_rerun_config()), + GO2Zenoh.blueprint(mid360_mount_rpy_deg=MID360_MOUNT_RPY_DEG), + MovementManager.blueprint(), +).global_config(transport="zenoh", n_workers=4, robot_model="unitree_go2") + +# Adds the voxel map. It consumes GO2Zenoh's lidar + odometry directly: the bridge stamps +# them exactly as PointLio does locally (frames odom / mid360_link, xyz+intensity at +# point_step 16). +_ray_tracer = RayTracingVoxelMap.blueprint( + voxel_size=voxel_size, + emit_every=1, + global_emit_every=50, + min_health=-1, + max_health=5, + support_min=4, +) + +go2_zenoh_raycaster = autoconnect( + vis_module( + viewer_backend=global_config.viewer, rerun_config=_rerun_config(_MAP_OVERRIDE, _MAP_MAX_HZ) + ), + GO2Zenoh.blueprint(mid360_mount_rpy_deg=MID360_MOUNT_RPY_DEG), + _ray_tracer, + MovementManager.blueprint(), +).global_config(transport="zenoh", n_workers=6, robot_model="unitree_go2") + +go2_zenoh_nav = autoconnect( + vis_module( + viewer_backend=global_config.viewer, + rerun_config=_rerun_config({**_MAP_OVERRIDE, **_NAV_OVERRIDE}, _MAP_MAX_HZ), + ), GO2Zenoh.blueprint(mid360_mount_rpy_deg=MID360_MOUNT_RPY_DEG), # Level the tilted-sensor odometry into the body frame so the follower steers on a # true heading. The ray tracer keeps the raw sensor odometry. - # # OdomBodyFrame.blueprint(mount_rotation=_mount_rotation()), - RayTracingVoxelMap.blueprint( + OdomBodyFrame.blueprint(mount_rotation=_mount_rotation()), + _ray_tracer, + # global_map is remapped off so the planner runs purely on the + # incremental local_map + region_bounds pair. + MLSPlannerNative.blueprint( + world_frame="odom", voxel_size=voxel_size, - emit_every=1, - global_emit_every=50, - min_health=-1, - max_health=5, - support_min=4, + robot_height=0.3, + surface_closing_radius=0.3, + wall_clearance_m=0.1, + wall_buffer_m=0.75, + wall_buffer_weight=100.0, + step_threshold_m=0.16, + step_penalty_weight=4.0, + viz_publish_hz=0.0, + ).remappings([(MLSPlannerNative, "global_map", "global_map_unused")]), + GoalRelay.blueprint(), + BasicPathFollower.blueprint(speed=0.5, heading_gain=0.4, max_angular=0.6).remappings( + [(BasicPathFollower, "odometry", "body_odometry")] ), - # # global_map is remapped off so the planner runs purely on the - # # incremental local_map + region_bounds pair. - # MLSPlannerNative.blueprint( - # world_frame="odom", - # voxel_size=voxel_size, - # robot_height=0.3, - # surface_closing_radius=0.3, - # wall_clearance_m=0.1, - # wall_buffer_m=0.75, - # wall_buffer_weight=100.0, - # step_threshold_m=0.16, - # step_penalty_weight=4.0, - # viz_publish_hz=0.0, - # ).remappings([(MLSPlannerNative, "global_map", "global_map_unused")]), - # # GoalRelay.blueprint(), - # # BasicPathFollower.blueprint(speed=0.5, heading_gain=0.4, max_angular=0.6).remappings( - # # [(BasicPathFollower, "odometry", "body_odometry")] - # # ), MovementManager.blueprint(), ).global_config(transport="zenoh", n_workers=8, robot_model="unitree_go2") From 56c4e423adccf99cf6f66124b02ca3f1fe75d0fe Mon Sep 17 00:00:00 2001 From: Ivan Nikolic Date: Wed, 22 Jul 2026 18:55:19 +0300 Subject: [PATCH 05/16] go2 zenoh: set_lidar rpc --- dimos/robot/unitree/go2/zenoh/zenohconnection.py | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/dimos/robot/unitree/go2/zenoh/zenohconnection.py b/dimos/robot/unitree/go2/zenoh/zenohconnection.py index fe81d8bb934..5defd22a2ff 100644 --- a/dimos/robot/unitree/go2/zenoh/zenohconnection.py +++ b/dimos/robot/unitree/go2/zenoh/zenohconnection.py @@ -139,6 +139,12 @@ def hello(self) -> None: def jump(self) -> None: self.send_command("jump") + @rpc + def set_lidar(self, enabled: bool) -> None: + """The head L1 on/off (what obstacle avoidance runs on). Not the MID-360: + `odometry`, `lidar` and `pointlio_map` keep flowing either way.""" + self.send_command("lidar on" if enabled else "lidar off") + def transforms(self) -> list[Transform]: """The mount tree, rooted at mid360_link because Point-LIO owns that frame. From 9a39fdf3dd8bd031217a2f8daa6afedd1897dcdb Mon Sep 17 00:00:00 2001 From: Ivan Nikolic Date: Wed, 22 Jul 2026 19:27:32 +0300 Subject: [PATCH 06/16] iterating on details, nav works --- dimos/mapping/ray_tracing/rust/flake.lock | 8 +++--- dimos/mapping/ray_tracing/rust/flake.nix | 2 +- dimos/robot/unitree/go2/zenoh/blueprints.py | 32 +++++++++++++++------ 3 files changed, 28 insertions(+), 14 deletions(-) diff --git a/dimos/mapping/ray_tracing/rust/flake.lock b/dimos/mapping/ray_tracing/rust/flake.lock index 071efa165c9..7177d3e5807 100644 --- a/dimos/mapping/ray_tracing/rust/flake.lock +++ b/dimos/mapping/ray_tracing/rust/flake.lock @@ -3,11 +3,11 @@ "dimos-repo": { "flake": false, "locked": { - "lastModified": 1782159599, - "narHash": "sha256-U2FwqtFzYen8Or50oTYGrqSi8vRePlS3m581ti2T56Q=", + "lastModified": 1784733707, + "narHash": "sha256-5aAeA8OxX8u5prs9LmTinxQiBVAYJb+cOcws3C/ff24=", "ref": "main", - "rev": "0bfe9cbbc79c52afa1ccea783147438f89bd7cf2", - "revCount": 836, + "rev": "e12db8b165bd6e29a133cf95d92bf6430390cbf3", + "revCount": 966, "type": "git", "url": "file:../../../.." }, diff --git a/dimos/mapping/ray_tracing/rust/flake.nix b/dimos/mapping/ray_tracing/rust/flake.nix index 897d8818a23..15c40909f35 100644 --- a/dimos/mapping/ray_tracing/rust/flake.nix +++ b/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"; }; diff --git a/dimos/robot/unitree/go2/zenoh/blueprints.py b/dimos/robot/unitree/go2/zenoh/blueprints.py index 2c027adddc9..f363014199e 100644 --- a/dimos/robot/unitree/go2/zenoh/blueprints.py +++ b/dimos/robot/unitree/go2/zenoh/blueprints.py @@ -31,6 +31,7 @@ """ import math +import os from typing import Any from dimos.core.coordination.blueprints import autoconnect @@ -41,6 +42,11 @@ from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 from dimos.navigation.basic_path_follower.module import BasicPathFollower from dimos.navigation.movement_manager.movement_manager import MovementManager +from dimos.navigation.nav_3d.evaluator.blueprints import ( + _render_node_edges, + _render_nodes, + _render_surface_map, +) from dimos.navigation.nav_3d.mls_planner.goal_relay import GoalRelay from dimos.navigation.nav_3d.mls_planner.mls_planner_native import MLSPlannerNative from dimos.navigation.nav_3d.mls_planner.odom_body_frame import OdomBodyFrame @@ -49,6 +55,11 @@ voxel_size = 0.08 +# Opt-in planner introspection: set DIMOS_PLANNER_VIZ=1 to have MLSPlannerNative publish +# its surface map and search graph, and to render them (see _NAV_OVERRIDE). Costs a +# rebuild of that geometry every viz tick, so it is off in normal operation. +_PLANNER_VIZ = os.getenv("DIMOS_PLANNER_VIZ", "").lower() in ("1", "true", "yes", "on") + # The lidar mount rotation on this rig (fixed-axis rpy, degrees), feeding both the static # tf GO2Zenoh publishes and the rotation that levels its odometry — they must agree or nav # steers off-heading. 60 deg of tilt, carried on roll because the lidar sits yawed 90 deg @@ -105,7 +116,7 @@ def _rerun_blueprint() -> Any: def _convert_map(grid: PointCloud2) -> Any: - return grid.to_rerun(voxel_size=0.04) + return grid.to_rerun(voxel_size=0.025) def _convert_scan(grid: PointCloud2) -> Any: @@ -135,9 +146,9 @@ def _render_path(msg: Any) -> Any: # drops whole messages, and dropping H.264 samples breaks decoding until the next # keyframe, so the camera has to be slowed at the source instead. (0 here would mean # unlimited, not off.) -_BRIDGE_MAX_HZ = { - "world/lidar": 2.0, - "world/pointlio_map": 0.5, +_BRIDGE_MAX_HZ: dict[str, float] = { + # "world/lidar": 2.0, + # "world/pointlio_map": 0.5, } _MAP_OVERRIDE = {"world/global_map": _render_global_map} @@ -149,10 +160,13 @@ def _render_path(msg: Any) -> Any: _NAV_OVERRIDE = { "world/path": _render_path, - # Planner debug layers, off — viz_publish_hz=0.0 means nothing arrives anyway. - "world/surface_map": None, - "world/nodes": None, - "world/node_edges": None, + # The planner's own view of the world: the traversable surface it extracted, the graph + # nodes it sampled on that surface, and the edges between them coloured green->red by + # traversability cost. Off unless DIMOS_PLANNER_VIZ is set — it is a lot of geometry, + # and with viz_publish_hz at 0.0 the planner does not even emit it. + "world/surface_map": _render_surface_map if _PLANNER_VIZ else None, + "world/nodes": _render_nodes if _PLANNER_VIZ else None, + "world/node_edges": _render_node_edges if _PLANNER_VIZ else None, } @@ -219,7 +233,7 @@ def _rerun_config( wall_buffer_weight=100.0, step_threshold_m=0.16, step_penalty_weight=4.0, - viz_publish_hz=0.0, + viz_publish_hz=2.0 if _PLANNER_VIZ else 0.0, ).remappings([(MLSPlannerNative, "global_map", "global_map_unused")]), GoalRelay.blueprint(), BasicPathFollower.blueprint(speed=0.5, heading_gain=0.4, max_angular=0.6).remappings( From 54208d3f8b446c1a80a8e7bec370718c8fab9527 Mon Sep 17 00:00:00 2001 From: Ivan Nikolic Date: Wed, 22 Jul 2026 19:41:48 +0300 Subject: [PATCH 07/16] planner visualizations --- .../navigation/nav_3d/evaluator/blueprints.py | 52 ++--------- dimos/navigation/nav_3d/mls_planner/viz.py | 91 +++++++++++++++++++ .../navigation/unitree_go2_mls_htc.py | 9 +- .../navigation/unitree_go2_nav_3d.py | 9 +- dimos/robot/unitree/go2/zenoh/blueprints.py | 26 ++---- 5 files changed, 117 insertions(+), 70 deletions(-) create mode 100644 dimos/navigation/nav_3d/mls_planner/viz.py diff --git a/dimos/navigation/nav_3d/evaluator/blueprints.py b/dimos/navigation/nav_3d/evaluator/blueprints.py index 3e8fc27f1eb..71ec4a9bf92 100644 --- a/dimos/navigation/nav_3d/evaluator/blueprints.py +++ b/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, } ), ) diff --git a/dimos/navigation/nav_3d/mls_planner/viz.py b/dimos/navigation/nav_3d/mls_planner/viz.py new file mode 100644 index 00000000000..df92a96eb7c --- /dev/null +++ b/dimos/navigation/nav_3d/mls_planner/viz.py @@ -0,0 +1,91 @@ +# 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. + +"""Rendering for :class:`MLSPlannerNative`'s introspection layers. + +The planner can publish what it actually searched over: the traversable surface it +extracted, the graph nodes it sampled on that surface, and the weighted edges between +them. Its ``viz_publish_hz`` config decides whether it emits them (0.0 = not at all, +which is the default — the geometry is rebuilt from scratch every tick). +:func:`planner_visual_override` reads that same number so a blueprint only has to set it +in one place; drawing and publishing cannot drift apart. +""" + +from __future__ import annotations + +from typing import TYPE_CHECKING, Any + +import numpy as np + +from dimos.msgs.nav_msgs.LineSegments3D import LineSegments3D +from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 + +if TYPE_CHECKING: + from rerun._baseclasses import Archetype + +# Small lift so graph artifacts render visibly above the surface points instead of z-fighting. +_GRAPH_Z_LIFT = 0.05 + + +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: + import rerun as rr + + 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.""" + import rerun as rr + + 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)) + + +def planner_visual_override(viz_publish_hz: float) -> dict[str, Any]: + """rerun overrides for the planner's debug entities, keyed off its own publish rate. + + Pass the same value given to ``MLSPlannerNative.blueprint(viz_publish_hz=...)``. + """ + on = viz_publish_hz > 0.0 + return { + "world/surface_map": render_surface_map if on else None, + "world/nodes": render_nodes if on else None, + "world/node_edges": render_node_edges if on else None, + } diff --git a/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_mls_htc.py b/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_mls_htc.py index c36b317a167..1468c8c989f 100644 --- a/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_mls_htc.py +++ b/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_mls_htc.py @@ -28,11 +28,14 @@ from dimos.navigation.movement_manager.movement_manager import MovementManager from dimos.navigation.nav_3d.mls_planner.goal_relay import GoalRelay from dimos.navigation.nav_3d.mls_planner.mls_planner_native import MLSPlannerNative +from dimos.navigation.nav_3d.mls_planner.viz import planner_visual_override from dimos.robot.unitree.go2.blueprints.basic.unitree_go2_basic import rerun_config from dimos.robot.unitree.go2.connection import GO2Connection from dimos.visualization.vis_module import vis_module voxel_size = 0.05 +# Raise above 0 to draw what the planner searched over (surface, nodes, weighted edges). +planner_viz_hz = 0.0 # Height of the head-mounted lidar above the ground while standing. # While in case of Go2 it's ~ .3m, but in this blueprint # MLSPlanner works better on Go2 lidar when the value is 0.5 @@ -63,9 +66,7 @@ def _render_path(msg: Any) -> Any: "world/global_map": _render_global_map, "world/planner_path": None, "world/path": _render_path, - "world/surface_map": None, - "world/nodes": None, - "world/node_edges": None, + **planner_visual_override(planner_viz_hz), }, } @@ -86,7 +87,7 @@ def _render_path(msg: Any) -> Any: wall_buffer_weight=100.0, step_threshold_m=0.16, step_penalty_weight=1.0, - viz_publish_hz=0.0, + viz_publish_hz=planner_viz_hz, ).remappings( [ (MLSPlannerNative, "path", "planner_path"), diff --git a/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py b/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py index 7e9f5f8fdd2..0280be7f570 100644 --- a/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py +++ b/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py @@ -37,6 +37,7 @@ from dimos.navigation.nav_3d.mls_planner.goal_relay import GoalRelay from dimos.navigation.nav_3d.mls_planner.mls_planner_native import MLSPlannerNative from dimos.navigation.nav_3d.mls_planner.odom_body_frame import OdomBodyFrame +from dimos.navigation.nav_3d.mls_planner.viz import planner_visual_override from dimos.robot.unitree.go2.blueprints.basic.unitree_go2_basic import rerun_config from dimos.robot.unitree.go2.connection import GO2Connection from dimos.robot.unitree.go2.go2_mid360_static_transforms import ( @@ -46,6 +47,8 @@ from dimos.visualization.vis_module import vis_module voxel_size = 0.08 +# Raise above 0 to draw what the planner searched over (surface, nodes, weighted edges). +planner_viz_hz = 0.0 # base_link <- lidar mount rotation, so nav reads odometry in the level body frame. _sensor_mount_rotation = list(base_link_from_mid360().rotation.to_tuple()) @@ -157,9 +160,7 @@ def _static_sensor_axes(rr: Any) -> list[Any]: "world/camera_info": None, "world/color_image": None, "world/lidar": None, - "world/surface_map": None, - "world/nodes": None, - "world/node_edges": None, + **planner_visual_override(planner_viz_hz), }, } @@ -198,7 +199,7 @@ def _static_sensor_axes(rr: Any) -> list[Any]: wall_buffer_weight=100.0, step_threshold_m=0.16, step_penalty_weight=4.0, - viz_publish_hz=0.0, + viz_publish_hz=planner_viz_hz, ).remappings([(MLSPlannerNative, "global_map", "global_map_unused")]), GoalRelay.blueprint(), BasicPathFollower.blueprint(speed=0.5, heading_gain=0.4, max_angular=0.6).remappings( diff --git a/dimos/robot/unitree/go2/zenoh/blueprints.py b/dimos/robot/unitree/go2/zenoh/blueprints.py index f363014199e..b1eeb67b0ab 100644 --- a/dimos/robot/unitree/go2/zenoh/blueprints.py +++ b/dimos/robot/unitree/go2/zenoh/blueprints.py @@ -31,7 +31,6 @@ """ import math -import os from typing import Any from dimos.core.coordination.blueprints import autoconnect @@ -42,23 +41,18 @@ from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 from dimos.navigation.basic_path_follower.module import BasicPathFollower from dimos.navigation.movement_manager.movement_manager import MovementManager -from dimos.navigation.nav_3d.evaluator.blueprints import ( - _render_node_edges, - _render_nodes, - _render_surface_map, -) from dimos.navigation.nav_3d.mls_planner.goal_relay import GoalRelay from dimos.navigation.nav_3d.mls_planner.mls_planner_native import MLSPlannerNative from dimos.navigation.nav_3d.mls_planner.odom_body_frame import OdomBodyFrame +from dimos.navigation.nav_3d.mls_planner.viz import planner_visual_override from dimos.robot.unitree.go2.zenoh.zenohconnection import GO2Zenoh from dimos.visualization.vis_module import vis_module voxel_size = 0.08 - -# Opt-in planner introspection: set DIMOS_PLANNER_VIZ=1 to have MLSPlannerNative publish -# its surface map and search graph, and to render them (see _NAV_OVERRIDE). Costs a -# rebuild of that geometry every viz tick, so it is off in normal operation. -_PLANNER_VIZ = os.getenv("DIMOS_PLANNER_VIZ", "").lower() in ("1", "true", "yes", "on") +# Raise above 0 to see what the planner searched over: its traversable surface, the graph +# nodes on it and the edges between them, coloured green->red by cost. Drives both the +# planner's publishing and the rerun overrides. 2.0 is a good working rate. +planner_viz_hz = 0.0 # The lidar mount rotation on this rig (fixed-axis rpy, degrees), feeding both the static # tf GO2Zenoh publishes and the rotation that levels its odometry — they must agree or nav @@ -160,13 +154,7 @@ def _render_path(msg: Any) -> Any: _NAV_OVERRIDE = { "world/path": _render_path, - # The planner's own view of the world: the traversable surface it extracted, the graph - # nodes it sampled on that surface, and the edges between them coloured green->red by - # traversability cost. Off unless DIMOS_PLANNER_VIZ is set — it is a lot of geometry, - # and with viz_publish_hz at 0.0 the planner does not even emit it. - "world/surface_map": _render_surface_map if _PLANNER_VIZ else None, - "world/nodes": _render_nodes if _PLANNER_VIZ else None, - "world/node_edges": _render_node_edges if _PLANNER_VIZ else None, + **planner_visual_override(planner_viz_hz), } @@ -233,7 +221,7 @@ def _rerun_config( wall_buffer_weight=100.0, step_threshold_m=0.16, step_penalty_weight=4.0, - viz_publish_hz=2.0 if _PLANNER_VIZ else 0.0, + viz_publish_hz=planner_viz_hz, ).remappings([(MLSPlannerNative, "global_map", "global_map_unused")]), GoalRelay.blueprint(), BasicPathFollower.blueprint(speed=0.5, heading_gain=0.4, max_angular=0.6).remappings( From 2bda3c5e923ae0791755078d45fce9a6626d58b3 Mon Sep 17 00:00:00 2001 From: Ivan Nikolic Date: Wed, 22 Jul 2026 19:45:02 +0300 Subject: [PATCH 08/16] flake relock --- dimos/mapping/ray_tracing/rust/flake.lock | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/dimos/mapping/ray_tracing/rust/flake.lock b/dimos/mapping/ray_tracing/rust/flake.lock index 7177d3e5807..efd45a0fcd1 100644 --- a/dimos/mapping/ray_tracing/rust/flake.lock +++ b/dimos/mapping/ray_tracing/rust/flake.lock @@ -3,11 +3,11 @@ "dimos-repo": { "flake": false, "locked": { - "lastModified": 1784733707, - "narHash": "sha256-5aAeA8OxX8u5prs9LmTinxQiBVAYJb+cOcws3C/ff24=", + "lastModified": 1784689481, + "narHash": "sha256-mA+EF/WT3g3roVDoaTZxpvOoL9vZHep+oNED2evYvxQ=", "ref": "main", - "rev": "e12db8b165bd6e29a133cf95d92bf6430390cbf3", - "revCount": 966, + "rev": "cdd577bfc0a7775d125462ad51ee956f65cb943c", + "revCount": 962, "type": "git", "url": "file:../../../.." }, From 265aaed7995d445764471eb1f8b9e4ec6cf492cf Mon Sep 17 00:00:00 2001 From: Ivan Nikolic Date: Wed, 22 Jul 2026 19:59:03 +0300 Subject: [PATCH 09/16] cleaner map --- dimos/robot/unitree/go2/zenoh/blueprints.py | 9 +++++++++ dimos/robot/unitree/go2/zenoh/zenohconnection.py | 5 ----- 2 files changed, 9 insertions(+), 5 deletions(-) diff --git a/dimos/robot/unitree/go2/zenoh/blueprints.py b/dimos/robot/unitree/go2/zenoh/blueprints.py index b1eeb67b0ab..76bc729dc02 100644 --- a/dimos/robot/unitree/go2/zenoh/blueprints.py +++ b/dimos/robot/unitree/go2/zenoh/blueprints.py @@ -101,6 +101,15 @@ def _rerun_blueprint() -> Any: name="3D", background=rrb.Background(kind="SolidColor", color=[0, 0, 0]), line_grid=rrb.LineGrid3D(plane=rr.components.Plane3D.XY.with_distance(0.5)), + # Point-LIO's own world map duplicates what the ray tracer builds, and it + # is the heaviest cloud on the graph. Hidden rather than dropped, so it + # stays in the entity tree and can be ticked back on in the viewer. + overrides={ + "world/pointlio_map": rrb.EntityBehavior(visible=False), + "world/local_map": rrb.EntityBehavior(visible=False), + "world/lidar": rrb.EntityBehavior(visible=False), + "world/nodes": rrb.EntityBehavior(visible=False), + }, ), column_shares=[1, 2], ), diff --git a/dimos/robot/unitree/go2/zenoh/zenohconnection.py b/dimos/robot/unitree/go2/zenoh/zenohconnection.py index 5defd22a2ff..8506442a5a9 100644 --- a/dimos/robot/unitree/go2/zenoh/zenohconnection.py +++ b/dimos/robot/unitree/go2/zenoh/zenohconnection.py @@ -98,11 +98,6 @@ def start(self) -> None: Disposable(self.odometry.transport.subscribe(self._publish_tf, self.odometry)) ) self.spawn(self._publish_camera_info()) - self.standup() - - @rpc - def stop(self) -> None: - self.liedown() @rpc def send_command(self, verb: str) -> None: From 958738349672ad06f7b73c1fdd875f5877e8d574 Mon Sep 17 00:00:00 2001 From: Ivan Nikolic Date: Wed, 22 Jul 2026 20:30:26 +0300 Subject: [PATCH 10/16] go2 zenoh: stand up on start, lie down on stop MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Also park the L1 at startup — dimos reads the MID-360 through Point-LIO, so the head is 20% of a core spinning for nothing. --- dimos/robot/unitree/go2/zenoh/zenohconnection.py | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/dimos/robot/unitree/go2/zenoh/zenohconnection.py b/dimos/robot/unitree/go2/zenoh/zenohconnection.py index 8506442a5a9..536a1e1db26 100644 --- a/dimos/robot/unitree/go2/zenoh/zenohconnection.py +++ b/dimos/robot/unitree/go2/zenoh/zenohconnection.py @@ -93,12 +93,21 @@ class GO2Zenoh(StaticTfPublisher): @rpc def start(self) -> None: - super().start() # static mount tree onto tf + super().start() self.register_disposable( Disposable(self.odometry.transport.subscribe(self._publish_tf, self.odometry)) ) self.spawn(self._publish_camera_info()) + time.sleep(1) # don't ask idk + self.standup() + self.set_lidar(False) + + @rpc + def stop(self) -> None: + self.liedown() + super().stop() + @rpc def send_command(self, verb: str) -> None: """Fire an action verb at the bridge (see go2web ``topics::sport_id``).""" @@ -109,7 +118,6 @@ def sport_command(self, api_id: int) -> None: """Same, by raw sport api id — the bridge parses a numeric verb.""" self.send_command(str(api_id)) - # The verbs the WebRTC GO2Connection exposes as rpcs, same names. @rpc def standup(self) -> None: self.send_command("stand-up") @@ -136,8 +144,6 @@ def jump(self) -> None: @rpc def set_lidar(self, enabled: bool) -> None: - """The head L1 on/off (what obstacle avoidance runs on). Not the MID-360: - `odometry`, `lidar` and `pointlio_map` keep flowing either way.""" self.send_command("lidar on" if enabled else "lidar off") def transforms(self) -> list[Transform]: From 21ca92e366f1176e86f703e6aadc1c2e508cf8d1 Mon Sep 17 00:00:00 2001 From: Ivan Nikolic Date: Wed, 22 Jul 2026 20:35:31 +0300 Subject: [PATCH 11/16] wait for zenoh init --- dimos/robot/unitree/go2/zenoh/zenohconnection.py | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/dimos/robot/unitree/go2/zenoh/zenohconnection.py b/dimos/robot/unitree/go2/zenoh/zenohconnection.py index 536a1e1db26..d44c34c5fb7 100644 --- a/dimos/robot/unitree/go2/zenoh/zenohconnection.py +++ b/dimos/robot/unitree/go2/zenoh/zenohconnection.py @@ -31,6 +31,7 @@ import asyncio import math +import threading import time from pydantic import Field @@ -99,7 +100,16 @@ def start(self) -> None: ) self.spawn(self._publish_camera_info()) - time.sleep(1) # don't ask idk + # Off the calling thread: start() has to return for the rest of the graph to come + # up, and the first verbs go nowhere until zenoh has matched our command publisher + # against the bridge's subscriber. Cancelled by stop() if we never get that far. + timer = threading.Timer(3.0, self._startup_pose) + timer.daemon = True + timer.start() + self.register_disposable(Disposable(timer.cancel)) + + def _startup_pose(self) -> None: + """Stand, and drop the head L1 — Point-LIO runs off the MID-360, not that one.""" self.standup() self.set_lidar(False) From 354a90a31bea370ff56138d232873704ea1c2e7e Mon Sep 17 00:00:00 2001 From: Ivan Nikolic Date: Wed, 22 Jul 2026 21:52:19 +0300 Subject: [PATCH 12/16] chore: bump dimos-viewer to 0.32.0a2 Fixes ffmpeg respawn churn (per-GOP decoder resets + end-of-video misfires under load) and ~1s native-viewer video latency on live H.264 streams. dimensionalOS/dimos-viewer#25 --- dimos/robot/unitree/go2/zenoh/blueprints.py | 69 +++++++------------ .../unitree/go2/zenoh/zenohconnection.py | 50 +++++--------- pyproject.toml | 4 +- uv.lock | 28 ++++---- 4 files changed, 61 insertions(+), 90 deletions(-) diff --git a/dimos/robot/unitree/go2/zenoh/blueprints.py b/dimos/robot/unitree/go2/zenoh/blueprints.py index 76bc729dc02..6e00f75e29c 100644 --- a/dimos/robot/unitree/go2/zenoh/blueprints.py +++ b/dimos/robot/unitree/go2/zenoh/blueprints.py @@ -15,18 +15,12 @@ """Go2 blueprints for a robot running the go2web zenoh bridge. -The same stack as ``unitree_go2_nav_3d``, minus the two modules the robot now runs -itself: no WebRTC ``GO2Connection`` and no local ``PointLio`` — odom, lidar and video -arrive over zenoh already (see :class:`GO2Zenoh`), which is why the graphs run on the -zenoh transport. - -Three layers, each a superset of the one above it, so a failure can be bisected by -dropping down a level: - -- ``go2-zenoh-basic`` — streams plus teleop. Confirms the bridge, the tf tree and the - camera without any mapping. -- ``go2-zenoh-raycaster`` — adds :class:`RayTracingVoxelMap`, so the voxel maps can be - eyeballed before a planner consumes them. +The ``unitree_go2_nav_3d`` stack minus the modules the robot now runs itself: no WebRTC +``GO2Connection``, no local ``PointLio``. Three layers, each a superset of the one above, +so a failure can be bisected by dropping down a level: + +- ``go2-zenoh-basic`` — streams plus teleop; the bridge, tf and camera, no mapping. +- ``go2-zenoh-raycaster`` — adds :class:`RayTracingVoxelMap`. - ``go2-zenoh-nav`` — the full stack: planner, goal relay and path follower. """ @@ -49,23 +43,19 @@ from dimos.visualization.vis_module import vis_module voxel_size = 0.08 -# Raise above 0 to see what the planner searched over: its traversable surface, the graph -# nodes on it and the edges between them, coloured green->red by cost. Drives both the -# planner's publishing and the rerun overrides. 2.0 is a good working rate. +# Raise above 0 (2.0 works) to draw what the planner searched over: surface, nodes and +# cost-coloured edges. Drives both its publishing and the rerun overrides. planner_viz_hz = 0.0 -# The lidar mount rotation on this rig (fixed-axis rpy, degrees), feeding both the static -# tf GO2Zenoh publishes and the rotation that levels its odometry — they must agree or nav -# steers off-heading. 60 deg of tilt, carried on roll because the lidar sits yawed 90 deg -# on its bracket; verified against Point-LIO's own attitude on the standing robot. +# Feeds both the static tf GO2Zenoh publishes and the rotation that levels its odometry — +# they must agree or nav steers off-heading. Verified against Point-LIO's own attitude. MID360_MOUNT_RPY_DEG = (-60.0, 0.0, -90.0) def _mount_rotation() -> list[float]: """base_link <- lidar rotation, so nav reads odometry in the level body frame. - base_link -> front_camera carries no rotation, so the composed base_link -> mid360_link - rotation is just the mount rpy above. + base_link -> front_camera carries no rotation, so this is just the mount rpy above. """ rpy = Vector3(*(math.radians(d) for d in MID360_MOUNT_RPY_DEG)) return list(Quaternion.from_euler(rpy).to_tuple()) @@ -74,11 +64,10 @@ def _mount_rotation() -> list[float]: def _camera_info_to_pinhole(camera_info: Any) -> Any: """Log the pinhole onto the video's entity instead of camera_info's own. - The rerun bridge names entities after topics, so camera_info and video land on - sibling paths — and a Pinhole only projects the entity it lives on and that entity's - children, which is why the frustum draws but stays empty. No ``optical_frame`` here: - the video carries frame_id ``camera_optical``, so the bridge already anchors that - entity to the tf frame, and a second parent would be rejected. + Entities are named after topics, so the two land on sibling paths — and a Pinhole only + projects its own entity and its children, hence a frustum that draws but stays empty. + No ``optical_frame``: the video's frame_id already anchors it, a second parent is + rejected. """ return camera_info.to_rerun(image_topic="world/video") @@ -86,9 +75,8 @@ def _camera_info_to_pinhole(camera_info: Any) -> Any: def _rerun_blueprint() -> Any: """Split layout: camera feed + 3D world, as the WebRTC go2 blueprint has. - The 2D view sits on ``world/video`` rather than ``world/color_image`` — over zenoh the - camera arrives as an H.264 ``CompressedVideo`` on the ``video`` port, decoded in the - viewer, and that is also where the pinhole is logged. + The 2D view sits on ``world/video``, not ``world/color_image`` — over zenoh the camera + arrives as H.264 on the ``video`` port, which is also where the pinhole is logged. """ import rerun as rr import rerun.blueprint as rrb @@ -101,9 +89,8 @@ def _rerun_blueprint() -> Any: name="3D", background=rrb.Background(kind="SolidColor", color=[0, 0, 0]), line_grid=rrb.LineGrid3D(plane=rr.components.Plane3D.XY.with_distance(0.5)), - # Point-LIO's own world map duplicates what the ray tracer builds, and it - # is the heaviest cloud on the graph. Hidden rather than dropped, so it - # stays in the entity tree and can be ticked back on in the viewer. + # Hidden rather than dropped: still in the entity tree, tickable in the + # viewer. overrides={ "world/pointlio_map": rrb.EntityBehavior(visible=False), "world/local_map": rrb.EntityBehavior(visible=False), @@ -144,11 +131,9 @@ def _render_path(msg: Any) -> Any: "world/pointlio_map": _convert_map, "world/lidar": _convert_scan, } -# The clouds are what costs: the world maps run up to 300k points x 16 B per message at -# ~2 Hz, the per-scan lidar ~10 Hz. Deliberately no entry for world/video — the throttle -# drops whole messages, and dropping H.264 samples breaks decoding until the next -# keyframe, so the camera has to be slowed at the source instead. (0 here would mean -# unlimited, not off.) +# The clouds are what costs: world maps up to 300k points at ~2 Hz, per-scan lidar ~10 Hz. +# No entry for world/video on purpose — the throttle drops whole messages, and dropping +# H.264 samples breaks decoding until the next keyframe. (0 means unlimited, not off.) _BRIDGE_MAX_HZ: dict[str, float] = { # "world/lidar": 2.0, # "world/pointlio_map": 0.5, @@ -178,18 +163,16 @@ def _rerun_config( } -# Bridge streams + teleop only: no mapping, no planning. cmd_vel still reaches the robot -# through MovementManager, so this is the layer to drive from when something upstream is -# suspect. +# Streams + teleop only. cmd_vel still reaches the robot through MovementManager, so this +# is the layer to drive from when something upstream is suspect. go2_zenoh_basic = autoconnect( vis_module(viewer_backend=global_config.viewer, rerun_config=_rerun_config()), GO2Zenoh.blueprint(mid360_mount_rpy_deg=MID360_MOUNT_RPY_DEG), MovementManager.blueprint(), ).global_config(transport="zenoh", n_workers=4, robot_model="unitree_go2") -# Adds the voxel map. It consumes GO2Zenoh's lidar + odometry directly: the bridge stamps -# them exactly as PointLio does locally (frames odom / mid360_link, xyz+intensity at -# point_step 16). +# Consumes GO2Zenoh's lidar + odometry directly: the bridge stamps them exactly as +# PointLio does locally (frames odom / mid360_link, xyz+intensity at point_step 16). _ray_tracer = RayTracingVoxelMap.blueprint( voxel_size=voxel_size, emit_every=1, diff --git a/dimos/robot/unitree/go2/zenoh/zenohconnection.py b/dimos/robot/unitree/go2/zenoh/zenohconnection.py index d44c34c5fb7..154a05a716b 100644 --- a/dimos/robot/unitree/go2/zenoh/zenohconnection.py +++ b/dimos/robot/unitree/go2/zenoh/zenohconnection.py @@ -14,17 +14,13 @@ """The Go2 as it appears on the graph when it runs the go2web zenoh bridge. -The bridge (go2web ``src/dimos_zenoh.rs``, on by default via the ``dimos_zenoh`` cargo -feature) publishes Point-LIO odom, the per-scan and world clouds and H.264 video -straight onto dimos's own zenoh keys, and consumes ``cmd_vel`` and ``command``. Nothing -here produces the streams — declaring the ports is what puts the robot's topics on the -graph, the same arrangement as a NativeModule whose binary publishes out of process (see -:class:`PointLio`). The port names are the wire contract: the bridge's keys are -``dimos//``, so renaming a port here silently disconnects it. Remap in -the blueprint instead. - -What the bridge does not send, and this module supplies: the ``odom -> mid360_link`` tf -edge, the static mount tree, and the camera intrinsics. +The bridge (go2web ``src/dimos_zenoh.rs``) publishes odom, the clouds and H.264 video and +consumes ``cmd_vel``/``command``; nothing here produces them, declaring the ports is what +puts them on the graph. Port names are the wire contract — keys are +``dimos//`` — so remap in the blueprint rather than renaming. + +This module adds what the bridge does not send: the ``odom -> mid360_link`` tf edge, the +static mount tree, and the camera intrinsics. """ from __future__ import annotations @@ -51,9 +47,8 @@ from dimos.protocol.tf.static_tf_publisher import StaticTfPublisher, StaticTfPublisherConfig from dimos.robot.unitree.go2.connection import _camera_info_static -# Mount geometry, measured on this rig (metres). Kept here rather than shared with -# go2_mid360_static_transforms: that describes the recording rig, which carries the -# lidar at a different angle and hangs its tree off base_link. +# Mount geometry measured on this rig (metres). Not go2_mid360_static_transforms — that +# is the recording rig: different lidar angle, tree hung off base_link. CAMERA_XYZ = Vector3(0.32715, -0.00003, 0.04297) # base_link -> front_camera MID360_XYZ = Vector3(-0.032, 0.0, 0.12) # front_camera -> mid360_link: 3.2cm back, 12cm up # rpy mapping a sensor frame to its optical frame (x-right, y-down, z-forward) @@ -61,12 +56,9 @@ class GO2ZenohConfig(StaticTfPublisherConfig): - # Mid-360 mount rotation on the front_camera -> mid360_link edge: fixed-axis rpy, - # degrees. The lidar is yawed 90 deg on its bracket, so its 60 deg downward tilt - # lands on roll about the lidar's own x-axis rather than pitch — Point-LIO reports - # (-60.7, -1.4, +0.8) for this rig standing still, the 1.4 being the dog's stance. - # Both yaw signs level the body; they differ by 180 deg of heading, so flip the sign - # if the camera ends up looking backwards. + # front_camera -> mid360_link, fixed-axis rpy in degrees. The 60 deg tilt lands on + # roll because the lidar sits yawed 90 deg on its bracket. Both yaw signs level the + # body but differ by 180 deg of heading — flip it if the camera looks backwards. mid360_mount_rpy_deg: tuple[float, float, float] = (-60.0, 0.0, -90.0) camera_info_hz: float = Field(default=1.0, gt=0.0) @@ -78,9 +70,8 @@ class GO2Zenoh(StaticTfPublisher): # Consumed by the on-robot bridge, never published here. cmd_vel: In[Twist] - # One-shot action verbs ("sit", "hello", ...) or a bare sport api id as a - # string. Consumed by the bridge too, but nothing else in the graph produces - # them, so the rpcs below publish onto it — no zenoh RPC, just a topic. + # Action verbs ("sit", "hello", ...) or a bare sport api id. The rpcs below publish + # onto it; nothing else in the graph does. command: In[String] odometry: Out[Odometry] lidar: Out[PointCloud2] # per-scan, in the LIO's own sensor frame @@ -100,9 +91,8 @@ def start(self) -> None: ) self.spawn(self._publish_camera_info()) - # Off the calling thread: start() has to return for the rest of the graph to come - # up, and the first verbs go nowhere until zenoh has matched our command publisher - # against the bridge's subscriber. Cancelled by stop() if we never get that far. + # Off the calling thread so start() returns; verbs sent before zenoh matches our + # publisher against the bridge are dropped. timer = threading.Timer(3.0, self._startup_pose) timer.daemon = True timer.start() @@ -159,11 +149,9 @@ def set_lidar(self, enabled: bool) -> None: def transforms(self) -> list[Transform]: """The mount tree, rooted at mid360_link because Point-LIO owns that frame. - The rig is measured outward from the body, but the only live edge here is - odom -> mid360_link. Publishing the measured direction as-is would leave - mid360_link with two parents (odom and front_camera) and the body would snap - between them at 35 Hz, so the two edges above the lidar are inverted and the - body hangs off the sensor instead. + Measured outward from the body, but odom -> mid360_link is the only live edge, so + the two edges above the lidar are inverted — otherwise mid360_link has two parents + and the body snaps between them at 35 Hz. """ base_to_camera = Transform( translation=CAMERA_XYZ, diff --git a/pyproject.toml b/pyproject.toml index 464cf93b7e5..0d2ce745e51 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -138,7 +138,7 @@ dependencies = [ # TODO: rerun shouldn't be required but rn its in core (there is NO WAY to use dimos without rerun rn) # remove this once rerun is optional in core "rerun-sdk==0.32.0", - "dimos-viewer==0.32.0a1", + "dimos-viewer==0.32.0a2", "toolz>=1.1.0", "protobuf>=6.33.5,<7", "psutil>=7.0.0", @@ -199,7 +199,7 @@ misc = [ visualization = [ "rerun-sdk==0.32.0", - "dimos-viewer==0.32.0a1", + "dimos-viewer==0.32.0a2", # Rerun URDF robot visualization. yourdfpy depends on trimesh[easy], # which pulls embreex; embreex has no Linux aarch64 wheel. "yourdfpy>=0.0.60; sys_platform != 'linux' or platform_machine != 'aarch64'", diff --git a/uv.lock b/uv.lock index dcac4524a0e..42782ed63e2 100644 --- a/uv.lock +++ b/uv.lock @@ -2022,8 +2022,8 @@ requires-dist = [ { name = "dimos", extras = ["base", "mapping"], marker = "extra == 'unitree'" }, { name = "dimos", extras = ["unitree"], marker = "extra == 'unitree-dds'" }, { name = "dimos-lcm", specifier = ">=0.1.3" }, - { name = "dimos-viewer", specifier = "==0.32.0a1" }, - { name = "dimos-viewer", marker = "extra == 'visualization'", specifier = "==0.32.0a1" }, + { name = "dimos-viewer", specifier = "==0.32.0a2" }, + { name = "dimos-viewer", marker = "extra == 'visualization'", specifier = "==0.32.0a2" }, { name = "drake", marker = "platform_machine != 'aarch64' and sys_platform == 'darwin' and extra == 'manipulation'", specifier = "==1.45.0" }, { name = "drake", marker = "platform_machine != 'aarch64' and sys_platform != 'darwin' and extra == 'manipulation'", specifier = ">=1.40.0" }, { name = "eclipse-zenoh", specifier = ">=1.0.0,<2.0" }, @@ -2306,18 +2306,18 @@ wheels = [ [[package]] name = "dimos-viewer" -version = "0.32.0a1" -source = { registry = "https://pypi.org/simple" } -wheels = [ - { url = "https://files.pythonhosted.org/packages/da/12/f5ccd760ed27c930d08289589f420e3d3e4c707cb44bc4bd85a3e7b8cd18/dimos_viewer-0.32.0a1-cp310-cp310-macosx_11_0_arm64.whl", hash = "sha256:1c0aa61c7369337af9fea5d49ad648fbee36ed50cfdf2463450a61866fc73783", size = 38326738, upload-time = "2026-05-19T14:54:55.116Z" }, - { url = "https://files.pythonhosted.org/packages/d8/fa/b1e1e0d7d125dfbcb5465af22a44de56ae892d309f07231d4a462b5d6c1b/dimos_viewer-0.32.0a1-cp310-cp310-manylinux_2_28_aarch64.whl", hash = "sha256:4763823cc555069b703c980a8fa938e1c8f858f1a8acf00b6e257ac7f5be7e93", size = 42253912, upload-time = "2026-05-19T14:54:58.58Z" }, - { url = "https://files.pythonhosted.org/packages/09/d9/cccad09df71d14b84f95fb384213d0d2eb36efb25db11fafe12e3f27454e/dimos_viewer-0.32.0a1-cp310-cp310-manylinux_2_28_x86_64.whl", hash = "sha256:6db249eff2014f746e7b840a9354dc57e182d8b05bee360fdc934f34c0d4a6da", size = 44794930, upload-time = "2026-05-19T14:55:02.2Z" }, - { url = "https://files.pythonhosted.org/packages/ec/37/394f74ad8981c4666d9014189f3fb154e7f796c408d770d94f52ed4b1ccb/dimos_viewer-0.32.0a1-cp311-cp311-macosx_11_0_arm64.whl", hash = "sha256:86f2dc0f5db732b28ce5d9f13749a12b26f3b3c891e68bae8fcb4c065f08ddf5", size = 38326731, upload-time = "2026-05-19T14:55:05.95Z" }, - { url = "https://files.pythonhosted.org/packages/35/0d/677674fd9a8ac73689642cd7c4bb1b177df9d096f39a74dadd1420ffc221/dimos_viewer-0.32.0a1-cp311-cp311-manylinux_2_28_aarch64.whl", hash = "sha256:f22176cea73d6e898230432de19ca07772a7c1293171ad86dae0b7e54e1fbbf5", size = 42253900, upload-time = "2026-05-19T14:55:09.56Z" }, - { url = "https://files.pythonhosted.org/packages/fd/c3/0c32f2a5e0908dcab3d8b652704d68124cbe2d74ca418561e503fa216cff/dimos_viewer-0.32.0a1-cp311-cp311-manylinux_2_28_x86_64.whl", hash = "sha256:f4a4b6e1aa83b50421a7133b22c27c0924b2f984a242d30273087f66928af5d8", size = 44794937, upload-time = "2026-05-19T14:55:12.888Z" }, - { url = "https://files.pythonhosted.org/packages/3f/51/efcf47f154b5940d09d6e480f3f75f00a927396d9be1f5b2523b1ff9d62e/dimos_viewer-0.32.0a1-cp312-cp312-macosx_11_0_arm64.whl", hash = "sha256:a4cc88e26567955ce964a250c7250cd3d52e99f330d892420699f0580de8c4f3", size = 38326735, upload-time = "2026-05-19T14:55:16.477Z" }, - { url = "https://files.pythonhosted.org/packages/fa/3e/7564e12f24f4678ba5e311a538eda0a2e46add9afcd58d08984e66bc280c/dimos_viewer-0.32.0a1-cp312-cp312-manylinux_2_28_aarch64.whl", hash = "sha256:68354192271a201a71491245a7b444b0e4047b74c9d2496a395010356917c0c3", size = 42253910, upload-time = "2026-05-19T14:55:20.232Z" }, - { url = "https://files.pythonhosted.org/packages/ca/4b/d97f13e3585582c79037cd24952ac99de593a30c300ddcc8c83d9cab1128/dimos_viewer-0.32.0a1-cp312-cp312-manylinux_2_28_x86_64.whl", hash = "sha256:ebdd64ce20d73e9d6e0256ef099a3d544c14b9b227aa005ba8c362052889b3c4", size = 44794790, upload-time = "2026-05-19T14:55:24.211Z" }, +version = "0.32.0a2" +source = { registry = "https://pypi.org/simple" } +wheels = [ + { url = "https://files.pythonhosted.org/packages/2c/57/df6e81ca83379e264a63bb87fc9fceb3dc7e445300ac8655756be3619ec5/dimos_viewer-0.32.0a2-cp310-cp310-macosx_11_0_arm64.whl", hash = "sha256:22281f7db2d4524710f3bb68a68a75e7017868b9d4fd4a58673876afcff2e3d9", size = 38327656, upload-time = "2026-07-22T18:49:37.633Z" }, + { url = "https://files.pythonhosted.org/packages/5d/ca/3c610ccb10c334e06b40c8d0894801323dbebd506746011c989cd0473b20/dimos_viewer-0.32.0a2-cp310-cp310-manylinux_2_28_aarch64.whl", hash = "sha256:f75f72d39c8cde15ebb17e3edefca3523992d3991ec44f54fb004840ba077f7e", size = 42255685, upload-time = "2026-07-22T18:49:40.867Z" }, + { url = "https://files.pythonhosted.org/packages/49/2a/b413d4f86bc306dd26f6ee1206cc3f4f0df6e3f9e9ec2a4d20ec5824d616/dimos_viewer-0.32.0a2-cp310-cp310-manylinux_2_28_x86_64.whl", hash = "sha256:c38af675e708494aad8d66b4a7a35974633400d897e4dd73f9dba8562c04282a", size = 44799199, upload-time = "2026-07-22T18:49:44.014Z" }, + { url = "https://files.pythonhosted.org/packages/da/ae/159ff85671a4f13d683ed181ab2d917747a7cc837b7aa81a9b32a4aaad19/dimos_viewer-0.32.0a2-cp311-cp311-macosx_11_0_arm64.whl", hash = "sha256:f284e5d9271334c93190c64d6eff7fd4addc5f1ad19c3598f1976a93e3ff85fd", size = 38327655, upload-time = "2026-07-22T18:49:47.112Z" }, + { url = "https://files.pythonhosted.org/packages/04/67/68a3bab1a817a9f143f7f50355afb93c45d999814f386c3a0c7c7cd6becc/dimos_viewer-0.32.0a2-cp311-cp311-manylinux_2_28_aarch64.whl", hash = "sha256:1d44158e854109c48c243640d8a88cc2eced3c5bcd1932b93080252e45de4ca6", size = 42255680, upload-time = "2026-07-22T18:49:51.011Z" }, + { url = "https://files.pythonhosted.org/packages/8e/b1/15d3579109ee6b6f8aa93642340eced7c444849433fc04ed0c3715f963ae/dimos_viewer-0.32.0a2-cp311-cp311-manylinux_2_28_x86_64.whl", hash = "sha256:c13479d8266606b1e7dcf1296d091b229374a83d84ba16849b2bbd830413ed5f", size = 44799215, upload-time = "2026-07-22T18:49:54.529Z" }, + { url = "https://files.pythonhosted.org/packages/5c/d1/9582095e8b2fdc8708afe3e9e4b9bfaba925c7d53ede8e675bf556bdd593/dimos_viewer-0.32.0a2-cp312-cp312-macosx_11_0_arm64.whl", hash = "sha256:bdeb607ffef680d1e56dcdf5cd7c11eca148dbb2e957cbd43b8642efe0857d3a", size = 38327650, upload-time = "2026-07-22T18:49:57.804Z" }, + { url = "https://files.pythonhosted.org/packages/f3/60/f48f22a1ab1a8208d9cc6892ceb37c74baf80859be7f3c75e76a3e082ddf/dimos_viewer-0.32.0a2-cp312-cp312-manylinux_2_28_aarch64.whl", hash = "sha256:4f22e03a6c1290911c467723407c5ed8fd189a4c6447d32647feb706b87dc727", size = 42255679, upload-time = "2026-07-22T18:50:01.071Z" }, + { url = "https://files.pythonhosted.org/packages/2e/15/b38b583cdf07082d58ca33500343da8153d7859133e2c924922873364c42/dimos_viewer-0.32.0a2-cp312-cp312-manylinux_2_28_x86_64.whl", hash = "sha256:4a32c39c8cc8e9a03f2cc9d8446e7d49a132692f833cbd7277c185f171ede614", size = 44799194, upload-time = "2026-07-22T18:50:04.462Z" }, ] [[package]] From e384a922e1c63142657b0bc40542b5db2482ffdc Mon Sep 17 00:00:00 2001 From: Ivan Nikolic Date: Wed, 22 Jul 2026 22:29:57 +0300 Subject: [PATCH 13/16] added dan planner, blueprint cleanup --- dimos/robot/all_blueprints.py | 1 + dimos/robot/unitree/go2/zenoh/blueprints.py | 121 +++++++++----------- 2 files changed, 58 insertions(+), 64 deletions(-) diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 05b0fe88984..0090be58a49 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -64,6 +64,7 @@ "drone-basic": "dimos.robot.drone.blueprints.basic.drone_basic:drone_basic", "dual-xarm6-planner": "dimos.robot.manipulators.xarm.blueprints.basic:dual_xarm6_planner", "go2-zenoh-basic": "dimos.robot.unitree.go2.zenoh.blueprints:go2_zenoh_basic", + "go2-zenoh-htc": "dimos.robot.unitree.go2.zenoh.blueprints:go2_zenoh_htc", "go2-zenoh-nav": "dimos.robot.unitree.go2.zenoh.blueprints:go2_zenoh_nav", "go2-zenoh-raycaster": "dimos.robot.unitree.go2.zenoh.blueprints:go2_zenoh_raycaster", "keyboard-teleop-a1z": "dimos.robot.manipulators.a1z.blueprints.teleop:keyboard_teleop_a1z", diff --git a/dimos/robot/unitree/go2/zenoh/blueprints.py b/dimos/robot/unitree/go2/zenoh/blueprints.py index 6e00f75e29c..803554e69eb 100644 --- a/dimos/robot/unitree/go2/zenoh/blueprints.py +++ b/dimos/robot/unitree/go2/zenoh/blueprints.py @@ -22,6 +22,8 @@ - ``go2-zenoh-basic`` — streams plus teleop; the bridge, tf and camera, no mapping. - ``go2-zenoh-raycaster`` — adds :class:`RayTracingVoxelMap`. - ``go2-zenoh-nav`` — the full stack: planner, goal relay and path follower. +- ``go2-zenoh-htc`` — ``go2-zenoh-nav`` with the follower swapped for the + ``DanLocalPlanner`` + ``DanHolonomicTC`` pair from ``unitree-go2-mls-htc``. """ import math @@ -34,6 +36,8 @@ from dimos.msgs.geometry_msgs.Vector3 import Vector3 from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 from dimos.navigation.basic_path_follower.module import BasicPathFollower +from dimos.navigation.dannav.holonomic_tc.module import DanHolonomicTC +from dimos.navigation.dannav.local_planner.module import DanLocalPlanner from dimos.navigation.movement_manager.movement_manager import MovementManager from dimos.navigation.nav_3d.mls_planner.goal_relay import GoalRelay from dimos.navigation.nav_3d.mls_planner.mls_planner_native import MLSPlannerNative @@ -45,7 +49,7 @@ voxel_size = 0.08 # Raise above 0 (2.0 works) to draw what the planner searched over: surface, nodes and # cost-coloured edges. Drives both its publishing and the rerun overrides. -planner_viz_hz = 0.0 +planner_viz_hz = 2.0 # Feeds both the static tf GO2Zenoh publishes and the rotation that levels its odometry — # they must agree or nav steers off-heading. Verified against Point-LIO's own attitude. @@ -109,12 +113,8 @@ def _convert_map(grid: PointCloud2) -> Any: return grid.to_rerun(voxel_size=0.025) -def _convert_scan(grid: PointCloud2) -> Any: - return grid.to_rerun(color=[255, 0, 0], voxel_size=0.05) - - def _render_global_map(msg: Any) -> Any: - return msg.to_rerun() + return msg.to_rerun(voxel_size=0.02) def _render_path(msg: Any) -> Any: @@ -125,41 +125,21 @@ def _render_path(msg: Any) -> Any: return msg -# What the bridge itself sends, present in all three blueprints. -_BRIDGE_OVERRIDE = { - "world/camera_info": _camera_info_to_pinhole, - "world/pointlio_map": _convert_map, - "world/lidar": _convert_scan, -} -# The clouds are what costs: world maps up to 300k points at ~2 Hz, per-scan lidar ~10 Hz. -# No entry for world/video on purpose — the throttle drops whole messages, and dropping -# H.264 samples breaks decoding until the next keyframe. (0 means unlimited, not off.) -_BRIDGE_MAX_HZ: dict[str, float] = { - # "world/lidar": 2.0, - # "world/pointlio_map": 0.5, -} - -_MAP_OVERRIDE = {"world/global_map": _render_global_map} -_MAP_MAX_HZ = { - # Already rate-limited at the source by global_emit_every, roughly every 5s. - "world/global_map": 0, - "world/local_map": 0.5, -} - -_NAV_OVERRIDE = { - "world/path": _render_path, - **planner_visual_override(planner_viz_hz), -} - - def _rerun_config( visual_override: dict[str, Any] | None = None, max_hz: dict[str, Any] | None = None ) -> dict[str, Any]: """The bridge's own view, plus whatever the layer above it adds.""" return { "blueprint": _rerun_blueprint, - "visual_override": {**_BRIDGE_OVERRIDE, **(visual_override or {})}, - "max_hz": {**_BRIDGE_MAX_HZ, **(max_hz or {})}, + "visual_override": { + "world/camera_info": _camera_info_to_pinhole, + "world/pointlio_map": _convert_map, + "world/lidar": None, + "world/local_map": None, + "world/global_map": _render_global_map, + "world/path": _render_path, + **planner_visual_override(planner_viz_hz), + }, } @@ -171,8 +151,7 @@ def _rerun_config( MovementManager.blueprint(), ).global_config(transport="zenoh", n_workers=4, robot_model="unitree_go2") -# Consumes GO2Zenoh's lidar + odometry directly: the bridge stamps them exactly as -# PointLio does locally (frames odom / mid360_link, xyz+intensity at point_step 16). +# Shared by the raycaster/nav/htc layers below. _ray_tracer = RayTracingVoxelMap.blueprint( voxel_size=voxel_size, emit_every=1, @@ -182,42 +161,56 @@ def _rerun_config( support_min=4, ) +# global_map is remapped off so the planner runs purely on the +# incremental local_map + region_bounds pair. +_mls_planner = MLSPlannerNative.blueprint( + world_frame="odom", + voxel_size=voxel_size, + robot_height=0.3, + surface_closing_radius=0.3, + wall_clearance_m=0.1, + wall_buffer_m=0.75, + wall_buffer_weight=100.0, + step_threshold_m=0.16, + step_penalty_weight=4.0, + viz_publish_hz=planner_viz_hz, +).remappings([(MLSPlannerNative, "global_map", "global_map_unused")]) + +# Consumes GO2Zenoh's lidar + odometry directly: the bridge stamps them exactly as +# PointLio does locally (frames odom / mid360_link, xyz+intensity at point_step 16). go2_zenoh_raycaster = autoconnect( - vis_module( - viewer_backend=global_config.viewer, rerun_config=_rerun_config(_MAP_OVERRIDE, _MAP_MAX_HZ) - ), - GO2Zenoh.blueprint(mid360_mount_rpy_deg=MID360_MOUNT_RPY_DEG), + go2_zenoh_basic, _ray_tracer, - MovementManager.blueprint(), ).global_config(transport="zenoh", n_workers=6, robot_model="unitree_go2") go2_zenoh_nav = autoconnect( - vis_module( - viewer_backend=global_config.viewer, - rerun_config=_rerun_config({**_MAP_OVERRIDE, **_NAV_OVERRIDE}, _MAP_MAX_HZ), - ), - GO2Zenoh.blueprint(mid360_mount_rpy_deg=MID360_MOUNT_RPY_DEG), - # Level the tilted-sensor odometry into the body frame so the follower steers on a - # true heading. The ray tracer keeps the raw sensor odometry. + go2_zenoh_raycaster, + _mls_planner, OdomBodyFrame.blueprint(mount_rotation=_mount_rotation()), - _ray_tracer, - # global_map is remapped off so the planner runs purely on the - # incremental local_map + region_bounds pair. - MLSPlannerNative.blueprint( - world_frame="odom", - voxel_size=voxel_size, - robot_height=0.3, - surface_closing_radius=0.3, - wall_clearance_m=0.1, - wall_buffer_m=0.75, - wall_buffer_weight=100.0, - step_threshold_m=0.16, - step_penalty_weight=4.0, - viz_publish_hz=planner_viz_hz, - ).remappings([(MLSPlannerNative, "global_map", "global_map_unused")]), GoalRelay.blueprint(), BasicPathFollower.blueprint(speed=0.5, heading_gain=0.4, max_angular=0.6).remappings( [(BasicPathFollower, "odometry", "body_odometry")] ), MovementManager.blueprint(), ).global_config(transport="zenoh", n_workers=8, robot_model="unitree_go2") + +# The nav stack with BasicPathFollower swapped for the DanLocalPlanner + DanHolonomicTC +# pair from unitree-go2-mls-htc. The raw planner stream moves to planner_path; the gate +# forwards committed paths on path, so world/planner_path is muted in rerun. +go2_zenoh_htc = autoconnect( + go2_zenoh_raycaster, + OdomBodyFrame.blueprint(mount_rotation=_mount_rotation()), + _mls_planner.remappings([(MLSPlannerNative, "path", "planner_path")]), + # Fed the leveled odometry, so its start_pose doubles as the body-frame PoseStamped + # the Dan modules consume — mirroring mls_htc, where planner start and follower odom + # are the same topic. + GoalRelay.blueprint().remappings([(GoalRelay, "odometry", "body_odometry")]), + # Setting resample_spacing_m to > 0.0 will smooth out jagged paths returned by MLSP + DanLocalPlanner.blueprint(resample_spacing_m=0.1).remappings( + [(DanLocalPlanner, "odom", "start_pose")] + ), + DanHolonomicTC.blueprint(run_profile="walk").remappings( + [(DanHolonomicTC, "odom", "start_pose")] + ), + MovementManager.blueprint(), +).global_config(transport="zenoh", n_workers=9, robot_model="unitree_go2") From e1f18b32acee3bc1c6d9b82c0b9d3d660ff03cd0 Mon Sep 17 00:00:00 2001 From: Ivan Nikolic Date: Wed, 22 Jul 2026 22:43:00 +0300 Subject: [PATCH 14/16] blueprint cleanup 2 --- dimos/robot/unitree/go2/zenoh/blueprints.py | 45 +++++++++---------- .../unitree/go2/zenoh/zenohconnection.py | 2 +- 2 files changed, 22 insertions(+), 25 deletions(-) diff --git a/dimos/robot/unitree/go2/zenoh/blueprints.py b/dimos/robot/unitree/go2/zenoh/blueprints.py index 803554e69eb..3dbd791c58b 100644 --- a/dimos/robot/unitree/go2/zenoh/blueprints.py +++ b/dimos/robot/unitree/go2/zenoh/blueprints.py @@ -34,7 +34,6 @@ from dimos.mapping.ray_tracing.module import RayTracingVoxelMap from dimos.msgs.geometry_msgs.Quaternion import Quaternion from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 from dimos.navigation.basic_path_follower.module import BasicPathFollower from dimos.navigation.dannav.holonomic_tc.module import DanHolonomicTC from dimos.navigation.dannav.local_planner.module import DanLocalPlanner @@ -97,7 +96,6 @@ def _rerun_blueprint() -> Any: # viewer. overrides={ "world/pointlio_map": rrb.EntityBehavior(visible=False), - "world/local_map": rrb.EntityBehavior(visible=False), "world/lidar": rrb.EntityBehavior(visible=False), "world/nodes": rrb.EntityBehavior(visible=False), }, @@ -109,11 +107,7 @@ def _rerun_blueprint() -> Any: ) -def _convert_map(grid: PointCloud2) -> Any: - return grid.to_rerun(voxel_size=0.025) - - -def _render_global_map(msg: Any) -> Any: +def _render_map(msg: Any) -> Any: return msg.to_rerun(voxel_size=0.02) @@ -125,20 +119,19 @@ def _render_path(msg: Any) -> Any: return msg -def _rerun_config( - visual_override: dict[str, Any] | None = None, max_hz: dict[str, Any] | None = None -) -> dict[str, Any]: +def _rerun_config(visual_override: dict[str, Any] | None = None) -> dict[str, Any]: """The bridge's own view, plus whatever the layer above it adds.""" return { "blueprint": _rerun_blueprint, "visual_override": { "world/camera_info": _camera_info_to_pinhole, - "world/pointlio_map": _convert_map, + "world/pointlio_map": _render_map, "world/lidar": None, - "world/local_map": None, - "world/global_map": _render_global_map, + "world/local_map": _render_map, + "world/global_map": _render_map, "world/path": _render_path, **planner_visual_override(planner_viz_hz), + **(visual_override or {}), }, } @@ -151,16 +144,6 @@ def _rerun_config( MovementManager.blueprint(), ).global_config(transport="zenoh", n_workers=4, robot_model="unitree_go2") -# Shared by the raycaster/nav/htc layers below. -_ray_tracer = RayTracingVoxelMap.blueprint( - voxel_size=voxel_size, - emit_every=1, - global_emit_every=50, - min_health=-1, - max_health=5, - support_min=4, -) - # global_map is remapped off so the planner runs purely on the # incremental local_map + region_bounds pair. _mls_planner = MLSPlannerNative.blueprint( @@ -180,9 +163,23 @@ def _rerun_config( # PointLio does locally (frames odom / mid360_link, xyz+intensity at point_step 16). go2_zenoh_raycaster = autoconnect( go2_zenoh_basic, - _ray_tracer, + # Re-declared with the pointlio map muted: the raytraced maps replace it here, and + # autoconnect keeps the newest duplicate, so this vis module wins over basic's. + vis_module( + viewer_backend=global_config.viewer, + rerun_config=_rerun_config({"world/pointlio_map": None, "world/lidar": None}), + ), + RayTracingVoxelMap.blueprint( + voxel_size=voxel_size, + emit_every=1, + global_emit_every=50, + min_health=-1, + max_health=5, + support_min=4, + ), ).global_config(transport="zenoh", n_workers=6, robot_model="unitree_go2") + go2_zenoh_nav = autoconnect( go2_zenoh_raycaster, _mls_planner, diff --git a/dimos/robot/unitree/go2/zenoh/zenohconnection.py b/dimos/robot/unitree/go2/zenoh/zenohconnection.py index 154a05a716b..9638a13ba3f 100644 --- a/dimos/robot/unitree/go2/zenoh/zenohconnection.py +++ b/dimos/robot/unitree/go2/zenoh/zenohconnection.py @@ -93,7 +93,7 @@ def start(self) -> None: # Off the calling thread so start() returns; verbs sent before zenoh matches our # publisher against the bridge are dropped. - timer = threading.Timer(3.0, self._startup_pose) + timer = threading.Timer(5.0, self._startup_pose) timer.daemon = True timer.start() self.register_disposable(Disposable(timer.cancel)) From cab4409fd4859626946073f251adbba85f5be13f Mon Sep 17 00:00:00 2001 From: Ivan Nikolic Date: Wed, 22 Jul 2026 23:06:43 +0300 Subject: [PATCH 15/16] nice surface maps --- .../nav_3d/mls_planner/rust/src/main.rs | 47 +++++++++++++++++-- dimos/navigation/nav_3d/mls_planner/viz.py | 45 ++++++++++++++++-- dimos/robot/unitree/go2/zenoh/blueprints.py | 4 +- .../unitree/go2/zenoh/zenohconnection.py | 2 +- 4 files changed, 86 insertions(+), 12 deletions(-) diff --git a/dimos/navigation/nav_3d/mls_planner/rust/src/main.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/main.rs index 8e86561784a..51c79826b05 100644 --- a/dimos/navigation/nav_3d/mls_planner/rust/src/main.rs +++ b/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 = Arc>>; @@ -296,11 +297,15 @@ impl Worker { let frame = &self.config.world_frame; let graph = planner.graph(); - let surface_points: Vec = planner - .surface() - .map(|(ix, iy, iz)| surface_point_xyz(ix, iy, iz, voxel_size)) + let surface_points: Vec = 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 = 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); diff --git a/dimos/navigation/nav_3d/mls_planner/viz.py b/dimos/navigation/nav_3d/mls_planner/viz.py index df92a96eb7c..ae1e9a21b01 100644 --- a/dimos/navigation/nav_3d/mls_planner/viz.py +++ b/dimos/navigation/nav_3d/mls_planner/viz.py @@ -24,6 +24,7 @@ from __future__ import annotations +from functools import partial from typing import TYPE_CHECKING, Any import numpy as np @@ -38,8 +39,30 @@ _GRAPH_Z_LIFT = 0.05 -def render_surface_map(msg: PointCloud2) -> Archetype: - return msg.to_rerun(voxel_size=0.1, colors=[40, 75, 130]) +def render_surface_map( + msg: PointCloud2, + voxel_size: float = 0.1, + wall_clearance_m: float = 0.0, + clearance_clamp_m: float = 1.0, +) -> Archetype: + """Floor cells colored by wall clearance: dark navy where tight, pale blue in the open. + + Clearance rides the cloud's intensity channel; cells below ``wall_clearance_m`` are + untraversable and dropped. Falls back to a flat color when the channel is absent. + """ + import rerun as rr + + pts = msg.points_f32() + clearance = msg.intensities_f32() + if clearance is None or len(clearance) != len(pts): + return msg.to_rerun(voxel_size=voxel_size, colors=[40, 75, 130]) + passable = clearance >= wall_clearance_m + pts, clearance = pts[passable], clearance[passable] + norm = np.clip(np.nan_to_num(clearance / clearance_clamp_m, nan=1.0, posinf=1.0), 0.0, 1.0) + tight = np.array([4.0, 8.0, 48.0]) + open_ = np.array([150.0, 200.0, 255.0]) + colors = (tight + norm[:, None] * (open_ - tight)).astype(np.uint8) + return rr.Points3D(positions=pts, colors=colors, radii=voxel_size / 2) def render_nodes(msg: PointCloud2) -> Archetype: @@ -78,14 +101,26 @@ def render_node_edges(msg: LineSegments3D) -> Archetype: return rr.LineStrips3D(strips, colors=colors, radii=[0.04] * len(strips)) -def planner_visual_override(viz_publish_hz: float) -> dict[str, Any]: +def planner_visual_override( + viz_publish_hz: float, + voxel_size: float = 0.1, + wall_clearance_m: float = 0.0, + clearance_clamp_m: float = 1.0, +) -> dict[str, Any]: """rerun overrides for the planner's debug entities, keyed off its own publish rate. - Pass the same value given to ``MLSPlannerNative.blueprint(viz_publish_hz=...)``. + Pass the same ``viz_publish_hz``, ``voxel_size`` and ``wall_clearance_m`` given to + ``MLSPlannerNative.blueprint(...)``. """ on = viz_publish_hz > 0.0 + surface = partial( + render_surface_map, + voxel_size=voxel_size, + wall_clearance_m=wall_clearance_m, + clearance_clamp_m=clearance_clamp_m, + ) return { - "world/surface_map": render_surface_map if on else None, + "world/surface_map": surface if on else None, "world/nodes": render_nodes if on else None, "world/node_edges": render_node_edges if on else None, } diff --git a/dimos/robot/unitree/go2/zenoh/blueprints.py b/dimos/robot/unitree/go2/zenoh/blueprints.py index 3dbd791c58b..05fb4062545 100644 --- a/dimos/robot/unitree/go2/zenoh/blueprints.py +++ b/dimos/robot/unitree/go2/zenoh/blueprints.py @@ -108,7 +108,7 @@ def _rerun_blueprint() -> Any: def _render_map(msg: Any) -> Any: - return msg.to_rerun(voxel_size=0.02) + return msg.to_rerun(voxel_size=0.01) def _render_path(msg: Any) -> Any: @@ -130,7 +130,7 @@ def _rerun_config(visual_override: dict[str, Any] | None = None) -> dict[str, An "world/local_map": _render_map, "world/global_map": _render_map, "world/path": _render_path, - **planner_visual_override(planner_viz_hz), + **planner_visual_override(planner_viz_hz, voxel_size=voxel_size, wall_clearance_m=0.1), **(visual_override or {}), }, } diff --git a/dimos/robot/unitree/go2/zenoh/zenohconnection.py b/dimos/robot/unitree/go2/zenoh/zenohconnection.py index 9638a13ba3f..2732941d0d5 100644 --- a/dimos/robot/unitree/go2/zenoh/zenohconnection.py +++ b/dimos/robot/unitree/go2/zenoh/zenohconnection.py @@ -93,7 +93,7 @@ def start(self) -> None: # Off the calling thread so start() returns; verbs sent before zenoh matches our # publisher against the bridge are dropped. - timer = threading.Timer(5.0, self._startup_pose) + timer = threading.Timer(4.0, self._startup_pose) timer.daemon = True timer.start() self.register_disposable(Disposable(timer.cancel)) From f671227faaee398dda0fd79d275285804df5d20c Mon Sep 17 00:00:00 2001 From: Ivan Nikolic Date: Wed, 22 Jul 2026 23:14:18 +0300 Subject: [PATCH 16/16] good viz --- dimos/navigation/nav_3d/mls_planner/viz.py | 8 ++++++-- dimos/robot/unitree/go2/zenoh/zenohconnection.py | 2 +- 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/dimos/navigation/nav_3d/mls_planner/viz.py b/dimos/navigation/nav_3d/mls_planner/viz.py index ae1e9a21b01..2964eb6dda8 100644 --- a/dimos/navigation/nav_3d/mls_planner/viz.py +++ b/dimos/navigation/nav_3d/mls_planner/viz.py @@ -62,7 +62,11 @@ def render_surface_map( tight = np.array([4.0, 8.0, 48.0]) open_ = np.array([150.0, 200.0, 255.0]) colors = (tight + norm[:, None] * (open_ - tight)).astype(np.uint8) - return rr.Points3D(positions=pts, colors=colors, radii=voxel_size / 2) + return rr.Points3D( + positions=pts, + colors=colors, + radii=voxel_size * 0.5, + ) def render_nodes(msg: PointCloud2) -> Archetype: @@ -98,7 +102,7 @@ def render_node_edges(msg: LineSegments3D) -> Archetype: ] for p1, p2 in msg._segments ] - return rr.LineStrips3D(strips, colors=colors, radii=[0.04] * len(strips)) + return rr.LineStrips3D(strips, colors=colors, radii=[0.01] * len(strips)) def planner_visual_override( diff --git a/dimos/robot/unitree/go2/zenoh/zenohconnection.py b/dimos/robot/unitree/go2/zenoh/zenohconnection.py index 2732941d0d5..9638a13ba3f 100644 --- a/dimos/robot/unitree/go2/zenoh/zenohconnection.py +++ b/dimos/robot/unitree/go2/zenoh/zenohconnection.py @@ -93,7 +93,7 @@ def start(self) -> None: # Off the calling thread so start() returns; verbs sent before zenoh matches our # publisher against the bridge are dropped. - timer = threading.Timer(4.0, self._startup_pose) + timer = threading.Timer(5.0, self._startup_pose) timer.daemon = True timer.start() self.register_disposable(Disposable(timer.cancel))