From 0acf3759f40907c5c315479ab4ea0cbfd3e5bb08 Mon Sep 17 00:00:00 2001 From: spomichter Date: Sun, 1 Mar 2026 17:07:19 +0000 Subject: [PATCH 1/9] feat(nav): add clicked_point input to replanning planner subscribe to PointStamped clicks from rerun viewer, convert to PoseStamped via .to_pose_stamped() and feed to goal handler. wire with: LCMTransport("/clicked_point", PointStamped) DIM-643 --- dimos/navigation/replanning_a_star/module.py | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/dimos/navigation/replanning_a_star/module.py b/dimos/navigation/replanning_a_star/module.py index 60dfad73ae..4dad9a2843 100644 --- a/dimos/navigation/replanning_a_star/module.py +++ b/dimos/navigation/replanning_a_star/module.py @@ -21,7 +21,7 @@ from dimos.core.global_config import GlobalConfig, global_config from dimos.core.module import Module from dimos.core.stream import In, Out -from dimos.msgs.geometry_msgs import PoseStamped, Twist +from dimos.msgs.geometry_msgs import PointStamped, PoseStamped, Twist from dimos.msgs.nav_msgs import OccupancyGrid, Path from dimos.navigation.base import NavigationInterface, NavigationState from dimos.navigation.replanning_a_star.global_planner import GlobalPlanner @@ -31,6 +31,7 @@ class ReplanningAStarPlanner(Module, NavigationInterface): odom: In[PoseStamped] # TODO: Use TF. global_costmap: In[OccupancyGrid] goal_request: In[PoseStamped] + clicked_point: In[PointStamped] target: In[PoseStamped] goal_reached: Out[Bool] @@ -60,6 +61,14 @@ def start(self) -> None: ) self._disposables.add(Disposable(self.target.subscribe(self._planner.handle_goal_request))) + self._disposables.add( + Disposable( + self.clicked_point.subscribe( + lambda pt: self._planner.handle_goal_request(pt.to_pose_stamped()) + ) + ) + ) + self._disposables.add(self._planner.path.subscribe(self.path.publish)) self._disposables.add(self._planner.cmd_vel.subscribe(self.cmd_vel.publish)) From 5594368bdb79f6e1b3097c06df32cc3e9fc3e32c Mon Sep 17 00:00:00 2001 From: spomichter Date: Sun, 1 Mar 2026 17:32:04 +0000 Subject: [PATCH 2/9] feat(examples): add rerun click-to-navigate test script runs Go2 smart blueprint with viewer_mode=none, connects to custom rerun viewer on gRPC port 9877, wires /clicked_point LCM transport to planner's clicked_point input. DIM-643 --- examples/rerun_click_test.py | 120 +++++++++++++++++++++++++++++++++++ 1 file changed, 120 insertions(+) create mode 100644 examples/rerun_click_test.py diff --git a/examples/rerun_click_test.py b/examples/rerun_click_test.py new file mode 100644 index 0000000000..52523d6263 --- /dev/null +++ b/examples/rerun_click_test.py @@ -0,0 +1,120 @@ +#!/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. + +"""E2E test: Rerun viewer click → Go2 navigation goal. + +Usage: + 1. Start the custom rerun viewer (with LCM click support): + ./target/release/custom_callback_viewer + + 2. Run this script: + uv run python examples/rerun_click_test.py + + 3. Click on entities in the Rerun viewer → robot navigates to click position. + +The custom viewer listens on gRPC port 9877 and publishes PointStamped +clicks to /clicked_point via LCM UDP multicast. + +This script runs the Go2 smart blueprint with viewer_mode="none" (no +built-in viewer) and connects to the custom viewer's gRPC endpoint. +""" + +import signal +import sys +import time + +import rerun as rr + +from dimos.core.blueprints import autoconnect +from dimos.core.transport import LCMTransport +from dimos.mapping.costmapper import cost_mapper +from dimos.mapping.voxels import voxel_mapper +from dimos.msgs.geometry_msgs import PointStamped +from dimos.navigation.replanning_a_star.module import replanning_a_star_planner +from dimos.protocol.pubsub.impl.lcmpubsub import LCM +from dimos.robot.unitree.go2.blueprints.basic.unitree_go2_basic import ( + _convert_camera_info, + _convert_global_map, + _convert_navigation_costmap, + _static_base_link, + _transports_base, +) +from dimos.robot.unitree.go2.connection import go2_connection +from dimos.visualization.rerun.bridge import rerun_bridge +from dimos.web.websocket_vis.websocket_vis_module import websocket_vis + +CUSTOM_VIEWER_URL = "rerun+http://127.0.0.1:9877/proxy" + +rerun_config = { + "pubsubs": [LCM(autoconf=True)], + "viewer_mode": "none", # don't spawn viewer — we use the custom one + "visual_override": { + "world/camera_info": _convert_camera_info, + "world/global_map": _convert_global_map, + "world/navigation_costmap": _convert_navigation_costmap, + }, + "static": { + "world/tf/base_link": _static_base_link, + }, +} + +# Build the blueprint: basic Go2 + nav + click-to-nav transport +with_vis = autoconnect(_transports_base, rerun_bridge(**rerun_config)) + +blueprint = ( + autoconnect( + with_vis, + go2_connection(), + websocket_vis(), + voxel_mapper(voxel_size=0.1), + cost_mapper(), + replanning_a_star_planner(), + ) + .transports( + { + ("clicked_point", PointStamped): LCMTransport("/clicked_point", PointStamped), + } + ) + .global_config(n_workers=6, robot_model="unitree_go2") +) + + +def main() -> None: + print("Building blueprint...") + coordinator = blueprint.build() + + print("Starting modules...") + coordinator.start() + + # Connect to the custom viewer's gRPC endpoint + print(f"Connecting to custom Rerun viewer at {CUSTOM_VIEWER_URL}...") + rr.connect_grpc(CUSTOM_VIEWER_URL) + print("Connected! Click on entities in the Rerun viewer to send navigation goals.") + print("Press Ctrl+C to stop.") + + def shutdown(*_: object) -> None: + print("\nShutting down...") + coordinator.stop() + sys.exit(0) + + signal.signal(signal.SIGINT, shutdown) + signal.signal(signal.SIGTERM, shutdown) + + while True: + time.sleep(1) + + +if __name__ == "__main__": + main() From 60c3a8f7366dedbd2bb4750077fc90123163ac3a Mon Sep 17 00:00:00 2001 From: spomichter Date: Sun, 1 Mar 2026 17:39:59 +0000 Subject: [PATCH 3/9] fix(examples): add --simulation flag to click test supports --simulation (mujoco), --robot-ip, --viewer-url args --- examples/rerun_click_test.py | 98 ++++++++++++++++++++++-------------- 1 file changed, 61 insertions(+), 37 deletions(-) diff --git a/examples/rerun_click_test.py b/examples/rerun_click_test.py index 52523d6263..d289a936b9 100644 --- a/examples/rerun_click_test.py +++ b/examples/rerun_click_test.py @@ -19,8 +19,8 @@ 1. Start the custom rerun viewer (with LCM click support): ./target/release/custom_callback_viewer - 2. Run this script: - uv run python examples/rerun_click_test.py + 2. Run this script (simulation mode): + uv run python examples/rerun_click_test.py --simulation 3. Click on entities in the Rerun viewer → robot navigates to click position. @@ -31,13 +31,16 @@ built-in viewer) and connects to the custom viewer's gRPC endpoint. """ +import argparse import signal import sys import time +from typing import Any import rerun as rr from dimos.core.blueprints import autoconnect +from dimos.core.global_config import global_config from dimos.core.transport import LCMTransport from dimos.mapping.costmapper import cost_mapper from dimos.mapping.voxels import voxel_mapper @@ -57,51 +60,72 @@ CUSTOM_VIEWER_URL = "rerun+http://127.0.0.1:9877/proxy" -rerun_config = { - "pubsubs": [LCM(autoconf=True)], - "viewer_mode": "none", # don't spawn viewer — we use the custom one - "visual_override": { - "world/camera_info": _convert_camera_info, - "world/global_map": _convert_global_map, - "world/navigation_costmap": _convert_navigation_costmap, - }, - "static": { - "world/tf/base_link": _static_base_link, - }, -} - -# Build the blueprint: basic Go2 + nav + click-to-nav transport -with_vis = autoconnect(_transports_base, rerun_bridge(**rerun_config)) - -blueprint = ( - autoconnect( - with_vis, - go2_connection(), - websocket_vis(), - voxel_mapper(voxel_size=0.1), - cost_mapper(), - replanning_a_star_planner(), - ) - .transports( - { - ("clicked_point", PointStamped): LCMTransport("/clicked_point", PointStamped), - } - ) - .global_config(n_workers=6, robot_model="unitree_go2") -) + +def _rerun_config() -> dict[str, Any]: + return { + "pubsubs": [LCM(autoconf=True)], + "viewer_mode": "none", # don't spawn viewer — we use the custom one + "visual_override": { + "world/camera_info": _convert_camera_info, + "world/global_map": _convert_global_map, + "world/navigation_costmap": _convert_navigation_costmap, + }, + "static": { + "world/tf/base_link": _static_base_link, + }, + } def main() -> None: + parser = argparse.ArgumentParser(description="Rerun click-to-navigate test") + parser.add_argument("--simulation", action="store_true", help="Run in simulation (mujoco)") + parser.add_argument("--robot-ip", type=str, default=None, help="Robot IP (for real hardware)") + parser.add_argument( + "--viewer-url", + type=str, + default=CUSTOM_VIEWER_URL, + help="Custom viewer gRPC URL", + ) + args = parser.parse_args() + + # Configure global config + if args.simulation: + global_config.update(simulation=True) + global_config.update(viewer_backend="rerun") + if args.robot_ip: + global_config.update(robot_ip=args.robot_ip) + print("Building blueprint...") + with_vis = autoconnect(_transports_base, rerun_bridge(**_rerun_config())) + + blueprint = ( + autoconnect( + with_vis, + go2_connection(), + websocket_vis(), + voxel_mapper(voxel_size=0.1), + cost_mapper(), + replanning_a_star_planner(), + ) + .transports( + { + ("clicked_point", PointStamped): LCMTransport("/clicked_point", PointStamped), + } + ) + .global_config(n_workers=6, robot_model="unitree_go2") + ) + coordinator = blueprint.build() print("Starting modules...") coordinator.start() # Connect to the custom viewer's gRPC endpoint - print(f"Connecting to custom Rerun viewer at {CUSTOM_VIEWER_URL}...") - rr.connect_grpc(CUSTOM_VIEWER_URL) - print("Connected! Click on entities in the Rerun viewer to send navigation goals.") + print(f"Connecting to custom Rerun viewer at {args.viewer_url}...") + rr.connect_grpc(args.viewer_url) + print("Connected!") + print() + print("Click on entities in the Rerun viewer to send navigation goals.") print("Press Ctrl+C to stop.") def shutdown(*_: object) -> None: From 6fef8dff0a83657ecee98f710b266603471d2963 Mon Sep 17 00:00:00 2001 From: spomichter Date: Sun, 1 Mar 2026 17:51:06 +0000 Subject: [PATCH 4/9] fix(examples): init rerun and connect grpc before coordinator.start rr.init + rr.connect_grpc must happen before the bridge starts, otherwise rerun disables itself with no active sink --- examples/rerun_click_test.py | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/examples/rerun_click_test.py b/examples/rerun_click_test.py index d289a936b9..3607e6b1a1 100644 --- a/examples/rerun_click_test.py +++ b/examples/rerun_click_test.py @@ -91,10 +91,18 @@ def main() -> None: # Configure global config if args.simulation: global_config.update(simulation=True) - global_config.update(viewer_backend="rerun") + global_config.update(viewer_backend="rerun") if args.robot_ip: global_config.update(robot_ip=args.robot_ip) + # Initialize rerun and connect to custom viewer BEFORE building blueprint. + # This must happen before coordinator.start() so the bridge's rr.init("dimos") + # finds an active sink and doesn't disable rerun. + print(f"Connecting to custom Rerun viewer at {args.viewer_url}...") + rr.init("dimos") + rr.connect_grpc(args.viewer_url) + print("Connected to viewer.") + print("Building blueprint...") with_vis = autoconnect(_transports_base, rerun_bridge(**_rerun_config())) @@ -119,11 +127,6 @@ def main() -> None: print("Starting modules...") coordinator.start() - - # Connect to the custom viewer's gRPC endpoint - print(f"Connecting to custom Rerun viewer at {args.viewer_url}...") - rr.connect_grpc(args.viewer_url) - print("Connected!") print() print("Click on entities in the Rerun viewer to send navigation goals.") print("Press Ctrl+C to stop.") From 17f8c86cc68ba22048713f62a943ded0db6320d0 Mon Sep 17 00:00:00 2001 From: spomichter Date: Sun, 1 Mar 2026 17:56:53 +0000 Subject: [PATCH 5/9] fix(examples): connect grpc after bridge start to avoid double rr.init the bridge calls rr.init('dimos') in start(). calling rr.init before that resets the recording stream. now we connect_grpc after start() so the bridge's rr.init creates the stream, then we add our gRPC sink. --- examples/rerun_click_test.py | 70 +++++++++++++++++------------------- 1 file changed, 33 insertions(+), 37 deletions(-) diff --git a/examples/rerun_click_test.py b/examples/rerun_click_test.py index 3607e6b1a1..713e5d5156 100644 --- a/examples/rerun_click_test.py +++ b/examples/rerun_click_test.py @@ -27,17 +27,15 @@ The custom viewer listens on gRPC port 9877 and publishes PointStamped clicks to /clicked_point via LCM UDP multicast. -This script runs the Go2 smart blueprint with viewer_mode="none" (no -built-in viewer) and connects to the custom viewer's gRPC endpoint. +This script uses the standard Go2 smart blueprint but overrides the +viewer_backend to avoid the built-in rerun bridge (which would spawn +its own viewer). Instead it connects to the custom viewer externally. """ import argparse import signal import sys import time -from typing import Any - -import rerun as rr from dimos.core.blueprints import autoconnect from dimos.core.global_config import global_config @@ -47,13 +45,6 @@ from dimos.msgs.geometry_msgs import PointStamped from dimos.navigation.replanning_a_star.module import replanning_a_star_planner from dimos.protocol.pubsub.impl.lcmpubsub import LCM -from dimos.robot.unitree.go2.blueprints.basic.unitree_go2_basic import ( - _convert_camera_info, - _convert_global_map, - _convert_navigation_costmap, - _static_base_link, - _transports_base, -) from dimos.robot.unitree.go2.connection import go2_connection from dimos.visualization.rerun.bridge import rerun_bridge from dimos.web.websocket_vis.websocket_vis_module import websocket_vis @@ -61,21 +52,6 @@ CUSTOM_VIEWER_URL = "rerun+http://127.0.0.1:9877/proxy" -def _rerun_config() -> dict[str, Any]: - return { - "pubsubs": [LCM(autoconf=True)], - "viewer_mode": "none", # don't spawn viewer — we use the custom one - "visual_override": { - "world/camera_info": _convert_camera_info, - "world/global_map": _convert_global_map, - "world/navigation_costmap": _convert_navigation_costmap, - }, - "static": { - "world/tf/base_link": _static_base_link, - }, - } - - def main() -> None: parser = argparse.ArgumentParser(description="Rerun click-to-navigate test") parser.add_argument("--simulation", action="store_true", help="Run in simulation (mujoco)") @@ -88,23 +64,28 @@ def main() -> None: ) args = parser.parse_args() - # Configure global config + # Configure global config BEFORE any imports that read it if args.simulation: global_config.update(simulation=True) - global_config.update(viewer_backend="rerun") if args.robot_ip: global_config.update(robot_ip=args.robot_ip) - # Initialize rerun and connect to custom viewer BEFORE building blueprint. - # This must happen before coordinator.start() so the bridge's rr.init("dimos") - # finds an active sink and doesn't disable rerun. - print(f"Connecting to custom Rerun viewer at {args.viewer_url}...") - rr.init("dimos") - rr.connect_grpc(args.viewer_url) - print("Connected to viewer.") + # Use the rerun bridge with viewer_mode="native" but point it at + # our custom viewer's gRPC port via RERUN_CONNECT_ADDR env var. + # This way the bridge handles rr.init + rr.spawn normally, but + # spawn connects to our already-running custom viewer. + # + # Actually, rr.spawn starts a NEW viewer process. We don't want that. + # Instead: use "none" mode and call connect_grpc AFTER bridge.start(). + global_config.update(viewer_backend="rerun") + + rerun_config = { + "pubsubs": [LCM(autoconf=True)], + "viewer_mode": "none", + } print("Building blueprint...") - with_vis = autoconnect(_transports_base, rerun_bridge(**_rerun_config())) + with_vis = autoconnect(rerun_bridge(**rerun_config)) blueprint = ( autoconnect( @@ -127,6 +108,21 @@ def main() -> None: print("Starting modules...") coordinator.start() + + # Connect to the custom viewer AFTER coordinator.start() so that + # rr.init("dimos") from the bridge has already run. This adds our + # gRPC sink to the existing recording stream. + import rerun as rr + + print(f"Connecting to custom Rerun viewer at {args.viewer_url}...") + rr.connect_grpc(args.viewer_url) + + if not rr.is_enabled(): + print("WARNING: Rerun is disabled. Data won't reach the viewer.") + print("Try: rr.init('dimos') manually before this point.") + else: + print("Connected!") + print() print("Click on entities in the Rerun viewer to send navigation goals.") print("Press Ctrl+C to stop.") From effe6cc9ddfaca82e2908735e83f0813d3792f34 Mon Sep 17 00:00:00 2001 From: spomichter Date: Sun, 1 Mar 2026 18:04:35 +0000 Subject: [PATCH 6/9] fix(examples): init rerun in main process before connect_grpc the bridge runs in a worker subprocess (multiprocessing), so its rr.init() only affects that subprocess. the main process needs its own rr.init() + connect_grpc() for data to reach the viewer. also adds --replay flag for headless testing. --- examples/rerun_click_test.py | 51 +++++++++++++----------------------- 1 file changed, 18 insertions(+), 33 deletions(-) diff --git a/examples/rerun_click_test.py b/examples/rerun_click_test.py index 713e5d5156..8c12f55cca 100644 --- a/examples/rerun_click_test.py +++ b/examples/rerun_click_test.py @@ -22,14 +22,13 @@ 2. Run this script (simulation mode): uv run python examples/rerun_click_test.py --simulation + Or replay mode (no robot, headless): + uv run python examples/rerun_click_test.py --replay + 3. Click on entities in the Rerun viewer → robot navigates to click position. The custom viewer listens on gRPC port 9877 and publishes PointStamped clicks to /clicked_point via LCM UDP multicast. - -This script uses the standard Go2 smart blueprint but overrides the -viewer_backend to avoid the built-in rerun bridge (which would spawn -its own viewer). Instead it connects to the custom viewer externally. """ import argparse @@ -37,6 +36,8 @@ import sys import time +import rerun as rr + from dimos.core.blueprints import autoconnect from dimos.core.global_config import global_config from dimos.core.transport import LCMTransport @@ -55,6 +56,7 @@ def main() -> None: parser = argparse.ArgumentParser(description="Rerun click-to-navigate test") parser.add_argument("--simulation", action="store_true", help="Run in simulation (mujoco)") + parser.add_argument("--replay", action="store_true", help="Run in replay mode (no robot)") parser.add_argument("--robot-ip", type=str, default=None, help="Robot IP (for real hardware)") parser.add_argument( "--viewer-url", @@ -64,27 +66,25 @@ def main() -> None: ) args = parser.parse_args() - # Configure global config BEFORE any imports that read it + # Configure global config if args.simulation: global_config.update(simulation=True) + if args.replay: + global_config.update(replay=True) + global_config.update(viewer_backend="rerun") if args.robot_ip: global_config.update(robot_ip=args.robot_ip) - # Use the rerun bridge with viewer_mode="native" but point it at - # our custom viewer's gRPC port via RERUN_CONNECT_ADDR env var. - # This way the bridge handles rr.init + rr.spawn normally, but - # spawn connects to our already-running custom viewer. - # - # Actually, rr.spawn starts a NEW viewer process. We don't want that. - # Instead: use "none" mode and call connect_grpc AFTER bridge.start(). - global_config.update(viewer_backend="rerun") - - rerun_config = { - "pubsubs": [LCM(autoconf=True)], - "viewer_mode": "none", - } + # Initialize rerun in the MAIN process and connect to the custom viewer. + # The bridge runs in a worker subprocess via multiprocessing, so its + # rr.init() only affects that subprocess. We need our own rr.init() + + # connect_grpc() in the main process for rr.log() calls to reach the viewer. + rr.init("dimos") + rr.connect_grpc(args.viewer_url) + print(f"Connected to custom Rerun viewer at {args.viewer_url}") print("Building blueprint...") + rerun_config = {"pubsubs": [LCM(autoconf=True)], "viewer_mode": "none"} with_vis = autoconnect(rerun_bridge(**rerun_config)) blueprint = ( @@ -108,21 +108,6 @@ def main() -> None: print("Starting modules...") coordinator.start() - - # Connect to the custom viewer AFTER coordinator.start() so that - # rr.init("dimos") from the bridge has already run. This adds our - # gRPC sink to the existing recording stream. - import rerun as rr - - print(f"Connecting to custom Rerun viewer at {args.viewer_url}...") - rr.connect_grpc(args.viewer_url) - - if not rr.is_enabled(): - print("WARNING: Rerun is disabled. Data won't reach the viewer.") - print("Try: rr.init('dimos') manually before this point.") - else: - print("Connected!") - print() print("Click on entities in the Rerun viewer to send navigation goals.") print("Press Ctrl+C to stop.") From 4aa63ec7a82d439a105b095af2dfb2f1c1153e5d Mon Sep 17 00:00:00 2001 From: spomichter Date: Sun, 1 Mar 2026 18:11:49 +0000 Subject: [PATCH 7/9] feat(rerun): add viewer_mode=connect for external viewer support MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit adds connect mode to rerun bridge — connects to an external viewer via grpc instead of spawning a new one. needed for custom viewers like the click-to-navigate fork. example script now works like a normal blueprint. DIM-643 --- dimos/visualization/rerun/bridge.py | 5 +- examples/rerun_click_test.py | 126 ++++++++++------------------ 2 files changed, 46 insertions(+), 85 deletions(-) diff --git a/dimos/visualization/rerun/bridge.py b/dimos/visualization/rerun/bridge.py index af91f1b8b8..47bce27dcf 100644 --- a/dimos/visualization/rerun/bridge.py +++ b/dimos/visualization/rerun/bridge.py @@ -123,7 +123,7 @@ class RerunConvertible(Protocol): def to_rerun(self) -> RerunData: ... -ViewerMode = Literal["native", "web", "none"] +ViewerMode = Literal["native", "web", "connect", "none"] def _default_blueprint() -> Blueprint: @@ -158,6 +158,7 @@ class Config(ModuleConfig): entity_prefix: str = "world" topic_to_entity: Callable[[Any], str] | None = None viewer_mode: ViewerMode = "native" + connect_url: str = "rerun+http://127.0.0.1:9877/proxy" memory_limit: str = "25%" # Blueprint factory: callable(rrb) -> Blueprint for viewer layout configuration @@ -265,6 +266,8 @@ def start(self) -> None: elif self.config.viewer_mode == "web": server_uri = rr.serve_grpc() rr.serve_web_viewer(connect_to=server_uri, open_browser=False) + elif self.config.viewer_mode == "connect": + rr.connect_grpc(self.config.connect_url) # "none" - just init, no viewer (connect externally) if self.config.blueprint: diff --git a/examples/rerun_click_test.py b/examples/rerun_click_test.py index 8c12f55cca..adbdfc7860 100644 --- a/examples/rerun_click_test.py +++ b/examples/rerun_click_test.py @@ -19,110 +19,68 @@ 1. Start the custom rerun viewer (with LCM click support): ./target/release/custom_callback_viewer - 2. Run this script (simulation mode): + 2. Run this script: uv run python examples/rerun_click_test.py --simulation - Or replay mode (no robot, headless): - uv run python examples/rerun_click_test.py --replay - 3. Click on entities in the Rerun viewer → robot navigates to click position. - -The custom viewer listens on gRPC port 9877 and publishes PointStamped -clicks to /clicked_point via LCM UDP multicast. """ -import argparse -import signal -import sys -import time - -import rerun as rr - from dimos.core.blueprints import autoconnect -from dimos.core.global_config import global_config from dimos.core.transport import LCMTransport from dimos.mapping.costmapper import cost_mapper from dimos.mapping.voxels import voxel_mapper from dimos.msgs.geometry_msgs import PointStamped from dimos.navigation.replanning_a_star.module import replanning_a_star_planner from dimos.protocol.pubsub.impl.lcmpubsub import LCM +from dimos.robot.unitree.go2.blueprints.basic.unitree_go2_basic import ( + _convert_camera_info, + _convert_global_map, + _convert_navigation_costmap, + _static_base_link, + _transports_base, +) from dimos.robot.unitree.go2.connection import go2_connection from dimos.visualization.rerun.bridge import rerun_bridge from dimos.web.websocket_vis.websocket_vis_module import websocket_vis CUSTOM_VIEWER_URL = "rerun+http://127.0.0.1:9877/proxy" - -def main() -> None: - parser = argparse.ArgumentParser(description="Rerun click-to-navigate test") - parser.add_argument("--simulation", action="store_true", help="Run in simulation (mujoco)") - parser.add_argument("--replay", action="store_true", help="Run in replay mode (no robot)") - parser.add_argument("--robot-ip", type=str, default=None, help="Robot IP (for real hardware)") - parser.add_argument( - "--viewer-url", - type=str, - default=CUSTOM_VIEWER_URL, - help="Custom viewer gRPC URL", +rerun_config = { + "pubsubs": [LCM(autoconf=True)], + "viewer_mode": "connect", + "connect_url": CUSTOM_VIEWER_URL, + "visual_override": { + "world/camera_info": _convert_camera_info, + "world/global_map": _convert_global_map, + "world/navigation_costmap": _convert_navigation_costmap, + }, + "static": { + "world/tf/base_link": _static_base_link, + }, +} + +with_vis = autoconnect(_transports_base, rerun_bridge(**rerun_config)) + +rerun_click_test = ( + autoconnect( + with_vis, + go2_connection(), + websocket_vis(), + voxel_mapper(voxel_size=0.1), + cost_mapper(), + replanning_a_star_planner(), ) - args = parser.parse_args() - - # Configure global config - if args.simulation: - global_config.update(simulation=True) - if args.replay: - global_config.update(replay=True) - global_config.update(viewer_backend="rerun") - if args.robot_ip: - global_config.update(robot_ip=args.robot_ip) - - # Initialize rerun in the MAIN process and connect to the custom viewer. - # The bridge runs in a worker subprocess via multiprocessing, so its - # rr.init() only affects that subprocess. We need our own rr.init() + - # connect_grpc() in the main process for rr.log() calls to reach the viewer. - rr.init("dimos") - rr.connect_grpc(args.viewer_url) - print(f"Connected to custom Rerun viewer at {args.viewer_url}") - - print("Building blueprint...") - rerun_config = {"pubsubs": [LCM(autoconf=True)], "viewer_mode": "none"} - with_vis = autoconnect(rerun_bridge(**rerun_config)) - - blueprint = ( - autoconnect( - with_vis, - go2_connection(), - websocket_vis(), - voxel_mapper(voxel_size=0.1), - cost_mapper(), - replanning_a_star_planner(), - ) - .transports( - { - ("clicked_point", PointStamped): LCMTransport("/clicked_point", PointStamped), - } - ) - .global_config(n_workers=6, robot_model="unitree_go2") + .transports( + { + ("clicked_point", PointStamped): LCMTransport("/clicked_point", PointStamped), + } ) + .global_config(n_workers=6, robot_model="unitree_go2") +) - coordinator = blueprint.build() - - print("Starting modules...") +if __name__ == "__main__": + coordinator = rerun_click_test.build() coordinator.start() - print() - print("Click on entities in the Rerun viewer to send navigation goals.") - print("Press Ctrl+C to stop.") - - def shutdown(*_: object) -> None: - print("\nShutting down...") - coordinator.stop() - sys.exit(0) + import signal - signal.signal(signal.SIGINT, shutdown) - signal.signal(signal.SIGTERM, shutdown) - - while True: - time.sleep(1) - - -if __name__ == "__main__": - main() + signal.pause() From d507ecf6de1f43acba74a6d2e88f32f3896d8c3b Mon Sep 17 00:00:00 2001 From: spomichter Date: Sun, 1 Mar 2026 18:16:02 +0000 Subject: [PATCH 8/9] fix(examples): use build().loop() per readme convention --- examples/rerun_click_test.py | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/examples/rerun_click_test.py b/examples/rerun_click_test.py index adbdfc7860..f78c88decb 100644 --- a/examples/rerun_click_test.py +++ b/examples/rerun_click_test.py @@ -79,8 +79,4 @@ ) if __name__ == "__main__": - coordinator = rerun_click_test.build() - coordinator.start() - import signal - - signal.pause() + rerun_click_test.build().loop() From 5df284cf774e8cbeef7c2f81e67b3efdf2a880ca Mon Sep 17 00:00:00 2001 From: spomichter Date: Sun, 1 Mar 2026 18:21:17 +0000 Subject: [PATCH 9/9] feat(blueprints): add unitree-go2-click-nav blueprint proper blueprint registered in all_blueprints. run with: dimos run unitree-go2-click-nav --simulation --viewer-backend rerun-connect adds rerun-connect viewer backend that connects to an external rerun viewer (custom fork) instead of spawning a new one. DIM-643 --- dimos/core/global_config.py | 2 +- dimos/robot/all_blueprints.py | 1 + .../go2/blueprints/basic/unitree_go2_basic.py | 6 ++ .../blueprints/smart/unitree_go2_click_nav.py | 39 +++++++++ examples/rerun_click_test.py | 82 ------------------- 5 files changed, 47 insertions(+), 83 deletions(-) create mode 100644 dimos/robot/unitree/go2/blueprints/smart/unitree_go2_click_nav.py delete mode 100644 examples/rerun_click_test.py diff --git a/dimos/core/global_config.py b/dimos/core/global_config.py index 3660a957dc..15c37186ac 100644 --- a/dimos/core/global_config.py +++ b/dimos/core/global_config.py @@ -19,7 +19,7 @@ from dimos.mapping.occupancy.path_map import NavigationStrategy -ViewerBackend: TypeAlias = Literal["rerun", "rerun-web", "foxglove", "none"] +ViewerBackend: TypeAlias = Literal["rerun", "rerun-web", "rerun-connect", "foxglove", "none"] def _get_all_numbers(s: str) -> list[float]: diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 6026572388..2bf585273b 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -76,6 +76,7 @@ "unitree-go2-agentic-mcp": "dimos.robot.unitree.go2.blueprints.agentic.unitree_go2_agentic_mcp:unitree_go2_agentic_mcp", "unitree-go2-agentic-ollama": "dimos.robot.unitree.go2.blueprints.agentic.unitree_go2_agentic_ollama:unitree_go2_agentic_ollama", "unitree-go2-basic": "dimos.robot.unitree.go2.blueprints.basic.unitree_go2_basic:unitree_go2_basic", + "unitree-go2-click-nav": "dimos.robot.unitree.go2.blueprints.smart.unitree_go2_click_nav:unitree_go2_click_nav", "unitree-go2-detection": "dimos.robot.unitree.go2.blueprints.smart.unitree_go2_detection:unitree_go2_detection", "unitree-go2-ros": "dimos.robot.unitree.go2.blueprints.smart.unitree_go2_ros:unitree_go2_ros", "unitree-go2-spatial": "dimos.robot.unitree.go2.blueprints.smart.unitree_go2_spatial:unitree_go2_spatial", diff --git a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_basic.py b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_basic.py index 2537e86632..92e48fbb95 100644 --- a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_basic.py +++ b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_basic.py @@ -105,6 +105,12 @@ def _static_base_link(rr: Any) -> list[Any]: from dimos.visualization.rerun.bridge import rerun_bridge with_vis = autoconnect(_transports_base, rerun_bridge(**rerun_config)) + case "rerun-connect": + from dimos.visualization.rerun.bridge import rerun_bridge + + with_vis = autoconnect( + _transports_base, rerun_bridge(viewer_mode="connect", **rerun_config) + ) case "rerun-web": from dimos.visualization.rerun.bridge import rerun_bridge diff --git a/dimos/robot/unitree/go2/blueprints/smart/unitree_go2_click_nav.py b/dimos/robot/unitree/go2/blueprints/smart/unitree_go2_click_nav.py new file mode 100644 index 0000000000..fb083a289c --- /dev/null +++ b/dimos/robot/unitree/go2/blueprints/smart/unitree_go2_click_nav.py @@ -0,0 +1,39 @@ +#!/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. + +from dimos.core.blueprints import autoconnect +from dimos.core.transport import LCMTransport +from dimos.mapping.costmapper import cost_mapper +from dimos.mapping.voxels import voxel_mapper +from dimos.msgs.geometry_msgs import PointStamped +from dimos.navigation.replanning_a_star.module import replanning_a_star_planner +from dimos.robot.unitree.go2.blueprints.basic.unitree_go2_basic import unitree_go2_basic + +unitree_go2_click_nav = ( + autoconnect( + unitree_go2_basic, + voxel_mapper(voxel_size=0.1), + cost_mapper(), + replanning_a_star_planner(), + ) + .transports( + { + ("clicked_point", PointStamped): LCMTransport("/clicked_point", PointStamped), + } + ) + .global_config(n_workers=6, robot_model="unitree_go2") +) + +__all__ = ["unitree_go2_click_nav"] diff --git a/examples/rerun_click_test.py b/examples/rerun_click_test.py deleted file mode 100644 index f78c88decb..0000000000 --- a/examples/rerun_click_test.py +++ /dev/null @@ -1,82 +0,0 @@ -#!/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. - -"""E2E test: Rerun viewer click → Go2 navigation goal. - -Usage: - 1. Start the custom rerun viewer (with LCM click support): - ./target/release/custom_callback_viewer - - 2. Run this script: - uv run python examples/rerun_click_test.py --simulation - - 3. Click on entities in the Rerun viewer → robot navigates to click position. -""" - -from dimos.core.blueprints import autoconnect -from dimos.core.transport import LCMTransport -from dimos.mapping.costmapper import cost_mapper -from dimos.mapping.voxels import voxel_mapper -from dimos.msgs.geometry_msgs import PointStamped -from dimos.navigation.replanning_a_star.module import replanning_a_star_planner -from dimos.protocol.pubsub.impl.lcmpubsub import LCM -from dimos.robot.unitree.go2.blueprints.basic.unitree_go2_basic import ( - _convert_camera_info, - _convert_global_map, - _convert_navigation_costmap, - _static_base_link, - _transports_base, -) -from dimos.robot.unitree.go2.connection import go2_connection -from dimos.visualization.rerun.bridge import rerun_bridge -from dimos.web.websocket_vis.websocket_vis_module import websocket_vis - -CUSTOM_VIEWER_URL = "rerun+http://127.0.0.1:9877/proxy" - -rerun_config = { - "pubsubs": [LCM(autoconf=True)], - "viewer_mode": "connect", - "connect_url": CUSTOM_VIEWER_URL, - "visual_override": { - "world/camera_info": _convert_camera_info, - "world/global_map": _convert_global_map, - "world/navigation_costmap": _convert_navigation_costmap, - }, - "static": { - "world/tf/base_link": _static_base_link, - }, -} - -with_vis = autoconnect(_transports_base, rerun_bridge(**rerun_config)) - -rerun_click_test = ( - autoconnect( - with_vis, - go2_connection(), - websocket_vis(), - voxel_mapper(voxel_size=0.1), - cost_mapper(), - replanning_a_star_planner(), - ) - .transports( - { - ("clicked_point", PointStamped): LCMTransport("/clicked_point", PointStamped), - } - ) - .global_config(n_workers=6, robot_model="unitree_go2") -) - -if __name__ == "__main__": - rerun_click_test.build().loop()