From 801010504ad031b0c0dcd6f9a63f400c74581359 Mon Sep 17 00:00:00 2001
From: alexlin2 <alex.lin416@outlook.com>
Date: Thu, 18 Sep 2025 17:34:29 -0400
Subject: [PATCH 01/19] created new navbot class

---
 dimos/robot/unitree_webrtc/nav_bot.py    | 171 +++++++++++++++++++++++
 dimos/robot/unitree_webrtc/unitree_g1.py | 163 +++++----------------
 2 files changed, 208 insertions(+), 126 deletions(-)
 create mode 100644 dimos/robot/unitree_webrtc/nav_bot.py

diff --git a/dimos/robot/unitree_webrtc/nav_bot.py b/dimos/robot/unitree_webrtc/nav_bot.py
new file mode 100644
index 0000000000..787adcf2b5
--- /dev/null
+++ b/dimos/robot/unitree_webrtc/nav_bot.py
@@ -0,0 +1,171 @@
+#!/usr/bin/env python3
+# Copyright 2025 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.
+
+"""
+NavBot class for navigation-related functionality.
+Encapsulates ROS bridge and topic remapping for Unitree robots.
+"""
+
+import logging
+
+from dimos import core
+from dimos.core import Module, In, Out, rpc
+from dimos.msgs.geometry_msgs import PoseStamped, TwistStamped, Transform, Vector3
+from dimos.msgs.nav_msgs import Odometry
+from dimos.msgs.sensor_msgs import PointCloud2
+from dimos.msgs.tf2_msgs.TFMessage import TFMessage
+from dimos.protocol.tf import TF
+from dimos.robot.ros_bridge import ROSBridge, BridgeDirection
+from dimos.utils.transform_utils import euler_to_quaternion
+from geometry_msgs.msg import TwistStamped as ROSTwistStamped
+from geometry_msgs.msg import PoseStamped as ROSPoseStamped
+from nav_msgs.msg import Odometry as ROSOdometry
+from sensor_msgs.msg import PointCloud2 as ROSPointCloud2
+from tf2_msgs.msg import TFMessage as ROSTFMessage
+from dimos.utils.logging_config import setup_logger
+
+logger = setup_logger("dimos.robot.unitree_webrtc.nav_bot", level=logging.INFO)
+
+
+class TopicRemapModule(Module):
+    """Module that remaps Odometry to PoseStamped and publishes static transforms."""
+
+    odom: In[Odometry] = None
+    odom_pose: Out[PoseStamped] = None
+
+    def __init__(self, sensor_to_base_link_transform=None, *args, **kwargs):
+        Module.__init__(self, *args, **kwargs)
+        self.tf = TF()
+        self.sensor_to_base_link_transform = sensor_to_base_link_transform or [
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+        ]
+
+    @rpc
+    def start(self):
+        self.odom.subscribe(self._publish_odom_pose)
+        logger.info("TopicRemapModule started")
+
+    def _publish_odom_pose(self, msg: Odometry):
+        pose_msg = PoseStamped(
+            ts=msg.ts,
+            frame_id=msg.frame_id,
+            position=msg.pose.pose.position,
+            orientation=msg.pose.pose.orientation,
+        )
+        self.odom_pose.publish(pose_msg)
+
+        # Publish static transform from sensor to base_link
+        translation = Vector3(
+            self.sensor_to_base_link_transform[0],
+            self.sensor_to_base_link_transform[1],
+            self.sensor_to_base_link_transform[2],
+        )
+        euler_angles = Vector3(
+            self.sensor_to_base_link_transform[3],
+            self.sensor_to_base_link_transform[4],
+            self.sensor_to_base_link_transform[5],
+        )
+        rotation = euler_to_quaternion(euler_angles)
+
+        static_tf = Transform(
+            translation=translation,
+            rotation=rotation,
+            frame_id="sensor",
+            child_frame_id="base_link",
+            ts=msg.ts,
+        )
+        self.tf.publish(static_tf)
+
+
+class NavBot:
+    """
+    NavBot class for navigation-related functionality.
+    Manages ROS bridge and topic remapping for navigation.
+    """
+
+    def __init__(self, dimos=None, sensor_to_base_link_transform=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0]):
+        """
+        Initialize NavBot.
+
+        Args:
+            dimos: DIMOS instance (creates new one if None)
+            sensor_to_base_link_transform: Optional [x, y, z, roll, pitch, yaw] transform from sensor to base_link
+        """
+        if dimos is None:
+            self.dimos = core.start(2)
+        else:
+            self.dimos = dimos
+
+        self.sensor_to_base_link_transform = sensor_to_base_link_transform
+        self.ros_bridge = None
+        self.topic_remap_module = None
+        self.tf = TF()
+
+    def deploy_navigation_modules(self, bridge_name="nav_bot_ros_bridge"):
+        # Deploy topic remap module
+        logger.info("Deploying topic remap module...")
+        self.topic_remap_module = self.dimos.deploy(
+            TopicRemapModule, sensor_to_base_link_transform=self.sensor_to_base_link_transform
+        )
+        self.topic_remap_module.odom.transport = core.LCMTransport("/odom", Odometry)
+        self.topic_remap_module.odom_pose.transport = core.LCMTransport("/odom_pose", PoseStamped)
+
+        # Deploy ROS bridge
+        logger.info("Deploying ROS bridge...")
+        self.ros_bridge = ROSBridge(bridge_name)
+
+        # Configure ROS topics
+        self.ros_bridge.add_topic(
+            "/cmd_vel", TwistStamped, ROSTwistStamped, direction=BridgeDirection.ROS_TO_DIMOS
+        )
+        self.ros_bridge.add_topic(
+            "/state_estimation",
+            Odometry,
+            ROSOdometry,
+            direction=BridgeDirection.ROS_TO_DIMOS,
+            remap_topic="/odom",
+        )
+        self.ros_bridge.add_topic(
+            "/tf", TFMessage, ROSTFMessage, direction=BridgeDirection.ROS_TO_DIMOS
+        )
+        self.ros_bridge.add_topic(
+            "/registered_scan", PointCloud2, ROSPointCloud2, direction=BridgeDirection.ROS_TO_DIMOS
+        )
+        self.ros_bridge.add_topic(
+            "/odom_pose", PoseStamped, ROSPoseStamped, direction=BridgeDirection.DIMOS_TO_ROS
+        )
+
+    def start_navigation_modules(self):
+        if self.topic_remap_module:
+            self.topic_remap_module.start()
+            logger.info("Topic remap module started")
+
+        if self.ros_bridge:
+            logger.info("ROS bridge started")
+
+    def shutdown_navigation(self):
+        logger.info("Shutting down navigation modules...")
+
+        if self.ros_bridge is not None:
+            try:
+                self.ros_bridge.shutdown()
+                logger.info("ROS bridge shut down successfully")
+            except Exception as e:
+                logger.error(f"Error shutting down ROS bridge: {e}")
diff --git a/dimos/robot/unitree_webrtc/unitree_g1.py b/dimos/robot/unitree_webrtc/unitree_g1.py
index f319e2c87c..0293e63541 100644
--- a/dimos/robot/unitree_webrtc/unitree_g1.py
+++ b/dimos/robot/unitree_webrtc/unitree_g1.py
@@ -23,28 +23,10 @@
 import time
 
 from dimos import core
-from dimos.agents2 import Agent
-from dimos.agents2.cli.human import HumanInput
-from dimos.agents2.skills.ros_navigation import RosNavigation
-from dimos.agents2.spec import Model, Provider
-from dimos.core import In, Module, Out, rpc
-from dimos.core.dimos import Dimos
-from dimos.core.resource import Resource
-from dimos.hardware.camera import zed
-from dimos.hardware.camera.module import CameraModule
-from dimos.hardware.camera.webcam import Webcam
-from dimos.msgs.foxglove_msgs import ImageAnnotations
-from dimos.msgs.geometry_msgs import (
-    PoseStamped,
-    Quaternion,
-    Transform,
-    Twist,
-    TwistStamped,
-    Vector3,
-)
-from dimos.msgs.nav_msgs.Odometry import Odometry
-from dimos.msgs.sensor_msgs import CameraInfo, Image, Joy, PointCloud2
-from dimos.msgs.std_msgs.Bool import Bool
+from dimos.constants import DEFAULT_CAPACITY_COLOR_IMAGE, DEFAULT_CAPACITY_DEPTH_IMAGE
+from dimos.core import Module, In, Out, rpc
+from dimos.msgs.geometry_msgs import PoseStamped, Twist, TwistStamped
+from dimos.msgs.sensor_msgs import Image, CameraInfo, PointCloud2
 from dimos.msgs.tf2_msgs.TFMessage import TFMessage
 from dimos.msgs.vision_msgs import Detection2DArray
 from dimos.perception.detection2d.moduleDB import ObjectDBModule
@@ -52,6 +34,11 @@
 from dimos.protocol import pubsub
 from dimos.protocol.pubsub.lcmpubsub import LCM
 from dimos.robot.foxglove_bridge import FoxgloveBridge
+from dimos.web.websocket_vis.websocket_vis_module import WebsocketVisModule
+from dimos.robot.unitree_webrtc.connection import UnitreeWebRTCConnection
+from dimos.robot.unitree_webrtc.unitree_skills import MyUnitreeSkills
+from dimos.robot.unitree_webrtc.nav_bot import NavBot
+from dimos.skills.skills import SkillLibrary
 from dimos.robot.robot import Robot
 from dimos.robot.ros_bridge import BridgeDirection, ROSBridge
 from dimos.robot.unitree_webrtc.connection import UnitreeWebRTCConnection
@@ -85,9 +72,6 @@ class G1ConnectionModule(Module):
     """Simplified connection module for G1 - uses WebRTC for control."""
 
     movecmd: In[TwistStamped] = None
-    odom_in: In[Odometry] = None
-
-    odom_pose: Out[PoseStamped] = None
     ip: str
     connection_type: str = "webrtc"
 
@@ -99,46 +83,20 @@ def __init__(self, ip: str = None, connection_type: str = "webrtc", *args, **kwa
 
     @rpc
     def start(self):
-        """Start the connection and subscribe to sensor streams."""
-
-        super().start()
-
-        # Use the exact same UnitreeWebRTCConnection as Go2
         self.connection = UnitreeWebRTCConnection(self.ip)
-        self.connection.start()
-        unsub = self.movecmd.subscribe(self.move)
-        self._disposables.add(Disposable(unsub))
-        unsub = self.odom_in.subscribe(self._publish_odom_pose)
-        self._disposables.add(Disposable(unsub))
-
-    @rpc
-    def stop(self) -> None:
-        self.connection.stop()
-        super().stop()
-
-    def _publish_odom_pose(self, msg: Odometry):
-        self.odom_pose.publish(
-            PoseStamped(
-                ts=msg.ts,
-                frame_id=msg.frame_id,
-                position=msg.pose.pose.position,
-                orientation=msg.pose.orientation,
-            )
-        )
+        self.movecmd.subscribe(self.move)
 
     @rpc
     def move(self, twist_stamped: TwistStamped, duration: float = 0.0):
-        """Send movement command to robot."""
         twist = Twist(linear=twist_stamped.linear, angular=twist_stamped.angular)
         self.connection.move(twist, duration)
 
     @rpc
     def publish_request(self, topic: str, data: dict):
-        """Forward WebRTC publish requests to connection."""
         return self.connection.publish_request(topic, data)
 
 
-class UnitreeG1(Robot, Resource):
+class UnitreeG1(Robot, NavBot):
     """Unitree G1 humanoid robot."""
 
     def __init__(
@@ -151,9 +109,8 @@ def __init__(
         replay_path: str = None,
         enable_joystick: bool = False,
         enable_connection: bool = True,
-        enable_ros_bridge: bool = True,
-        enable_perception: bool = False,
         enable_camera: bool = False,
+        sensor_to_base_link_transform=None,
     ):
         """Initialize the G1 robot.
 
@@ -166,18 +123,17 @@ def __init__(
             replay_path: Path to replay recordings from (if replaying)
             enable_joystick: Enable pygame joystick control
             enable_connection: Enable robot connection module
-            enable_ros_bridge: Enable ROS bridge
-            enable_camera: Enable web camera module
+            enable_camera: Enable ZED camera module
+            sensor_to_base_link_transform: Optional [x, y, z, roll, pitch, yaw] transform from sensor to base_link
         """
-        super().__init__()
+        Robot.__init__(self)
+        NavBot.__init__(self, sensor_to_base_link_transform=sensor_to_base_link_transform)
         self.ip = ip
         self.output_dir = output_dir or os.path.join(os.getcwd(), "assets", "output")
         self.recording_path = recording_path
         self.replay_path = replay_path
         self.enable_joystick = enable_joystick
         self.enable_connection = enable_connection
-        self.enable_ros_bridge = enable_ros_bridge
-        self.enable_perception = enable_perception
         self.enable_camera = enable_camera
         self.websocket_port = websocket_port
         self.lcm = LCM()
@@ -193,15 +149,13 @@ def __init__(
         self.capabilities = [RobotCapability.LOCOMOTION]
 
         # Module references
-        self._dimos = Dimos(n=4)
         self.connection = None
         self.websocket_vis = None
         self.foxglove_bridge = None
         self.spatial_memory_module = None
         self.joystick = None
-        self.ros_bridge = None
-        self.camera = None
-        self._ros_nav = None
+        self.zed_camera = None
+
         self._setup_directories()
 
     def _setup_directories(self):
@@ -253,8 +207,7 @@ def _deploy_detection(self, goto):
         self.detection = detection
 
     def start(self):
-        self.lcm.start()
-        self._dimos.start()
+        """Start the robot system with all modules."""
 
         if self.enable_connection:
             self._deploy_connection()
@@ -267,8 +220,7 @@ def start(self):
         if self.enable_joystick:
             self._deploy_joystick()
 
-        if self.enable_ros_bridge:
-            self._deploy_ros_bridge()
+        self.deploy_navigation_modules(bridge_name="g1_ros_bridge")
 
         self.nav = self._dimos.deploy(NavigationModule)
         self.nav.goal_reached.transport = core.LCMTransport("/goal_reached", Bool)
@@ -327,12 +279,8 @@ def stop(self) -> None:
 
     def _deploy_connection(self):
         """Deploy and configure the connection module."""
-        self.connection = self._dimos.deploy(G1ConnectionModule, self.ip)
-
-        # Configure LCM transports
+        self.connection = self.dimos.deploy(G1ConnectionModule, self.ip)
         self.connection.movecmd.transport = core.LCMTransport("/cmd_vel", TwistStamped)
-        self.connection.odom_in.transport = core.LCMTransport("/state_estimation", Odometry)
-        self.connection.odom_pose.transport = core.LCMTransport("/odom", PoseStamped)
 
     def _deploy_camera(self):
         """Deploy and configure a standard webcam module."""
@@ -398,61 +346,12 @@ def _deploy_joystick(self):
         self.joystick.twist_out.transport = core.LCMTransport("/cmd_vel", Twist)
         logger.info("Joystick module deployed - pygame window will open")
 
-    def _deploy_ros_bridge(self):
-        """Deploy and configure ROS bridge."""
-        self.ros_bridge = ROSBridge("g1_ros_bridge")
-
-        # Add /cmd_vel topic from ROS to DIMOS
-        self.ros_bridge.add_topic(
-            "/cmd_vel", TwistStamped, ROSTwistStamped, direction=BridgeDirection.ROS_TO_DIMOS
-        )
-
-        # Add /state_estimation topic from ROS to DIMOS
-        self.ros_bridge.add_topic(
-            "/state_estimation", Odometry, ROSOdometry, direction=BridgeDirection.ROS_TO_DIMOS
-        )
-
-        # Add /tf topic from ROS to DIMOS
-        self.ros_bridge.add_topic(
-            "/tf", TFMessage, ROSTFMessage, direction=BridgeDirection.ROS_TO_DIMOS
-        )
-
-        from geometry_msgs.msg import PoseStamped as ROSPoseStamped
-        from std_msgs.msg import Bool as ROSBool
-
-        from dimos.msgs.std_msgs import Bool
-
-        # Navigation control topics from autonomy stack
-        self.ros_bridge.add_topic(
-            "/goal_pose", PoseStamped, ROSPoseStamped, direction=BridgeDirection.DIMOS_TO_ROS
-        )
-        self.ros_bridge.add_topic(
-            "/cancel_goal", Bool, ROSBool, direction=BridgeDirection.DIMOS_TO_ROS
-        )
-        self.ros_bridge.add_topic(
-            "/goal_reached", Bool, ROSBool, direction=BridgeDirection.ROS_TO_DIMOS
-        )
-
-        self.ros_bridge.add_topic("/joy", Joy, ROSJoy, direction=BridgeDirection.DIMOS_TO_ROS)
-
-        self.ros_bridge.add_topic(
-            "/registered_scan",
-            PointCloud2,
-            ROSPointCloud2,
-            direction=BridgeDirection.ROS_TO_DIMOS,
-            remap_topic="/map",
-        )
-
-        self.ros_bridge.start()
-
-        logger.info(
-            "ROS bridge deployed: /cmd_vel, /state_estimation, /tf, /registered_scan (ROS → DIMOS)"
-        )
-
     def _start_modules(self):
         """Start all deployed modules."""
         self._dimos.start_all_modules()
 
+        self.start_navigation_modules()
+
         # Initialize skills after connection is established
         if self.skill_library is not None:
             for skill in self.skill_library:
@@ -472,6 +371,20 @@ def get_odom(self) -> PoseStamped:
         # Note: odom functionality removed from G1ConnectionModule
         return None
 
+    def shutdown(self):
+        """Shutdown the robot and clean up resources."""
+        logger.info("Shutting down UnitreeG1...")
+
+        self.shutdown_navigation()
+
+        if self.websocket_vis:
+            try:
+                self.websocket_vis.stop()
+            except Exception as e:
+                logger.error(f"Error stopping websocket vis: {e}")
+
+        logger.info("UnitreeG1 shutdown complete")
+
     @property
     def spatial_memory(self) -> Optional[SpatialMemory]:
         return self.spatial_memory_module
@@ -506,8 +419,6 @@ def main():
         enable_joystick=args.joystick,
         enable_camera=args.camera,
         enable_connection=os.getenv("ROBOT_IP") is not None,
-        enable_ros_bridge=True,
-        enable_perception=True,
     )
     robot.start()
 

From 983b371c493d9e81b0f6fcc8012789a0090fe1ac Mon Sep 17 00:00:00 2001
From: alexlin2 <alex.lin416@outlook.com>
Date: Sat, 20 Sep 2025 04:59:36 -0400
Subject: [PATCH 02/19] Added super duper README, navbot fully supports ros
 navigation

---
 AUTONOMY_STACK_README.md                 | 178 +++++++++++++++++++++++
 dimos/robot/unitree_webrtc/nav_bot.py    |  13 ++
 dimos/robot/unitree_webrtc/unitree_g1.py |   4 +-
 3 files changed, 192 insertions(+), 3 deletions(-)
 create mode 100644 AUTONOMY_STACK_README.md

diff --git a/AUTONOMY_STACK_README.md b/AUTONOMY_STACK_README.md
new file mode 100644
index 0000000000..ca8f2d2876
--- /dev/null
+++ b/AUTONOMY_STACK_README.md
@@ -0,0 +1,178 @@
+# Autonomy Stack API Documentation
+
+## Quick Start
+
+### Setting Robot Configuration
+```bash
+export ROBOT_CONFIG_PATH="mechanum_drive"  # or "unitree/unitree_g1" or "unitree/unitree_b1"
+```
+
+## ROS Topics
+
+### Input Topics (Commands)
+
+| Topic | Type | Description |
+|-------|------|-------------|
+| `/way_point` | `geometry_msgs/PointStamped` | Send navigation goal (position only) |
+| `/goal_pose` | `geometry_msgs/PoseStamped` | Send goal with orientation |
+| `/cancel_goal` | `std_msgs/Bool` | Cancel current goal (data: true) |
+| `/joy` | `sensor_msgs/Joy` | Joystick input |
+| `/navigation_boundary` | `geometry_msgs/PolygonStamped` | Set navigation boundaries |
+| `/added_obstacles` | `sensor_msgs/PointCloud2` | Virtual obstacles |
+
+### Output Topics (Status)
+
+| Topic | Type | Description |
+|-------|------|-------------|
+| `/state_estimation` | `nav_msgs/Odometry` | Robot pose from SLAM |
+| `/registered_scan` | `sensor_msgs/PointCloud2` | Aligned lidar point cloud |
+| `/terrain_map` | `sensor_msgs/PointCloud2` | Local terrain map |
+| `/terrain_map_ext` | `sensor_msgs/PointCloud2` | Extended terrain map |
+| `/path` | `nav_msgs/Path` | Local path being followed |
+| `/cmd_vel` | `geometry_msgs/Twist` | Velocity commands to motors |
+| `/goal_reached` | `std_msgs/Bool` | True when goal reached, false when cancelled/new goal |
+
+### Map Topics
+
+| Topic | Type | Description |
+|-------|------|-------------|
+| `/overall_map` | `sensor_msgs/PointCloud2` | Global map (only in sim)|
+| `/registered_scan` | `sensor_msgs/PointCloud2` | Current scan in map frame |
+| `/terrain_map` | `sensor_msgs/PointCloud2` | Local obstacle map |
+
+## Usage Examples
+
+### Send Goal
+```bash
+ros2 topic pub /way_point geometry_msgs/msg/PointStamped "{
+  header: {frame_id: 'map'},
+  point: {x: 5.0, y: 3.0, z: 0.0}
+}" --once
+```
+
+### Cancel Goal
+```bash
+ros2 topic pub /cancel_goal std_msgs/msg/Bool "data: true" --once
+```
+
+### Monitor Robot State
+```bash
+ros2 topic echo /state_estimation
+```
+
+## Configuration Parameters
+
+### Vehicle Parameters (`localPlanner`)
+
+| Parameter | Default | Description |
+|-----------|---------|-------------|
+| `vehicleLength` | 0.5 | Robot length (m) |
+| `vehicleWidth` | 0.5 | Robot width (m) |
+| `maxSpeed` | 0.875 | Maximum speed (m/s) |
+| `autonomySpeed` | 0.875 | Autonomous mode speed (m/s) |
+
+### Goal Tolerance Parameters
+
+| Parameter | Default | Description |
+|-----------|---------|-------------|
+| `goalReachedThreshold` | 0.3-0.5 | Distance to consider goal reached (m) |
+| `goalClearRange` | 0.35-0.6 | Extra clearance around goal (m) |
+| `goalBehindRange` | 0.35-0.8 | Stop pursuing if goal behind within this distance (m) |
+| `omniDirGoalThre` | 1.0 | Distance for omnidirectional approach (m) |
+
+### Obstacle Avoidance
+
+| Parameter | Default | Description |
+|-----------|---------|-------------|
+| `obstacleHeightThre` | 0.1-0.2 | Height threshold for obstacles (m) |
+| `adjacentRange` | 3.5 | Sensor range for planning (m) |
+| `minRelZ` | -0.4 | Minimum relative height to consider (m) |
+| `maxRelZ` | 0.3 | Maximum relative height to consider (m) |
+
+### Path Planning
+
+| Parameter | Default | Description |
+|-----------|---------|-------------|
+| `pathScale` | 0.875 | Path resolution scale |
+| `minPathScale` | 0.675 | Minimum path scale when blocked |
+| `minPathRange` | 0.8 | Minimum planning range (m) |
+| `dirThre` | 90.0 | Direction threshold (degrees) |
+
+### Control Parameters (`pathFollower`)
+
+| Parameter | Default | Description |
+|-----------|---------|-------------|
+| `lookAheadDis` | 0.5 | Look-ahead distance (m) |
+| `maxAccel` | 2.0 | Maximum acceleration (m/s²) |
+| `slowDwnDisThre` | 0.875 | Slow down distance threshold (m) |
+
+### SLAM Blind Zones (`feature_extraction_node`)
+
+| Parameter | Mecanum | Description |
+|-----------|---------|-------------|
+| `blindFront` | 0.1 | Front blind zone (m) |
+| `blindBack` | -0.2 | Back blind zone (m) |
+| `blindLeft` | 0.1 | Left blind zone (m) |
+| `blindRight` | -0.1 | Right blind zone (m) |
+| `blindDiskRadius` | 0.4 | Cylindrical blind zone radius (m) |
+
+## Operating Modes
+
+### Mode Control
+- **Joystick L2**: Hold for autonomy mode
+- **Joystick R2**: Hold to disable obstacle checking
+
+### Speed Control
+The robot automatically adjusts speed based on:
+1. Obstacle proximity
+2. Path complexity
+3. Goal distance
+
+## Tuning Guide
+
+### For Tighter Navigation
+- Decrease `goalReachedThreshold` (e.g., 0.2)
+- Decrease `goalClearRange` (e.g., 0.3)
+- Decrease `vehicleLength/Width` slightly
+
+### For Smoother Navigation
+- Increase `goalReachedThreshold` (e.g., 0.5)
+- Increase `lookAheadDis` (e.g., 0.7)
+- Decrease `maxAccel` (e.g., 1.5)
+
+### For Aggressive Obstacle Avoidance
+- Increase `obstacleHeightThre` (e.g., 0.15)
+- Increase `adjacentRange` (e.g., 4.0)
+- Increase blind zone parameters
+
+## Common Issues
+
+### Robot Oscillates at Goal
+- Increase `goalReachedThreshold`
+- Increase `goalBehindRange`
+
+### Robot Stops Too Far from Goal
+- Decrease `goalReachedThreshold`
+- Decrease `goalClearRange`
+
+### Robot Hits Low Obstacles
+- Decrease `obstacleHeightThre`
+- Adjust `minRelZ` to include lower points
+
+## SLAM Configuration
+
+### Localization Mode
+Set in `livox_mid360.yaml`:
+```yaml
+local_mode: true
+init_x: 0.0
+init_y: 0.0
+init_yaw: 0.0
+```
+
+### Mapping Performance
+```yaml
+mapping_line_resolution: 0.1   # Decrease for higher quality
+mapping_plane_resolution: 0.2  # Decrease for higher quality
+max_iterations: 5               # Increase for better accuracy
+```
\ No newline at end of file
diff --git a/dimos/robot/unitree_webrtc/nav_bot.py b/dimos/robot/unitree_webrtc/nav_bot.py
index 787adcf2b5..bdf9d376e4 100644
--- a/dimos/robot/unitree_webrtc/nav_bot.py
+++ b/dimos/robot/unitree_webrtc/nav_bot.py
@@ -25,6 +25,7 @@
 from dimos.msgs.geometry_msgs import PoseStamped, TwistStamped, Transform, Vector3
 from dimos.msgs.nav_msgs import Odometry
 from dimos.msgs.sensor_msgs import PointCloud2
+from dimos_lcm.std_msgs import Bool
 from dimos.msgs.tf2_msgs.TFMessage import TFMessage
 from dimos.protocol.tf import TF
 from dimos.robot.ros_bridge import ROSBridge, BridgeDirection
@@ -33,6 +34,7 @@
 from geometry_msgs.msg import PoseStamped as ROSPoseStamped
 from nav_msgs.msg import Odometry as ROSOdometry
 from sensor_msgs.msg import PointCloud2 as ROSPointCloud2
+from std_msgs.msg import Bool as ROSBool
 from tf2_msgs.msg import TFMessage as ROSTFMessage
 from dimos.utils.logging_config import setup_logger
 
@@ -152,6 +154,17 @@ def deploy_navigation_modules(self, bridge_name="nav_bot_ros_bridge"):
             "/odom_pose", PoseStamped, ROSPoseStamped, direction=BridgeDirection.DIMOS_TO_ROS
         )
 
+        # Navigation control topics from autonomy stack
+        self.ros_bridge.add_topic(
+            "/goal_pose", PoseStamped, ROSPoseStamped, direction=BridgeDirection.ROS_TO_DIMOS
+        )
+        self.ros_bridge.add_topic(
+            "/cancel_goal", Bool, ROSBool, direction=BridgeDirection.ROS_TO_DIMOS
+        )
+        self.ros_bridge.add_topic(
+            "/goal_reached", Bool, ROSBool, direction=BridgeDirection.DIMOS_TO_ROS
+        )
+
     def start_navigation_modules(self):
         if self.topic_remap_module:
             self.topic_remap_module.start()
diff --git a/dimos/robot/unitree_webrtc/unitree_g1.py b/dimos/robot/unitree_webrtc/unitree_g1.py
index 0293e63541..5274829222 100644
--- a/dimos/robot/unitree_webrtc/unitree_g1.py
+++ b/dimos/robot/unitree_webrtc/unitree_g1.py
@@ -110,7 +110,6 @@ def __init__(
         enable_joystick: bool = False,
         enable_connection: bool = True,
         enable_camera: bool = False,
-        sensor_to_base_link_transform=None,
     ):
         """Initialize the G1 robot.
 
@@ -124,10 +123,9 @@ def __init__(
             enable_joystick: Enable pygame joystick control
             enable_connection: Enable robot connection module
             enable_camera: Enable ZED camera module
-            sensor_to_base_link_transform: Optional [x, y, z, roll, pitch, yaw] transform from sensor to base_link
         """
         Robot.__init__(self)
-        NavBot.__init__(self, sensor_to_base_link_transform=sensor_to_base_link_transform)
+        NavBot.__init__(self, sensor_to_base_link_transform=[0.0, 0.0, -0.04, 0.0, 0.0, 0.0])
         self.ip = ip
         self.output_dir = output_dir or os.path.join(os.getcwd(), "assets", "output")
         self.recording_path = recording_path

From 19d3a6cb6d64b9b2e184ce16c581781277f6af1b Mon Sep 17 00:00:00 2001
From: alexlin2 <alex.lin416@outlook.com>
Date: Mon, 22 Sep 2025 15:22:56 -0400
Subject: [PATCH 03/19] added navigate to goal and cancel navigation to navbot

---
 dimos/msgs/std_msgs/Bool.py              |   1 -
 dimos/msgs/std_msgs/__init__.py          |   3 +-
 dimos/robot/unitree_webrtc/nav_bot.py    | 184 -----------------------
 dimos/robot/unitree_webrtc/unitree_g1.py |  54 +++----
 4 files changed, 22 insertions(+), 220 deletions(-)
 delete mode 100644 dimos/robot/unitree_webrtc/nav_bot.py

diff --git a/dimos/msgs/std_msgs/Bool.py b/dimos/msgs/std_msgs/Bool.py
index 6af250277e..1303f25da9 100644
--- a/dimos/msgs/std_msgs/Bool.py
+++ b/dimos/msgs/std_msgs/Bool.py
@@ -16,7 +16,6 @@
 """Bool message type."""
 
 from typing import ClassVar
-
 from dimos_lcm.std_msgs import Bool as LCMBool
 
 try:
diff --git a/dimos/msgs/std_msgs/__init__.py b/dimos/msgs/std_msgs/__init__.py
index 898b1035b5..aa4792dce3 100644
--- a/dimos/msgs/std_msgs/__init__.py
+++ b/dimos/msgs/std_msgs/__init__.py
@@ -15,5 +15,6 @@
 from .Bool import Bool
 from .Header import Header
 from .Int32 import Int32
+from .Bool import Bool
 
-__all__ = ["Bool", "Header", "Int32"]
+__all__ = ["Header", "Int32", "Bool"]
diff --git a/dimos/robot/unitree_webrtc/nav_bot.py b/dimos/robot/unitree_webrtc/nav_bot.py
deleted file mode 100644
index bdf9d376e4..0000000000
--- a/dimos/robot/unitree_webrtc/nav_bot.py
+++ /dev/null
@@ -1,184 +0,0 @@
-#!/usr/bin/env python3
-# Copyright 2025 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.
-
-"""
-NavBot class for navigation-related functionality.
-Encapsulates ROS bridge and topic remapping for Unitree robots.
-"""
-
-import logging
-
-from dimos import core
-from dimos.core import Module, In, Out, rpc
-from dimos.msgs.geometry_msgs import PoseStamped, TwistStamped, Transform, Vector3
-from dimos.msgs.nav_msgs import Odometry
-from dimos.msgs.sensor_msgs import PointCloud2
-from dimos_lcm.std_msgs import Bool
-from dimos.msgs.tf2_msgs.TFMessage import TFMessage
-from dimos.protocol.tf import TF
-from dimos.robot.ros_bridge import ROSBridge, BridgeDirection
-from dimos.utils.transform_utils import euler_to_quaternion
-from geometry_msgs.msg import TwistStamped as ROSTwistStamped
-from geometry_msgs.msg import PoseStamped as ROSPoseStamped
-from nav_msgs.msg import Odometry as ROSOdometry
-from sensor_msgs.msg import PointCloud2 as ROSPointCloud2
-from std_msgs.msg import Bool as ROSBool
-from tf2_msgs.msg import TFMessage as ROSTFMessage
-from dimos.utils.logging_config import setup_logger
-
-logger = setup_logger("dimos.robot.unitree_webrtc.nav_bot", level=logging.INFO)
-
-
-class TopicRemapModule(Module):
-    """Module that remaps Odometry to PoseStamped and publishes static transforms."""
-
-    odom: In[Odometry] = None
-    odom_pose: Out[PoseStamped] = None
-
-    def __init__(self, sensor_to_base_link_transform=None, *args, **kwargs):
-        Module.__init__(self, *args, **kwargs)
-        self.tf = TF()
-        self.sensor_to_base_link_transform = sensor_to_base_link_transform or [
-            0.0,
-            0.0,
-            0.0,
-            0.0,
-            0.0,
-            0.0,
-        ]
-
-    @rpc
-    def start(self):
-        self.odom.subscribe(self._publish_odom_pose)
-        logger.info("TopicRemapModule started")
-
-    def _publish_odom_pose(self, msg: Odometry):
-        pose_msg = PoseStamped(
-            ts=msg.ts,
-            frame_id=msg.frame_id,
-            position=msg.pose.pose.position,
-            orientation=msg.pose.pose.orientation,
-        )
-        self.odom_pose.publish(pose_msg)
-
-        # Publish static transform from sensor to base_link
-        translation = Vector3(
-            self.sensor_to_base_link_transform[0],
-            self.sensor_to_base_link_transform[1],
-            self.sensor_to_base_link_transform[2],
-        )
-        euler_angles = Vector3(
-            self.sensor_to_base_link_transform[3],
-            self.sensor_to_base_link_transform[4],
-            self.sensor_to_base_link_transform[5],
-        )
-        rotation = euler_to_quaternion(euler_angles)
-
-        static_tf = Transform(
-            translation=translation,
-            rotation=rotation,
-            frame_id="sensor",
-            child_frame_id="base_link",
-            ts=msg.ts,
-        )
-        self.tf.publish(static_tf)
-
-
-class NavBot:
-    """
-    NavBot class for navigation-related functionality.
-    Manages ROS bridge and topic remapping for navigation.
-    """
-
-    def __init__(self, dimos=None, sensor_to_base_link_transform=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0]):
-        """
-        Initialize NavBot.
-
-        Args:
-            dimos: DIMOS instance (creates new one if None)
-            sensor_to_base_link_transform: Optional [x, y, z, roll, pitch, yaw] transform from sensor to base_link
-        """
-        if dimos is None:
-            self.dimos = core.start(2)
-        else:
-            self.dimos = dimos
-
-        self.sensor_to_base_link_transform = sensor_to_base_link_transform
-        self.ros_bridge = None
-        self.topic_remap_module = None
-        self.tf = TF()
-
-    def deploy_navigation_modules(self, bridge_name="nav_bot_ros_bridge"):
-        # Deploy topic remap module
-        logger.info("Deploying topic remap module...")
-        self.topic_remap_module = self.dimos.deploy(
-            TopicRemapModule, sensor_to_base_link_transform=self.sensor_to_base_link_transform
-        )
-        self.topic_remap_module.odom.transport = core.LCMTransport("/odom", Odometry)
-        self.topic_remap_module.odom_pose.transport = core.LCMTransport("/odom_pose", PoseStamped)
-
-        # Deploy ROS bridge
-        logger.info("Deploying ROS bridge...")
-        self.ros_bridge = ROSBridge(bridge_name)
-
-        # Configure ROS topics
-        self.ros_bridge.add_topic(
-            "/cmd_vel", TwistStamped, ROSTwistStamped, direction=BridgeDirection.ROS_TO_DIMOS
-        )
-        self.ros_bridge.add_topic(
-            "/state_estimation",
-            Odometry,
-            ROSOdometry,
-            direction=BridgeDirection.ROS_TO_DIMOS,
-            remap_topic="/odom",
-        )
-        self.ros_bridge.add_topic(
-            "/tf", TFMessage, ROSTFMessage, direction=BridgeDirection.ROS_TO_DIMOS
-        )
-        self.ros_bridge.add_topic(
-            "/registered_scan", PointCloud2, ROSPointCloud2, direction=BridgeDirection.ROS_TO_DIMOS
-        )
-        self.ros_bridge.add_topic(
-            "/odom_pose", PoseStamped, ROSPoseStamped, direction=BridgeDirection.DIMOS_TO_ROS
-        )
-
-        # Navigation control topics from autonomy stack
-        self.ros_bridge.add_topic(
-            "/goal_pose", PoseStamped, ROSPoseStamped, direction=BridgeDirection.ROS_TO_DIMOS
-        )
-        self.ros_bridge.add_topic(
-            "/cancel_goal", Bool, ROSBool, direction=BridgeDirection.ROS_TO_DIMOS
-        )
-        self.ros_bridge.add_topic(
-            "/goal_reached", Bool, ROSBool, direction=BridgeDirection.DIMOS_TO_ROS
-        )
-
-    def start_navigation_modules(self):
-        if self.topic_remap_module:
-            self.topic_remap_module.start()
-            logger.info("Topic remap module started")
-
-        if self.ros_bridge:
-            logger.info("ROS bridge started")
-
-    def shutdown_navigation(self):
-        logger.info("Shutting down navigation modules...")
-
-        if self.ros_bridge is not None:
-            try:
-                self.ros_bridge.shutdown()
-                logger.info("ROS bridge shut down successfully")
-            except Exception as e:
-                logger.error(f"Error shutting down ROS bridge: {e}")
diff --git a/dimos/robot/unitree_webrtc/unitree_g1.py b/dimos/robot/unitree_webrtc/unitree_g1.py
index 5274829222..afa34bf159 100644
--- a/dimos/robot/unitree_webrtc/unitree_g1.py
+++ b/dimos/robot/unitree_webrtc/unitree_g1.py
@@ -25,19 +25,15 @@
 from dimos import core
 from dimos.constants import DEFAULT_CAPACITY_COLOR_IMAGE, DEFAULT_CAPACITY_DEPTH_IMAGE
 from dimos.core import Module, In, Out, rpc
-from dimos.msgs.geometry_msgs import PoseStamped, Twist, TwistStamped
-from dimos.msgs.sensor_msgs import Image, CameraInfo, PointCloud2
-from dimos.msgs.tf2_msgs.TFMessage import TFMessage
-from dimos.msgs.vision_msgs import Detection2DArray
-from dimos.perception.detection2d.moduleDB import ObjectDBModule
-from dimos.perception.spatial_perception import SpatialMemory
+from dimos.msgs.geometry_msgs import PoseStamped, Twist, TwistStamped, Vector3, Quaternion
+from dimos.msgs.sensor_msgs import Image, CameraInfo
 from dimos.protocol import pubsub
 from dimos.protocol.pubsub.lcmpubsub import LCM
 from dimos.robot.foxglove_bridge import FoxgloveBridge
 from dimos.web.websocket_vis.websocket_vis_module import WebsocketVisModule
 from dimos.robot.unitree_webrtc.connection import UnitreeWebRTCConnection
 from dimos.robot.unitree_webrtc.unitree_skills import MyUnitreeSkills
-from dimos.robot.unitree_webrtc.nav_bot import NavBot
+from dimos.robot.nav_bot import NavBot
 from dimos.skills.skills import SkillLibrary
 from dimos.robot.robot import Robot
 from dimos.robot.ros_bridge import BridgeDirection, ROSBridge
@@ -138,8 +134,6 @@ def __init__(
 
         # Initialize skill library with G1 robot type
         if skill_library is None:
-            from dimos.robot.unitree_webrtc.unitree_skills import MyUnitreeSkills
-
             skill_library = MyUnitreeSkills(robot_type="g1")
         self.skill_library = skill_library
 
@@ -220,15 +214,8 @@ def start(self):
 
         self.deploy_navigation_modules(bridge_name="g1_ros_bridge")
 
-        self.nav = self._dimos.deploy(NavigationModule)
-        self.nav.goal_reached.transport = core.LCMTransport("/goal_reached", Bool)
-        self.nav.goal_pose.transport = core.LCMTransport("/goal_pose", PoseStamped)
-        self.nav.cancel_goal.transport = core.LCMTransport("/cancel_goal", Bool)
-        self.nav.joy.transport = core.LCMTransport("/joy", Joy)
-        self.nav.start()
-
-        self._deploy_camera()
-        self._deploy_detection(self.nav.go_to)
+        self._start_modules()
+        NavBot.start(self)
 
         self.lcm.start()
 
@@ -348,8 +335,6 @@ def _start_modules(self):
         """Start all deployed modules."""
         self._dimos.start_all_modules()
 
-        self.start_navigation_modules()
-
         # Initialize skills after connection is established
         if self.skill_library is not None:
             for skill in self.skill_library:
@@ -373,7 +358,8 @@ def shutdown(self):
         """Shutdown the robot and clean up resources."""
         logger.info("Shutting down UnitreeG1...")
 
-        self.shutdown_navigation()
+        # Shutdown navigation modules from NavBot
+        NavBot.shutdown(self)
 
         if self.websocket_vis:
             try:
@@ -420,19 +406,19 @@ def main():
     )
     robot.start()
 
-    # time.sleep(7)
-    # print("Starting navigation...")
-    # print(
-    #     robot.nav.go_to(
-    #         PoseStamped(
-    #             ts=time.time(),
-    #             frame_id="map",
-    #             position=Vector3(0.0, 0.0, 0.03),
-    #             orientation=Quaternion(0, 0, 0, 0),
-    #         ),
-    #         timeout=10,
-    #     ),
-    # )
+    pose = PoseStamped(
+        ts=time.time(),
+        frame_id="map",
+        position=Vector3(1.0, 1.0, 0.0),
+        orientation=Quaternion(0.0, 0.0, 0.0, 1.0),
+    )
+
+    time.sleep(2)
+    robot.navigate_to_goal(pose, blocking=False)
+
+    time.sleep(5)
+    robot.cancel_navigation()
+
     try:
         if args.joystick:
             print("\n" + "=" * 50)

From 6004a17906d2d64a44a2f388a74313dd5c5e63ea Mon Sep 17 00:00:00 2001
From: alexlin2 <alex.lin416@outlook.com>
Date: Mon, 22 Sep 2025 19:17:49 -0400
Subject: [PATCH 04/19] added unified readme

---
 AUTONOMY_STACK_README.md | 105 +++++++++++++++++++++++++++++++++++++--
 1 file changed, 102 insertions(+), 3 deletions(-)

diff --git a/AUTONOMY_STACK_README.md b/AUTONOMY_STACK_README.md
index ca8f2d2876..1db0a6185f 100644
--- a/AUTONOMY_STACK_README.md
+++ b/AUTONOMY_STACK_README.md
@@ -1,12 +1,111 @@
 # Autonomy Stack API Documentation
 
-## Quick Start
+## Prerequisites
+
+- Ubuntu 24.04
+- [ROS 2 Jazzy Installation](https://docs.ros.org/en/jazzy/Installation.html)
+
+Add the following line to your `~/.bashrc` to source the ROS 2 Jazzy setup script automatically:
+
+``` echo "source /opt/ros/jazzy/setup.bash" >> ~/.bashrc```
+
+## MID360 Ethernet Configuration (skip for sim)
+
+### Step 1: Configure Network Interface
+
+1. Open Network Settings in Ubuntu
+2. Find your Ethernet connection to the MID360
+3. Click the gear icon to edit settings
+4. Go to IPv4 tab
+5. Change Method from "Automatic (DHCP)" to "Manual"
+6. Add the following settings:
+   - **Address**: 192.168.1.5
+   - **Netmask**: 255.255.255.0
+   - **Gateway**: 192.168.1.1
+7. Click "Apply"
+
+### Step 2: Configure MID360 IP in JSON
+
+1. Find your MID360 serial number (on sticker under QR code)
+2. Note the last 2 digits (e.g., if serial ends in 89, use 189)
+3. Edit the configuration file:
+
+```bash
+cd ~/autonomy_stack_mecanum_wheel_platform
+nano src/utilities/livox_ros_driver2/config/MID360_config.json
+```
+
+4. Update line 28 with your IP (192.168.1.1xx where xx = last 2 digits):
+
+```json
+"ip" : "192.168.1.1xx",
+```
+
+5. Save and exit
+
+### Step 3: Verify Connection
+
+```bash
+ping 192.168.1.1xx  # Replace xx with your last 2 digits
+```
+
+## Robot Configuration
+
+### Setting Robot Type
+
+The system supports different robot configurations. Set the `ROBOT_CONFIG_PATH` environment variable to specify which robot configuration to use:
 
-### Setting Robot Configuration
 ```bash
-export ROBOT_CONFIG_PATH="mechanum_drive"  # or "unitree/unitree_g1" or "unitree/unitree_b1"
+# For Unitree G1 (default if not set)
+export ROBOT_CONFIG_PATH="unitree/unitree_g1"
+
+# Add to ~/.bashrc to make permanent
+echo 'export ROBOT_CONFIG_PATH="unitree/unitree_g1"' >> ~/.bashrc
+```
+
+Available robot configurations:
+- `unitree/unitree_g1` - Unitree G1 robot (default)
+- Add your custom robot configs in `src/base_autonomy/local_planner/config/`
+
+## System Launch
+
+### Simulation Mode
+
+```bash
+cd ~/autonomy_stack_mecanum_wheel_platform
+
+# Base autonomy only
+./system_simulation.sh
+
+# With route planner
+./system_simulation_with_route_planner.sh
+
+# With exploration planner
+./system_simulation_with_exploration_planner.sh
 ```
 
+### Real Robot Mode
+
+```bash
+cd ~/autonomy_stack_mecanum_wheel_platform
+
+# Base autonomy only
+./system_real_robot.sh
+
+# With route planner
+./system_real_robot_with_route_planner.sh
+
+# With exploration planner
+./system_real_robot_with_exploration_planner.sh
+```
+
+## Quick Troubleshooting
+
+- **Cannot ping MID360**: Check Ethernet cable and network settings
+- **SLAM drift**: Press clear-terrain-map button on joystick controller
+- **Joystick not recognized**: Unplug and replug USB dongle
+
+
 ## ROS Topics
 
 ### Input Topics (Commands)

From d7f589bca5081c0b1297c0119c6666be009ad50d Mon Sep 17 00:00:00 2001
From: alexlin2 <alex.lin416@outlook.com>
Date: Mon, 22 Sep 2025 19:28:53 -0400
Subject: [PATCH 05/19] added map to world static

---
 dimos/robot/unitree_webrtc/unitree_g1.py | 25 ++++++++++++------------
 1 file changed, 13 insertions(+), 12 deletions(-)

diff --git a/dimos/robot/unitree_webrtc/unitree_g1.py b/dimos/robot/unitree_webrtc/unitree_g1.py
index afa34bf159..3dbc77fc7b 100644
--- a/dimos/robot/unitree_webrtc/unitree_g1.py
+++ b/dimos/robot/unitree_webrtc/unitree_g1.py
@@ -406,18 +406,19 @@ def main():
     )
     robot.start()
 
-    pose = PoseStamped(
-        ts=time.time(),
-        frame_id="map",
-        position=Vector3(1.0, 1.0, 0.0),
-        orientation=Quaternion(0.0, 0.0, 0.0, 1.0),
-    )
-
-    time.sleep(2)
-    robot.navigate_to_goal(pose, blocking=False)
-
-    time.sleep(5)
-    robot.cancel_navigation()
+    # # test navigation
+    # pose = PoseStamped(
+    #     ts=time.time(),
+    #     frame_id="map",
+    #     position=Vector3(1.0, 1.0, 0.0),
+    #     orientation=Quaternion(0.0, 0.0, 0.0, 1.0),
+    # )
+
+    # time.sleep(2)
+    # robot.navigate_to_goal(pose, blocking=False)
+
+    # time.sleep(5)
+    # robot.cancel_navigation()
 
     try:
         if args.joystick:

From b4b45a2eb9837e5cccc4b790e8610c6a2622e045 Mon Sep 17 00:00:00 2001
From: alexlin2 <alex.lin416@outlook.com>
Date: Tue, 23 Sep 2025 20:16:45 -0400
Subject: [PATCH 06/19] updated README

---
 AUTONOMY_STACK_README.md | 6 ++++++
 1 file changed, 6 insertions(+)

diff --git a/AUTONOMY_STACK_README.md b/AUTONOMY_STACK_README.md
index 1db0a6185f..dfedc73f95 100644
--- a/AUTONOMY_STACK_README.md
+++ b/AUTONOMY_STACK_README.md
@@ -67,6 +67,12 @@ Available robot configurations:
 - `unitree/unitree_g1` - Unitree G1 robot (default)
 - Add your custom robot configs in `src/base_autonomy/local_planner/config/`
 
+## Build the system
+
+You must do this every you make a code change, this is not Python
+
+```colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release```
+
 ## System Launch
 
 ### Simulation Mode

From aed04ec65db36b3f27c67f67a153158d401ef749 Mon Sep 17 00:00:00 2001
From: alexlin2 <alex.lin416@outlook.com>
Date: Thu, 2 Oct 2025 21:20:30 -0400
Subject: [PATCH 07/19] made navigation module and modified navbot

---
 dimos/msgs/std_msgs/Int8.py              | 54 +++++++++++++++
 dimos/msgs/std_msgs/__init__.py          |  4 +-
 dimos/robot/nav_bot.py                   | 88 ++++++++++++++++++++++--
 dimos/robot/unitree_webrtc/unitree_g1.py |  2 -
 4 files changed, 138 insertions(+), 10 deletions(-)
 create mode 100644 dimos/msgs/std_msgs/Int8.py

diff --git a/dimos/msgs/std_msgs/Int8.py b/dimos/msgs/std_msgs/Int8.py
new file mode 100644
index 0000000000..f87a96a9ad
--- /dev/null
+++ b/dimos/msgs/std_msgs/Int8.py
@@ -0,0 +1,54 @@
+#!/usr/bin/env python3
+# Copyright 2025 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.
+
+# Copyright 2025 Dimensional Inc.
+
+"""Int32 message type."""
+
+from typing import ClassVar
+from dimos_lcm.std_msgs import Int8 as LCMInt8
+from std_msgs.msg import Int8 as ROSInt8
+
+
+class Int8(LCMInt8):
+    """ROS-compatible Int32 message."""
+
+    msg_name: ClassVar[str] = "std_msgs.Int8"
+
+    def __init__(self, data: int = 0):
+        """Initialize Int8 with data value."""
+        self.data = data
+
+    @classmethod
+    def from_ros_msg(cls, ros_msg: ROSInt8) -> "Int8":
+        """Create a Bool from a ROS std_msgs/Bool message.
+
+        Args:
+            ros_msg: ROS Int8 message
+
+        Returns:
+            Int8 instance
+        """
+        return cls(data=ros_msg.data)
+
+    def to_ros_msg(self) -> ROSInt8:
+        """Convert to a ROS std_msgs/Bool message.
+
+        Returns:
+            ROS Int8 message
+        """
+        ros_msg = ROSInt8()
+        ros_msg.data = self.data
+        return ros_msg
diff --git a/dimos/msgs/std_msgs/__init__.py b/dimos/msgs/std_msgs/__init__.py
index aa4792dce3..d46e2ce9a3 100644
--- a/dimos/msgs/std_msgs/__init__.py
+++ b/dimos/msgs/std_msgs/__init__.py
@@ -15,6 +15,6 @@
 from .Bool import Bool
 from .Header import Header
 from .Int32 import Int32
-from .Bool import Bool
+from .Int8 import Int8
 
-__all__ = ["Header", "Int32", "Bool"]
+__all__ = ["Bool", "Header", "Int32", "Int8"]
diff --git a/dimos/robot/nav_bot.py b/dimos/robot/nav_bot.py
index e65ed8214b..a61a839595 100644
--- a/dimos/robot/nav_bot.py
+++ b/dimos/robot/nav_bot.py
@@ -54,16 +54,34 @@
 # then deploy this module in any other run file.
 ############################################################
 class NavigationModule(Module):
+    """
+    Unified navigation module that handles both topic remapping and navigation control.
+    """
+
+    # Inputs
     goal_reached: In[Bool] = None
+    odom: In[Odometry] = None
 
+    # Outputs
     goal_pose: Out[PoseStamped] = None
     cancel_goal: Out[Bool] = None
+    soft_stop: Out[Int8] = None
     joy: Out[Joy] = None
+    odom_pose: Out[PoseStamped] = None
 
-    def __init__(self, *args, **kwargs):
+    def __init__(self, sensor_to_base_link_transform=None, *args, **kwargs):
         """Initialize NavigationModule."""
         Module.__init__(self, *args, **kwargs)
         self.goal_reach = None
+        self.tf = TF()
+        self.sensor_to_base_link_transform = sensor_to_base_link_transform or [
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+        ]
 
     @rpc
     def start(self):
@@ -80,6 +98,50 @@ def _on_goal_reached(self, msg: Bool):
         """Handle goal reached status messages."""
         self.goal_reach = msg.data
 
+    def _publish_odom_pose(self, msg: Odometry):
+        """Remap Odometry to PoseStamped and publish transforms."""
+        # Publish pose from odometry
+        if self.odom_pose:
+            pose_msg = PoseStamped(
+                ts=msg.ts,
+                frame_id=msg.frame_id,
+                position=msg.pose.pose.position,
+                orientation=msg.pose.pose.orientation,
+            )
+            self.odom_pose.publish(pose_msg)
+
+        # Publish static transforms
+        translation = Vector3(
+            self.sensor_to_base_link_transform[0],
+            self.sensor_to_base_link_transform[1],
+            self.sensor_to_base_link_transform[2],
+        )
+        euler_angles = Vector3(
+            self.sensor_to_base_link_transform[3],
+            self.sensor_to_base_link_transform[4],
+            self.sensor_to_base_link_transform[5],
+        )
+        rotation = euler_to_quaternion(euler_angles)
+
+        sensor_to_base_link_tf = Transform(
+            translation=translation,
+            rotation=rotation,
+            frame_id="sensor",
+            child_frame_id="base_link",
+            ts=msg.ts,
+        )
+
+        # Map to world static transform
+        map_to_world_tf = Transform(
+            translation=Vector3(0.0, 0.0, 0.0),
+            rotation=euler_to_quaternion(Vector3(0.0, 0.0, 0.0)),
+            frame_id="map",
+            child_frame_id="world",
+            ts=msg.ts,
+        )
+
+        self.tf.publish(sensor_to_base_link_tf, map_to_world_tf)
+
     def _set_autonomy_mode(self):
         """
         Set autonomy mode by publishing Joy message.
@@ -117,7 +179,7 @@ def _set_autonomy_mode(self):
             logger.info(f"Setting autonomy mode via Joy message")
 
     @rpc
-    def go_to(self, pose: PoseStamped, timeout: float = 60.0) -> bool:
+    def navigate_to(self, pose: PoseStamped, timeout: float = 60.0) -> bool:
         """
         Navigate to a target pose by publishing to LCM topics.
 
@@ -135,11 +197,14 @@ def go_to(self, pose: PoseStamped, timeout: float = 60.0) -> bool:
 
         self.goal_reach = None
         self._set_autonomy_mode()
+
+        self.soft_stop.publish(Int8(data=0))
         self.goal_pose.publish(pose)
 
         start_time = time.time()
         while time.time() - start_time < timeout:
             if self.goal_reach is not None:
+                self.soft_stop.publish(Int8(data=2))
                 return self.goal_reach
             time.sleep(0.1)
 
@@ -161,6 +226,7 @@ def stop_navigation(self) -> bool:
         if self.cancel_goal:
             cancel_msg = Bool(data=True)
             self.cancel_goal.publish(cancel_msg)
+            self.soft_stop.publish(Int8(data=2))
             return True
 
         return False
@@ -257,7 +323,7 @@ def __init__(self, dimos=None, sensor_to_base_link_transform=[0.0, 0.0, 0.0, 0.0
 
         self.sensor_to_base_link_transform = sensor_to_base_link_transform
         self.ros_bridge = None
-        self.topic_remap_module = None
+        self.navigation_module = None
         self.tf = TF()
         self.lcm = LCM()
 
@@ -289,8 +355,15 @@ def deploy_navigation_modules(self, bridge_name="nav_bot_ros_bridge"):
         self.topic_remap_module = self.dimos.deploy(
             TopicRemapModule, sensor_to_base_link_transform=self.sensor_to_base_link_transform
         )
-        self.topic_remap_module.odom.transport = core.LCMTransport("/odom", Odometry)
-        self.topic_remap_module.odom_pose.transport = core.LCMTransport("/odom_pose", PoseStamped)
+
+        # Configure all LCM transports for the navigation module
+        self.navigation_module.goal_reached.transport = core.LCMTransport("/goal_reached", Bool)
+        self.navigation_module.goal_pose.transport = core.LCMTransport("/goal_pose", PoseStamped)
+        self.navigation_module.cancel_goal.transport = core.LCMTransport("/cancel_goal", Bool)
+        self.navigation_module.soft_stop.transport = core.LCMTransport("/soft_stop", Int8)
+        self.navigation_module.joy.transport = core.LCMTransport("/joy", Joy)
+        self.navigation_module.odom.transport = core.LCMTransport("/odom", Odometry)
+        self.navigation_module.odom_pose.transport = core.LCMTransport("/odom_pose", PoseStamped)
 
         # Deploy ROS bridge
         logger.info("Deploying ROS bridge...")
@@ -361,7 +434,10 @@ def _set_autonomy_mode(self):
             ],
         )
 
-        self.lcm.publish(Topic("/joy", Joy), joy_msg)
+        # Start the navigation module
+        if self.navigation_module:
+            self.navigation_module.start()
+            logger.info("Navigation module started")
 
     def navigate_to_goal(self, pose: PoseStamped, blocking: bool = True, timeout: float = 30.0):
         """Navigate to a target pose using ROS topics.
diff --git a/dimos/robot/unitree_webrtc/unitree_g1.py b/dimos/robot/unitree_webrtc/unitree_g1.py
index 3dbc77fc7b..944b425c0e 100644
--- a/dimos/robot/unitree_webrtc/unitree_g1.py
+++ b/dimos/robot/unitree_webrtc/unitree_g1.py
@@ -212,8 +212,6 @@ def start(self):
         if self.enable_joystick:
             self._deploy_joystick()
 
-        self.deploy_navigation_modules(bridge_name="g1_ros_bridge")
-
         self._start_modules()
         NavBot.start(self)
 

From 3a08b4815b2181172477a21d5f8aba02c41187d2 Mon Sep 17 00:00:00 2001
From: alexlin2 <alex.lin416@outlook.com>
Date: Fri, 3 Oct 2025 15:51:02 -0400
Subject: [PATCH 08/19] conformed to Ivan's nav spec, made navigation module a
 standalone module that doesnt' need rosbridge

---
 dimos/robot/nav_bot.py | 287 ++++++++++++++++++++++++++++-------------
 1 file changed, 195 insertions(+), 92 deletions(-)

diff --git a/dimos/robot/nav_bot.py b/dimos/robot/nav_bot.py
index a61a839595..ec2a51fd98 100644
--- a/dimos/robot/nav_bot.py
+++ b/dimos/robot/nav_bot.py
@@ -20,6 +20,11 @@
 
 import logging
 import time
+import threading
+
+import rclpy
+from rclpy.node import Node
+from rclpy.executors import SingleThreadedExecutor
 
 from dimos import core
 from dimos.core import Module, In, Out, rpc
@@ -30,10 +35,13 @@
 from dimos.msgs.std_msgs import Bool
 from dimos.msgs.tf2_msgs.TFMessage import TFMessage
 from dimos.protocol.tf import TF
-from dimos.robot.ros_bridge import ROSBridge, BridgeDirection
 from dimos.utils.transform_utils import euler_to_quaternion
+from dimos.utils.logging_config import setup_logger
+
+# ROS2 message imports
 from geometry_msgs.msg import TwistStamped as ROSTwistStamped
 from geometry_msgs.msg import PoseStamped as ROSPoseStamped
+from geometry_msgs.msg import PointStamped as ROSPointStamped
 from nav_msgs.msg import Odometry as ROSOdometry
 from sensor_msgs.msg import PointCloud2 as ROSPointCloud2, Joy as ROSJoy, Image as ROSImage
 from std_msgs.msg import Bool as ROSBool
@@ -44,34 +52,30 @@
 
 logger = setup_logger("dimos.robot.unitree_webrtc.nav_bot", level=logging.INFO)
 
-############################################################
-# Navigation Module
-
-# first run unitree_g1.py to start the ROS bridge and webrtc connection and teleop
-# python dimos/robot/unitree_webrtc/unitree_g1.py
-
 
-# then deploy this module in any other run file.
-############################################################
-class NavigationModule(Module):
+class NavigationModule(Module, Node):
     """
-    Unified navigation module that handles both topic remapping and navigation control.
+    Handles navigation control and odometry remapping.
     """
 
-    # Inputs
-    goal_reached: In[Bool] = None
-    odom: In[Odometry] = None
+    goal_req: In[PoseStamped] = None
+    cancel_goal: In[Bool] = None
+    soft_stop: In[Int8] = None
 
-    # Outputs
-    goal_pose: Out[PoseStamped] = None
-    cancel_goal: Out[Bool] = None
-    soft_stop: Out[Int8] = None
-    joy: Out[Joy] = None
+    pointcloud: Out[PointCloud2] = None
+    goal_active: Out[PoseStamped] = None
+    goal_reached: Out[Bool] = None
+    odom: Out[Odometry] = None
+    cmd_vel: Out[TwistStamped] = None
     odom_pose: Out[PoseStamped] = None
 
     def __init__(self, sensor_to_base_link_transform=None, *args, **kwargs):
         """Initialize NavigationModule."""
         Module.__init__(self, *args, **kwargs)
+        if not rclpy.ok():
+            rclpy.init()
+        Node.__init__(self, "navigation_module")
+
         self.goal_reach = None
         self.tf = TF()
         self.sensor_to_base_link_transform = sensor_to_base_link_transform or [
@@ -82,6 +86,32 @@ def __init__(self, sensor_to_base_link_transform=None, *args, **kwargs):
             0.0,
             0.0,
         ]
+        self.spin_thread = None
+
+        # ROS2 Publishers
+        self.goal_pose_pub = self.create_publisher(ROSPoseStamped, "/goal_pose", 10)
+        self.cancel_goal_pub = self.create_publisher(ROSBool, "/cancel_goal", 10)
+        self.soft_stop_pub = self.create_publisher(ROSInt8, "/soft_stop", 10)
+        self.joy_pub = self.create_publisher(ROSJoy, "/joy", 10)
+
+        # ROS2 Subscribers
+        self.goal_reached_sub = self.create_subscription(
+            ROSBool, "/goal_reached", self._on_ros_goal_reached, 10
+        )
+        self.odom_sub = self.create_subscription(
+            ROSOdometry, "/state_estimation", self._on_ros_odom, 10
+        )
+        self.cmd_vel_sub = self.create_subscription(
+            ROSTwistStamped, "/cmd_vel", self._on_ros_cmd_vel, 10
+        )
+        self.goal_waypoint_sub = self.create_subscription(
+            ROSPointStamped, "/way_point", self._on_ros_goal_waypoint, 10
+        )
+        self.registered_scan_sub = self.create_subscription(
+            ROSPointCloud2, "/registered_scan", self._on_ros_registered_scan, 10
+        )
+
+        logger.info("NavigationModule initialized with ROS2 node")
 
     @rpc
     def start(self):
@@ -94,21 +124,60 @@ def start(self):
     def stop(self) -> None:
         super().stop()
 
-    def _on_goal_reached(self, msg: Bool):
-        """Handle goal reached status messages."""
+        logger.info("NavigationModule started with ROS2 spinning")
+
+    def _spin_node(self):
+        """Spin the ROS2 node to process callbacks."""
+        import rclpy
+
+        while self._running and rclpy.ok():
+            try:
+                rclpy.spin_once(self, timeout_sec=0.1)
+            except Exception as e:
+                if self._running:
+                    logger.error(f"ROS2 spin error: {e}")
+
+    def _on_ros_goal_reached(self, msg: ROSBool):
+        """Bridge ROS goal_reached to DIMOS."""
         self.goal_reach = msg.data
+        dimos_bool = Bool(data=msg.data)
+        self.goal_reached.publish(dimos_bool)
+
+    def _on_ros_goal_waypoint(self, msg: ROSPointStamped):
+        """Convert PointStamped waypoint to PoseStamped and publish as active goal."""
+        dimos_pose = PoseStamped(
+            ts=time.time(),
+            frame_id=msg.header.frame_id,
+            position=Vector3(msg.point.x, msg.point.y, msg.point.z),
+            orientation=Quaternion(0.0, 0.0, 0.0, 1.0),
+        )
+        self.goal_active.publish(dimos_pose)
+
+    def _on_ros_cmd_vel(self, msg: ROSTwistStamped):
+        """Bridge ROS cmd_vel to DIMOS."""
+        dimos_twist = TwistStamped.from_ros_msg(msg)
+        self.cmd_vel.publish(dimos_twist)
+
+    def _on_ros_odom(self, msg: ROSOdometry):
+        """Bridge ROS odometry to DIMOS and remap to PoseStamped."""
+        dimos_odom = Odometry.from_ros_msg(msg)
+        self.odom.publish(dimos_odom)
+
+        dimos_pose = PoseStamped(
+            ts=dimos_odom.ts,
+            frame_id=dimos_odom.frame_id,
+            position=dimos_odom.pose.pose.position,
+            orientation=dimos_odom.pose.pose.orientation,
+        )
+        self.odom_pose.publish(dimos_pose)
 
-    def _publish_odom_pose(self, msg: Odometry):
-        """Remap Odometry to PoseStamped and publish transforms."""
-        # Publish pose from odometry
-        if self.odom_pose:
-            pose_msg = PoseStamped(
-                ts=msg.ts,
-                frame_id=msg.frame_id,
-                position=msg.pose.pose.position,
-                orientation=msg.pose.pose.orientation,
-            )
-            self.odom_pose.publish(pose_msg)
+    def _on_ros_registered_scan(self, msg: ROSPointCloud2):
+        dimos_pointcloud = PointCloud2.from_ros_msg(msg)
+        self.pointcloud.publish(dimos_pointcloud)
+
+    def _on_ros_tf(self, msg: ROSTFMessage):
+        """Bridge ROS TF to DIMOS."""
+        ros_tf = TFMessage.from_ros_msg(msg)
 
         # Publish static transforms
         translation = Vector3(
@@ -131,7 +200,6 @@ def _publish_odom_pose(self, msg: Odometry):
             ts=msg.ts,
         )
 
-        # Map to world static transform
         map_to_world_tf = Transform(
             translation=Vector3(0.0, 0.0, 0.0),
             rotation=euler_to_quaternion(Vector3(0.0, 0.0, 0.0)),
@@ -140,56 +208,63 @@ def _publish_odom_pose(self, msg: Odometry):
             ts=msg.ts,
         )
 
-        self.tf.publish(sensor_to_base_link_tf, map_to_world_tf)
+        self.tf.publish(sensor_to_base_link_tf, map_to_world_tf, ros_tf)
 
-    def _set_autonomy_mode(self):
-        """
-        Set autonomy mode by publishing Joy message.
-        """
+    def _on_goal_pose(self, msg: PoseStamped):
+        """Handle DIMOS goal_pose by calling navigate_to."""
+        self.navigate_to(msg)
 
-        joy_msg = Joy(
-            frame_id="dimos",
-            axes=[
-                0.0,  # axis 0
-                0.0,  # axis 1
-                -1.0,  # axis 2
-                0.0,  # axis 3
-                1.0,  # axis 4
-                1.0,  # axis 5
-                0.0,  # axis 6
-                0.0,  # axis 7
-            ],
-            buttons=[
-                0,  # button 0
-                0,  # button 1
-                0,  # button 2
-                0,  # button 3
-                0,  # button 4
-                0,  # button 5
-                0,  # button 6
-                1,  # button 7 - controls autonomy mode
-                0,  # button 8
-                0,  # button 9
-                0,  # button 10
-            ],
-        )
+    def _on_cancel_goal(self, msg: Bool):
+        """Handle DIMOS cancel_goal by calling stop."""
+        if msg.data:
+            self.stop()
 
-        if self.joy:
-            self.joy.publish(joy_msg)
-            logger.info(f"Setting autonomy mode via Joy message")
+    def _on_soft_stop(self, msg: Int8):
+        """Handle DIMOS soft_stop and publish to ROS."""
+        ros_int8 = ROSInt8()
+        ros_int8.data = msg.data
+        self.soft_stop_pub.publish(ros_int8)
+
+    def _set_autonomy_mode(self):
+        """Set autonomy mode by publishing Joy message to ROS."""
+        joy_msg = ROSJoy()
+        joy_msg.axes = [
+            0.0,  # axis 0
+            0.0,  # axis 1
+            -1.0,  # axis 2
+            0.0,  # axis 3
+            1.0,  # axis 4
+            1.0,  # axis 5
+            0.0,  # axis 6
+            0.0,  # axis 7
+        ]
+        joy_msg.buttons = [
+            0,  # button 0
+            0,  # button 1
+            0,  # button 2
+            0,  # button 3
+            0,  # button 4
+            0,  # button 5
+            0,  # button 6
+            1,  # button 7 - controls autonomy mode
+            0,  # button 8
+            0,  # button 9
+            0,  # button 10
+        ]
+        self.joy_pub.publish(joy_msg)
+        logger.info("Setting autonomy mode via Joy message")
 
     @rpc
     def navigate_to(self, pose: PoseStamped, timeout: float = 60.0) -> bool:
         """
-        Navigate to a target pose by publishing to LCM topics.
+        Navigate to a target pose by publishing to ROS topics.
 
         Args:
             pose: Target pose to navigate to
-            blocking: If True, block until goal is reached
             timeout: Maximum time to wait for goal (seconds)
 
         Returns:
-            True if navigation was successful (or started if non-blocking)
+            True if navigation was successful
         """
         logger.info(
             f"Navigating to goal: ({pose.position.x:.2f}, {pose.position.y:.2f}, {pose.position.z:.2f})"
@@ -198,13 +273,20 @@ def navigate_to(self, pose: PoseStamped, timeout: float = 60.0) -> bool:
         self.goal_reach = None
         self._set_autonomy_mode()
 
-        self.soft_stop.publish(Int8(data=0))
-        self.goal_pose.publish(pose)
+        # Enable soft stop (0 = enable)
+        soft_stop_msg = ROSInt8()
+        soft_stop_msg.data = 0
+        self.soft_stop_pub.publish(soft_stop_msg)
+
+        ros_pose = pose.to_ros_msg()
+        self.goal_pose_pub.publish(ros_pose)
 
+        # Wait for goal to be reached
         start_time = time.time()
         while time.time() - start_time < timeout:
             if self.goal_reach is not None:
-                self.soft_stop.publish(Int8(data=2))
+                soft_stop_msg.data = 2
+                self.soft_stop_pub.publish(soft_stop_msg)
                 return self.goal_reach
             time.sleep(0.1)
 
@@ -216,20 +298,32 @@ def navigate_to(self, pose: PoseStamped, timeout: float = 60.0) -> bool:
     @rpc
     def stop_navigation(self) -> bool:
         """
-        Cancel current navigation by publishing to cancel_goal.
+        Cancel current navigation by publishing to ROS topics.
 
         Returns:
             True if cancel command was sent successfully
         """
         logger.info("Cancelling navigation")
 
-        if self.cancel_goal:
-            cancel_msg = Bool(data=True)
-            self.cancel_goal.publish(cancel_msg)
-            self.soft_stop.publish(Int8(data=2))
-            return True
+        cancel_msg = ROSBool()
+        cancel_msg.data = True
+        self.cancel_goal_pub.publish(cancel_msg)
 
-        return False
+        soft_stop_msg = ROSInt8()
+        soft_stop_msg.data = 2
+        self.soft_stop_pub.publish(soft_stop_msg)
+
+        return True
+
+    def shutdown(self):
+        """Cleanup the module's ROS2 resources."""
+        try:
+            self._running = False
+            if self.spin_thread:
+                self.spin_thread.join(timeout=1)
+            self.destroy_node()
+        except Exception as e:
+            logger.error(f"Error during shutdown: {e}")
 
 
 class TopicRemapModule(Module):
@@ -304,28 +398,31 @@ def _publish_odom_pose(self, msg: Odometry):
 
 class NavBot(Resource):
     """
-    NavBot class for navigation-related functionality.
-    Manages ROS bridge and topic remapping for navigation.
+    NavBot wrapper that deploys NavigationModule with proper DIMOS/ROS2 integration.
     """
 
-    def __init__(self, dimos=None, sensor_to_base_link_transform=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0]):
+    def __init__(self, dimos=None, sensor_to_base_link_transform=None):
         """
         Initialize NavBot.
 
         Args:
             dimos: DIMOS instance (creates new one if None)
-            sensor_to_base_link_transform: Optional [x, y, z, roll, pitch, yaw] transform from sensor to base_link
+            sensor_to_base_link_transform: Optional [x, y, z, roll, pitch, yaw] transform
         """
         if dimos is None:
             self.dimos = core.start(2)
         else:
             self.dimos = dimos
 
-        self.sensor_to_base_link_transform = sensor_to_base_link_transform
-        self.ros_bridge = None
+        self.sensor_to_base_link_transform = sensor_to_base_link_transform or [
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+        ]
         self.navigation_module = None
-        self.tf = TF()
-        self.lcm = LCM()
 
     def start(self):
         super().start()
@@ -356,13 +453,19 @@ def deploy_navigation_modules(self, bridge_name="nav_bot_ros_bridge"):
             TopicRemapModule, sensor_to_base_link_transform=self.sensor_to_base_link_transform
         )
 
-        # Configure all LCM transports for the navigation module
-        self.navigation_module.goal_reached.transport = core.LCMTransport("/goal_reached", Bool)
-        self.navigation_module.goal_pose.transport = core.LCMTransport("/goal_pose", PoseStamped)
+        self.navigation_module.goal_req.transport = core.LCMTransport("/goal", PoseStamped)
         self.navigation_module.cancel_goal.transport = core.LCMTransport("/cancel_goal", Bool)
         self.navigation_module.soft_stop.transport = core.LCMTransport("/soft_stop", Int8)
-        self.navigation_module.joy.transport = core.LCMTransport("/joy", Joy)
+
+        self.navigation_module.pointcloud.transport = core.LCMTransport(
+            "/pointcloud_map", PointCloud2
+        )
+        self.navigation_module.goal_active.transport = core.LCMTransport(
+            "/goal_active", PoseStamped
+        )
+        self.navigation_module.goal_reached.transport = core.LCMTransport("/goal_reached", Bool)
         self.navigation_module.odom.transport = core.LCMTransport("/odom", Odometry)
+        self.navigation_module.cmd_vel.transport = core.LCMTransport("/cmd_vel", TwistStamped)
         self.navigation_module.odom_pose.transport = core.LCMTransport("/odom_pose", PoseStamped)
 
         # Deploy ROS bridge

From 1d81aa8d53ea7db764032ba60c1054429ef2a27f Mon Sep 17 00:00:00 2001
From: alexlin2 <alex.lin416@outlook.com>
Date: Fri, 3 Oct 2025 15:53:19 -0400
Subject: [PATCH 09/19] reset g1 to dev

---
 dimos/robot/unitree_webrtc/unitree_g1.py | 104 ++++++++++++++++++-----
 1 file changed, 81 insertions(+), 23 deletions(-)

diff --git a/dimos/robot/unitree_webrtc/unitree_g1.py b/dimos/robot/unitree_webrtc/unitree_g1.py
index 944b425c0e..eaa2e02b86 100644
--- a/dimos/robot/unitree_webrtc/unitree_g1.py
+++ b/dimos/robot/unitree_webrtc/unitree_g1.py
@@ -25,15 +25,21 @@
 from dimos import core
 from dimos.constants import DEFAULT_CAPACITY_COLOR_IMAGE, DEFAULT_CAPACITY_DEPTH_IMAGE
 from dimos.core import Module, In, Out, rpc
-from dimos.msgs.geometry_msgs import PoseStamped, Twist, TwistStamped, Vector3, Quaternion
-from dimos.msgs.sensor_msgs import Image, CameraInfo
+from dimos.msgs.geometry_msgs import PoseStamped, Twist, TwistStamped
+from dimos.msgs.nav_msgs.Odometry import Odometry
+from dimos.msgs.sensor_msgs import CameraInfo, PointCloud2
+from dimos.msgs.tf2_msgs.TFMessage import TFMessage
 from dimos.protocol import pubsub
 from dimos.protocol.pubsub.lcmpubsub import LCM
 from dimos.robot.foxglove_bridge import FoxgloveBridge
 from dimos.web.websocket_vis.websocket_vis_module import WebsocketVisModule
 from dimos.robot.unitree_webrtc.connection import UnitreeWebRTCConnection
 from dimos.robot.unitree_webrtc.unitree_skills import MyUnitreeSkills
-from dimos.robot.nav_bot import NavBot
+from dimos.robot.ros_bridge import ROSBridge, BridgeDirection
+from geometry_msgs.msg import TwistStamped as ROSTwistStamped
+from nav_msgs.msg import Odometry as ROSOdometry
+from sensor_msgs.msg import PointCloud2 as ROSPointCloud2
+from tf2_msgs.msg import TFMessage as ROSTFMessage
 from dimos.skills.skills import SkillLibrary
 from dimos.robot.robot import Robot
 from dimos.robot.ros_bridge import BridgeDirection, ROSBridge
@@ -68,6 +74,9 @@ class G1ConnectionModule(Module):
     """Simplified connection module for G1 - uses WebRTC for control."""
 
     movecmd: In[TwistStamped] = None
+    odom_in: In[Odometry] = None
+
+    odom_pose: Out[PoseStamped] = None
     ip: str
     connection_type: str = "webrtc"
 
@@ -79,20 +88,35 @@ def __init__(self, ip: str = None, connection_type: str = "webrtc", *args, **kwa
 
     @rpc
     def start(self):
+        """Start the connection and subscribe to sensor streams."""
+        # Use the exact same UnitreeWebRTCConnection as Go2
         self.connection = UnitreeWebRTCConnection(self.ip)
         self.movecmd.subscribe(self.move)
+        self.odom_in.subscribe(self._publish_odom_pose)
+
+    def _publish_odom_pose(self, msg: Odometry):
+        self.odom_pose.publish(
+            PoseStamped(
+                ts=msg.ts,
+                frame_id=msg.frame_id,
+                position=msg.pose.pose.position,
+                orientation=msg.pose.orientation,
+            )
+        )
 
     @rpc
     def move(self, twist_stamped: TwistStamped, duration: float = 0.0):
+        """Send movement command to robot."""
         twist = Twist(linear=twist_stamped.linear, angular=twist_stamped.angular)
         self.connection.move(twist, duration)
 
     @rpc
     def publish_request(self, topic: str, data: dict):
+        """Forward WebRTC publish requests to connection."""
         return self.connection.publish_request(topic, data)
 
 
-class UnitreeG1(Robot, NavBot):
+class UnitreeG1(Robot):
     """Unitree G1 humanoid robot."""
 
     def __init__(
@@ -105,6 +129,7 @@ def __init__(
         replay_path: str = None,
         enable_joystick: bool = False,
         enable_connection: bool = True,
+        enable_ros_bridge: bool = True,
         enable_camera: bool = False,
     ):
         """Initialize the G1 robot.
@@ -118,22 +143,25 @@ def __init__(
             replay_path: Path to replay recordings from (if replaying)
             enable_joystick: Enable pygame joystick control
             enable_connection: Enable robot connection module
+            enable_ros_bridge: Enable ROS bridge
             enable_camera: Enable ZED camera module
         """
-        Robot.__init__(self)
-        NavBot.__init__(self, sensor_to_base_link_transform=[0.0, 0.0, -0.04, 0.0, 0.0, 0.0])
+        super().__init__()
         self.ip = ip
         self.output_dir = output_dir or os.path.join(os.getcwd(), "assets", "output")
         self.recording_path = recording_path
         self.replay_path = replay_path
         self.enable_joystick = enable_joystick
         self.enable_connection = enable_connection
+        self.enable_ros_bridge = enable_ros_bridge
         self.enable_camera = enable_camera
         self.websocket_port = websocket_port
         self.lcm = LCM()
 
         # Initialize skill library with G1 robot type
         if skill_library is None:
+            from dimos.robot.unitree_webrtc.unitree_skills import MyUnitreeSkills
+
             skill_library = MyUnitreeSkills(robot_type="g1")
         self.skill_library = skill_library
 
@@ -141,11 +169,13 @@ def __init__(
         self.capabilities = [RobotCapability.LOCOMOTION]
 
         # Module references
+        self.dimos = None
         self.connection = None
         self.websocket_vis = None
         self.foxglove_bridge = None
         self.spatial_memory_module = None
         self.joystick = None
+        self.ros_bridge = None
         self.zed_camera = None
 
         self._setup_directories()
@@ -200,6 +230,7 @@ def _deploy_detection(self, goto):
 
     def start(self):
         """Start the robot system with all modules."""
+        self.dimos = core.start(4)  # 2 workers for connection and visualization
 
         if self.enable_connection:
             self._deploy_connection()
@@ -212,8 +243,10 @@ def start(self):
         if self.enable_joystick:
             self._deploy_joystick()
 
+        if self.enable_ros_bridge:
+            self._deploy_ros_bridge()
+
         self._start_modules()
-        NavBot.start(self)
 
         self.lcm.start()
 
@@ -263,7 +296,11 @@ def stop(self) -> None:
     def _deploy_connection(self):
         """Deploy and configure the connection module."""
         self.connection = self.dimos.deploy(G1ConnectionModule, self.ip)
+
+        # Configure LCM transports
         self.connection.movecmd.transport = core.LCMTransport("/cmd_vel", TwistStamped)
+        self.connection.odom_in.transport = core.LCMTransport("/state_estimation", Odometry)
+        self.connection.odom_pose.transport = core.LCMTransport("/odom", PoseStamped)
 
     def _deploy_camera(self):
         """Deploy and configure a standard webcam module."""
@@ -329,6 +366,34 @@ def _deploy_joystick(self):
         self.joystick.twist_out.transport = core.LCMTransport("/cmd_vel", Twist)
         logger.info("Joystick module deployed - pygame window will open")
 
+    def _deploy_ros_bridge(self):
+        """Deploy and configure ROS bridge."""
+        self.ros_bridge = ROSBridge("g1_ros_bridge")
+
+        # Add /cmd_vel topic from ROS to DIMOS
+        self.ros_bridge.add_topic(
+            "/cmd_vel", TwistStamped, ROSTwistStamped, direction=BridgeDirection.ROS_TO_DIMOS
+        )
+
+        # Add /state_estimation topic from ROS to DIMOS
+        self.ros_bridge.add_topic(
+            "/state_estimation", Odometry, ROSOdometry, direction=BridgeDirection.ROS_TO_DIMOS
+        )
+
+        # Add /tf topic from ROS to DIMOS
+        self.ros_bridge.add_topic(
+            "/tf", TFMessage, ROSTFMessage, direction=BridgeDirection.ROS_TO_DIMOS
+        )
+
+        # Add /registered_scan topic from ROS to DIMOS
+        self.ros_bridge.add_topic(
+            "/registered_scan", PointCloud2, ROSPointCloud2, direction=BridgeDirection.ROS_TO_DIMOS
+        )
+
+        logger.info(
+            "ROS bridge deployed: /cmd_vel, /state_estimation, /tf, /registered_scan (ROS → DIMOS)"
+        )
+
     def _start_modules(self):
         """Start all deployed modules."""
         self._dimos.start_all_modules()
@@ -356,9 +421,15 @@ def shutdown(self):
         """Shutdown the robot and clean up resources."""
         logger.info("Shutting down UnitreeG1...")
 
-        # Shutdown navigation modules from NavBot
-        NavBot.shutdown(self)
+        # Shutdown ROS bridge if it exists
+        if self.ros_bridge is not None:
+            try:
+                self.ros_bridge.shutdown()
+                logger.info("ROS bridge shut down successfully")
+            except Exception as e:
+                logger.error(f"Error shutting down ROS bridge: {e}")
 
+        # Stop other modules if needed
         if self.websocket_vis:
             try:
                 self.websocket_vis.stop()
@@ -401,23 +472,10 @@ def main():
         enable_joystick=args.joystick,
         enable_camera=args.camera,
         enable_connection=os.getenv("ROBOT_IP") is not None,
+        enable_ros_bridge=True,
     )
     robot.start()
 
-    # # test navigation
-    # pose = PoseStamped(
-    #     ts=time.time(),
-    #     frame_id="map",
-    #     position=Vector3(1.0, 1.0, 0.0),
-    #     orientation=Quaternion(0.0, 0.0, 0.0, 1.0),
-    # )
-
-    # time.sleep(2)
-    # robot.navigate_to_goal(pose, blocking=False)
-
-    # time.sleep(5)
-    # robot.cancel_navigation()
-
     try:
         if args.joystick:
             print("\n" + "=" * 50)

From 699a16097ce66386762be1a3e0dbaa6855f0e2f8 Mon Sep 17 00:00:00 2001
From: alexlin2 <alex.lin416@outlook.com>
Date: Sun, 5 Oct 2025 12:59:37 -0400
Subject: [PATCH 10/19] fixed some comments

---
 AUTONOMY_STACK_README.md           |  1 +
 dimos/msgs/sensor_msgs/test_Joy.py | 11 -----------
 dimos/robot/nav_bot.py             | 24 +++++-------------------
 3 files changed, 6 insertions(+), 30 deletions(-)

diff --git a/AUTONOMY_STACK_README.md b/AUTONOMY_STACK_README.md
index dfedc73f95..70eff131ce 100644
--- a/AUTONOMY_STACK_README.md
+++ b/AUTONOMY_STACK_README.md
@@ -122,6 +122,7 @@ cd ~/autonomy_stack_mecanum_wheel_platform
 | `/goal_pose` | `geometry_msgs/PoseStamped` | Send goal with orientation |
 | `/cancel_goal` | `std_msgs/Bool` | Cancel current goal (data: true) |
 | `/joy` | `sensor_msgs/Joy` | Joystick input |
+| `/stop` | `std_msgs/Int8` | Soft Stop (2=stop all commmand, 0 = release) | 
 | `/navigation_boundary` | `geometry_msgs/PolygonStamped` | Set navigation boundaries |
 | `/added_obstacles` | `sensor_msgs/PointCloud2` | Virtual obstacles |
 
diff --git a/dimos/msgs/sensor_msgs/test_Joy.py b/dimos/msgs/sensor_msgs/test_Joy.py
index fd11624b08..9b2430adee 100644
--- a/dimos/msgs/sensor_msgs/test_Joy.py
+++ b/dimos/msgs/sensor_msgs/test_Joy.py
@@ -230,14 +230,3 @@ def test_edge_cases():
     assert joy3.axes == extreme_axes
 
     print("✓ Joy edge cases test passed")
-
-
-if __name__ == "__main__":
-    test_lcm_encode_decode()
-    test_initialization_methods()
-    test_equality()
-    test_string_representation()
-    if ROS_AVAILABLE:
-        test_ros_conversion()
-    test_edge_cases()
-    print("\nAll Joy tests passed! ✓")
diff --git a/dimos/robot/nav_bot.py b/dimos/robot/nav_bot.py
index ec2a51fd98..5d8d4746a1 100644
--- a/dimos/robot/nav_bot.py
+++ b/dimos/robot/nav_bot.py
@@ -70,14 +70,12 @@ class NavigationModule(Module, Node):
     odom_pose: Out[PoseStamped] = None
 
     def __init__(self, sensor_to_base_link_transform=None, *args, **kwargs):
-        """Initialize NavigationModule."""
         Module.__init__(self, *args, **kwargs)
         if not rclpy.ok():
             rclpy.init()
         Node.__init__(self, "navigation_module")
 
         self.goal_reach = None
-        self.tf = TF()
         self.sensor_to_base_link_transform = sensor_to_base_link_transform or [
             0.0,
             0.0,
@@ -127,9 +125,6 @@ def stop(self) -> None:
         logger.info("NavigationModule started with ROS2 spinning")
 
     def _spin_node(self):
-        """Spin the ROS2 node to process callbacks."""
-        import rclpy
-
         while self._running and rclpy.ok():
             try:
                 rclpy.spin_once(self, timeout_sec=0.1)
@@ -138,13 +133,11 @@ def _spin_node(self):
                     logger.error(f"ROS2 spin error: {e}")
 
     def _on_ros_goal_reached(self, msg: ROSBool):
-        """Bridge ROS goal_reached to DIMOS."""
         self.goal_reach = msg.data
         dimos_bool = Bool(data=msg.data)
         self.goal_reached.publish(dimos_bool)
 
     def _on_ros_goal_waypoint(self, msg: ROSPointStamped):
-        """Convert PointStamped waypoint to PoseStamped and publish as active goal."""
         dimos_pose = PoseStamped(
             ts=time.time(),
             frame_id=msg.header.frame_id,
@@ -154,12 +147,10 @@ def _on_ros_goal_waypoint(self, msg: ROSPointStamped):
         self.goal_active.publish(dimos_pose)
 
     def _on_ros_cmd_vel(self, msg: ROSTwistStamped):
-        """Bridge ROS cmd_vel to DIMOS."""
         dimos_twist = TwistStamped.from_ros_msg(msg)
         self.cmd_vel.publish(dimos_twist)
 
     def _on_ros_odom(self, msg: ROSOdometry):
-        """Bridge ROS odometry to DIMOS and remap to PoseStamped."""
         dimos_odom = Odometry.from_ros_msg(msg)
         self.odom.publish(dimos_odom)
 
@@ -176,7 +167,6 @@ def _on_ros_registered_scan(self, msg: ROSPointCloud2):
         self.pointcloud.publish(dimos_pointcloud)
 
     def _on_ros_tf(self, msg: ROSTFMessage):
-        """Bridge ROS TF to DIMOS."""
         ros_tf = TFMessage.from_ros_msg(msg)
 
         # Publish static transforms
@@ -211,22 +201,18 @@ def _on_ros_tf(self, msg: ROSTFMessage):
         self.tf.publish(sensor_to_base_link_tf, map_to_world_tf, ros_tf)
 
     def _on_goal_pose(self, msg: PoseStamped):
-        """Handle DIMOS goal_pose by calling navigate_to."""
         self.navigate_to(msg)
 
     def _on_cancel_goal(self, msg: Bool):
-        """Handle DIMOS cancel_goal by calling stop."""
         if msg.data:
             self.stop()
 
     def _on_soft_stop(self, msg: Int8):
-        """Handle DIMOS soft_stop and publish to ROS."""
         ros_int8 = ROSInt8()
         ros_int8.data = msg.data
         self.soft_stop_pub.publish(ros_int8)
 
     def _set_autonomy_mode(self):
-        """Set autonomy mode by publishing Joy message to ROS."""
         joy_msg = ROSJoy()
         joy_msg.axes = [
             0.0,  # axis 0
@@ -298,12 +284,12 @@ def navigate_to(self, pose: PoseStamped, timeout: float = 60.0) -> bool:
     @rpc
     def stop_navigation(self) -> bool:
         """
-        Cancel current navigation by publishing to ROS topics.
+        Stop current navigation by publishing to ROS topics.
 
         Returns:
-            True if cancel command was sent successfully
+            True if stop command was sent successfully
         """
-        logger.info("Cancelling navigation")
+        logger.info("Stopping navigation")
 
         cancel_msg = ROSBool()
         cancel_msg.data = True
@@ -315,8 +301,8 @@ def stop_navigation(self) -> bool:
 
         return True
 
-    def shutdown(self):
-        """Cleanup the module's ROS2 resources."""
+    @rpc
+    def stop(self):
         try:
             self._running = False
             if self.spin_thread:

From 22855ef8f30aa68cf0ce8974d3042f183da2a740 Mon Sep 17 00:00:00 2001
From: alexlin2 <alex.lin416@outlook.com>
Date: Mon, 6 Oct 2025 20:43:02 -0400
Subject: [PATCH 11/19] fixed all of Ivan's requests

---
 dimos/robot/nav_bot.py | 19 +++++++++++++++++++
 1 file changed, 19 insertions(+)

diff --git a/dimos/robot/nav_bot.py b/dimos/robot/nav_bot.py
index 5d8d4746a1..820ca89862 100644
--- a/dimos/robot/nav_bot.py
+++ b/dimos/robot/nav_bot.py
@@ -63,7 +63,10 @@ class NavigationModule(Module, Node):
     soft_stop: In[Int8] = None
 
     pointcloud: Out[PointCloud2] = None
+    global_pointcloud: Out[PointCloud2] = None
+
     goal_active: Out[PoseStamped] = None
+    path_active: Out[Path] = None
     goal_reached: Out[Bool] = None
     odom: Out[Odometry] = None
     cmd_vel: Out[TwistStamped] = None
@@ -108,6 +111,10 @@ def __init__(self, sensor_to_base_link_transform=None, *args, **kwargs):
         self.registered_scan_sub = self.create_subscription(
             ROSPointCloud2, "/registered_scan", self._on_ros_registered_scan, 10
         )
+        self.global_pointcloud_sub = self.create_subscription(
+            ROSPointCloud2, "/terrain_map_ext", self._on_ros_global_pointcloud, 10
+        )
+        self.path_sub = self.create_subscription(ROSPath, "/path", self._on_ros_path, 10)
 
         logger.info("NavigationModule initialized with ROS2 node")
 
@@ -166,6 +173,14 @@ def _on_ros_registered_scan(self, msg: ROSPointCloud2):
         dimos_pointcloud = PointCloud2.from_ros_msg(msg)
         self.pointcloud.publish(dimos_pointcloud)
 
+    def _on_ros_global_pointcloud(self, msg: ROSPointCloud2):
+        dimos_pointcloud = PointCloud2.from_ros_msg(msg)
+        self.global_pointcloud.publish(dimos_pointcloud)
+
+    def _on_ros_path(self, msg: ROSPath):
+        dimos_path = Path.from_ros_msg(msg)
+        self.path_active.publish(dimos_path)
+
     def _on_ros_tf(self, msg: ROSTFMessage):
         ros_tf = TFMessage.from_ros_msg(msg)
 
@@ -446,9 +461,13 @@ def deploy_navigation_modules(self, bridge_name="nav_bot_ros_bridge"):
         self.navigation_module.pointcloud.transport = core.LCMTransport(
             "/pointcloud_map", PointCloud2
         )
+        self.navigation_module.global_pointcloud.transport = core.LCMTransport(
+            "/global_pointcloud", PointCloud2
+        )
         self.navigation_module.goal_active.transport = core.LCMTransport(
             "/goal_active", PoseStamped
         )
+        self.navigation_module.path_active.transport = core.LCMTransport("/path_active", Path)
         self.navigation_module.goal_reached.transport = core.LCMTransport("/goal_reached", Bool)
         self.navigation_module.odom.transport = core.LCMTransport("/odom", Odometry)
         self.navigation_module.cmd_vel.transport = core.LCMTransport("/cmd_vel", TwistStamped)

From d802a395f76ca358035a6abe6a4a1d9c507c8487 Mon Sep 17 00:00:00 2001
From: alexlin2 <alex.lin416@outlook.com>
Date: Fri, 10 Oct 2025 00:48:43 -0400
Subject: [PATCH 12/19] all navbot

---
 dimos/{robot => navigation}/nav_bot.py | 39 +++++++++++---------------
 1 file changed, 17 insertions(+), 22 deletions(-)
 rename dimos/{robot => navigation}/nav_bot.py (94%)

diff --git a/dimos/robot/nav_bot.py b/dimos/navigation/nav_bot.py
similarity index 94%
rename from dimos/robot/nav_bot.py
rename to dimos/navigation/nav_bot.py
index 820ca89862..8180fed04c 100644
--- a/dimos/robot/nav_bot.py
+++ b/dimos/navigation/nav_bot.py
@@ -27,6 +27,7 @@
 from rclpy.executors import SingleThreadedExecutor
 
 from dimos import core
+from dimos.protocol import pubsub
 from dimos.core import Module, In, Out, rpc
 from dimos.core.resource import Resource
 from dimos.msgs.geometry_msgs import PoseStamped, TwistStamped, Transform, Vector3
@@ -53,14 +54,13 @@
 logger = setup_logger("dimos.robot.unitree_webrtc.nav_bot", level=logging.INFO)
 
 
-class NavigationModule(Module, Node):
+class NavigationModule(Module):
     """
     Handles navigation control and odometry remapping.
     """
 
     goal_req: In[PoseStamped] = None
     cancel_goal: In[Bool] = None
-    soft_stop: In[Int8] = None
 
     pointcloud: Out[PointCloud2] = None
     global_pointcloud: Out[PointCloud2] = None
@@ -73,10 +73,10 @@ class NavigationModule(Module, Node):
     odom_pose: Out[PoseStamped] = None
 
     def __init__(self, sensor_to_base_link_transform=None, *args, **kwargs):
-        Module.__init__(self, *args, **kwargs)
+        super().__init__(*args, **kwargs)
         if not rclpy.ok():
             rclpy.init()
-        Node.__init__(self, "navigation_module")
+        self._node = Node("navigation_module")
 
         self.goal_reach = None
         self.sensor_to_base_link_transform = sensor_to_base_link_transform or [
@@ -90,31 +90,31 @@ def __init__(self, sensor_to_base_link_transform=None, *args, **kwargs):
         self.spin_thread = None
 
         # ROS2 Publishers
-        self.goal_pose_pub = self.create_publisher(ROSPoseStamped, "/goal_pose", 10)
-        self.cancel_goal_pub = self.create_publisher(ROSBool, "/cancel_goal", 10)
-        self.soft_stop_pub = self.create_publisher(ROSInt8, "/soft_stop", 10)
-        self.joy_pub = self.create_publisher(ROSJoy, "/joy", 10)
+        self.goal_pose_pub = self._node.create_publisher(ROSPoseStamped, "/goal_pose", 10)
+        self.cancel_goal_pub = self._node.create_publisher(ROSBool, "/cancel_goal", 10)
+        self.soft_stop_pub = self._node.create_publisher(ROSInt8, "/soft_stop", 10)
+        self.joy_pub = self._node.create_publisher(ROSJoy, "/joy", 10)
 
         # ROS2 Subscribers
-        self.goal_reached_sub = self.create_subscription(
+        self.goal_reached_sub = self._node.create_subscription(
             ROSBool, "/goal_reached", self._on_ros_goal_reached, 10
         )
-        self.odom_sub = self.create_subscription(
+        self.odom_sub = self._node.create_subscription(
             ROSOdometry, "/state_estimation", self._on_ros_odom, 10
         )
-        self.cmd_vel_sub = self.create_subscription(
+        self.cmd_vel_sub = self._node.create_subscription(
             ROSTwistStamped, "/cmd_vel", self._on_ros_cmd_vel, 10
         )
-        self.goal_waypoint_sub = self.create_subscription(
+        self.goal_waypoint_sub = self._node.create_subscription(
             ROSPointStamped, "/way_point", self._on_ros_goal_waypoint, 10
         )
-        self.registered_scan_sub = self.create_subscription(
+        self.registered_scan_sub = self._node.create_subscription(
             ROSPointCloud2, "/registered_scan", self._on_ros_registered_scan, 10
         )
-        self.global_pointcloud_sub = self.create_subscription(
+        self.global_pointcloud_sub = self._node.create_subscription(
             ROSPointCloud2, "/terrain_map_ext", self._on_ros_global_pointcloud, 10
         )
-        self.path_sub = self.create_subscription(ROSPath, "/path", self._on_ros_path, 10)
+        self.path_sub = self._node.create_subscription(ROSPath, "/path", self._on_ros_path, 10)
 
         logger.info("NavigationModule initialized with ROS2 node")
 
@@ -134,7 +134,7 @@ def stop(self) -> None:
     def _spin_node(self):
         while self._running and rclpy.ok():
             try:
-                rclpy.spin_once(self, timeout_sec=0.1)
+                rclpy.spin_once(self._node, timeout_sec=0.1)
             except Exception as e:
                 if self._running:
                     logger.error(f"ROS2 spin error: {e}")
@@ -222,11 +222,6 @@ def _on_cancel_goal(self, msg: Bool):
         if msg.data:
             self.stop()
 
-    def _on_soft_stop(self, msg: Int8):
-        ros_int8 = ROSInt8()
-        ros_int8.data = msg.data
-        self.soft_stop_pub.publish(ros_int8)
-
     def _set_autonomy_mode(self):
         joy_msg = ROSJoy()
         joy_msg.axes = [
@@ -322,7 +317,7 @@ def stop(self):
             self._running = False
             if self.spin_thread:
                 self.spin_thread.join(timeout=1)
-            self.destroy_node()
+            self._node.destroy_node()
         except Exception as e:
             logger.error(f"Error during shutdown: {e}")
 

From 001f3bacab0fda0513d49fd72ef3b0623220c94a Mon Sep 17 00:00:00 2001
From: alexlin2 <alex.lin416@outlook.com>
Date: Fri, 10 Oct 2025 17:04:16 -0400
Subject: [PATCH 13/19] use navbot

---
 dimos/navigation/nav_bot.py | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/dimos/navigation/nav_bot.py b/dimos/navigation/nav_bot.py
index 8180fed04c..a689170f8b 100644
--- a/dimos/navigation/nav_bot.py
+++ b/dimos/navigation/nav_bot.py
@@ -44,8 +44,9 @@
 from geometry_msgs.msg import PoseStamped as ROSPoseStamped
 from geometry_msgs.msg import PointStamped as ROSPointStamped
 from nav_msgs.msg import Odometry as ROSOdometry
-from sensor_msgs.msg import PointCloud2 as ROSPointCloud2, Joy as ROSJoy, Image as ROSImage
-from std_msgs.msg import Bool as ROSBool
+from nav_msgs.msg import Path as ROSPath
+from sensor_msgs.msg import PointCloud2 as ROSPointCloud2, Joy as ROSJoy
+from std_msgs.msg import Bool as ROSBool, Header, Int8 as ROSInt8
 from tf2_msgs.msg import TFMessage as ROSTFMessage
 from dimos.utils.logging_config import setup_logger
 from dimos.protocol.pubsub.lcmpubsub import LCM, Topic

From f617dfb73d4af6527ff412f9018279fa101d9403 Mon Sep 17 00:00:00 2001
From: alexlin2 <alex.lin416@outlook.com>
Date: Fri, 10 Oct 2025 17:08:25 -0400
Subject: [PATCH 14/19] made comment changes

---
 dimos/msgs/sensor_msgs/test_Joy.py       |  2 +-
 dimos/navigation/nav_bot.py              | 10 ++-
 dimos/robot/unitree_webrtc/unitree_g1.py | 94 +++++++++++++++++++-----
 3 files changed, 82 insertions(+), 24 deletions(-)

diff --git a/dimos/msgs/sensor_msgs/test_Joy.py b/dimos/msgs/sensor_msgs/test_Joy.py
index 9b2430adee..174b9d8908 100644
--- a/dimos/msgs/sensor_msgs/test_Joy.py
+++ b/dimos/msgs/sensor_msgs/test_Joy.py
@@ -165,7 +165,7 @@ def test_string_representation():
     print("✓ Joy string representation test passed")
 
 
-@pytest.mark.skipif(not ROS_AVAILABLE, reason="ROS not available")
+@pytest.mark.ros
 def test_ros_conversion():
     """Test conversion to/from ROS Joy messages."""
     print("Testing Joy ROS conversion...")
diff --git a/dimos/navigation/nav_bot.py b/dimos/navigation/nav_bot.py
index a689170f8b..afbc610548 100644
--- a/dimos/navigation/nav_bot.py
+++ b/dimos/navigation/nav_bot.py
@@ -70,7 +70,7 @@ class NavigationModule(Module):
     path_active: Out[Path] = None
     goal_reached: Out[Bool] = None
     odom: Out[Odometry] = None
-    cmd_vel: Out[TwistStamped] = None
+    cmd_vel: Out[Twist] = None
     odom_pose: Out[PoseStamped] = None
 
     def __init__(self, sensor_to_base_link_transform=None, *args, **kwargs):
@@ -155,7 +155,11 @@ def _on_ros_goal_waypoint(self, msg: ROSPointStamped):
         self.goal_active.publish(dimos_pose)
 
     def _on_ros_cmd_vel(self, msg: ROSTwistStamped):
-        dimos_twist = TwistStamped.from_ros_msg(msg)
+        # Extract the twist from the stamped message
+        dimos_twist = Twist(
+            linear=Vector3(msg.twist.linear.x, msg.twist.linear.y, msg.twist.linear.z),
+            angular=Vector3(msg.twist.angular.x, msg.twist.angular.y, msg.twist.angular.z),
+        )
         self.cmd_vel.publish(dimos_twist)
 
     def _on_ros_odom(self, msg: ROSOdometry):
@@ -466,7 +470,7 @@ def deploy_navigation_modules(self, bridge_name="nav_bot_ros_bridge"):
         self.navigation_module.path_active.transport = core.LCMTransport("/path_active", Path)
         self.navigation_module.goal_reached.transport = core.LCMTransport("/goal_reached", Bool)
         self.navigation_module.odom.transport = core.LCMTransport("/odom", Odometry)
-        self.navigation_module.cmd_vel.transport = core.LCMTransport("/cmd_vel", TwistStamped)
+        self.navigation_module.cmd_vel.transport = core.LCMTransport("/cmd_vel", Twist)
         self.navigation_module.odom_pose.transport = core.LCMTransport("/odom_pose", PoseStamped)
 
         # Deploy ROS bridge
diff --git a/dimos/robot/unitree_webrtc/unitree_g1.py b/dimos/robot/unitree_webrtc/unitree_g1.py
index eaa2e02b86..52f7a67d9d 100644
--- a/dimos/robot/unitree_webrtc/unitree_g1.py
+++ b/dimos/robot/unitree_webrtc/unitree_g1.py
@@ -23,24 +23,34 @@
 import time
 
 from dimos import core
-from dimos.constants import DEFAULT_CAPACITY_COLOR_IMAGE, DEFAULT_CAPACITY_DEPTH_IMAGE
-from dimos.core import Module, In, Out, rpc
-from dimos.msgs.geometry_msgs import PoseStamped, Twist, TwistStamped
+from dimos.agents2 import Agent
+from dimos.agents2.cli.human import HumanInput
+from dimos.agents2.skills.ros_navigation import RosNavigation
+from dimos.agents2.spec import Model, Provider
+from dimos.core import In, Module, Out, rpc
+from dimos.hardware.camera import zed
+from dimos.hardware.camera.module import CameraModule
+from dimos.hardware.camera.webcam import Webcam
+from dimos.msgs.foxglove_msgs import ImageAnnotations
+from dimos.msgs.geometry_msgs import (
+    PoseStamped,
+    Quaternion,
+    Transform,
+    Twist,
+    TwistStamped,
+    Vector3,
+)
 from dimos.msgs.nav_msgs.Odometry import Odometry
-from dimos.msgs.sensor_msgs import CameraInfo, PointCloud2
+from dimos.msgs.sensor_msgs import CameraInfo, Image, Joy, PointCloud2
+from dimos.msgs.std_msgs.Bool import Bool
 from dimos.msgs.tf2_msgs.TFMessage import TFMessage
+from dimos.msgs.vision_msgs import Detection2DArray
+from dimos.perception.detection2d import Detection3DModule
+from dimos.perception.detection2d.moduleDB import ObjectDBModule
+from dimos.perception.spatial_perception import SpatialMemory
 from dimos.protocol import pubsub
 from dimos.protocol.pubsub.lcmpubsub import LCM
 from dimos.robot.foxglove_bridge import FoxgloveBridge
-from dimos.web.websocket_vis.websocket_vis_module import WebsocketVisModule
-from dimos.robot.unitree_webrtc.connection import UnitreeWebRTCConnection
-from dimos.robot.unitree_webrtc.unitree_skills import MyUnitreeSkills
-from dimos.robot.ros_bridge import ROSBridge, BridgeDirection
-from geometry_msgs.msg import TwistStamped as ROSTwistStamped
-from nav_msgs.msg import Odometry as ROSOdometry
-from sensor_msgs.msg import PointCloud2 as ROSPointCloud2
-from tf2_msgs.msg import TFMessage as ROSTFMessage
-from dimos.skills.skills import SkillLibrary
 from dimos.robot.robot import Robot
 from dimos.robot.ros_bridge import BridgeDirection, ROSBridge
 from dimos.robot.unitree_webrtc.connection import UnitreeWebRTCConnection
@@ -130,6 +140,7 @@ def __init__(
         enable_joystick: bool = False,
         enable_connection: bool = True,
         enable_ros_bridge: bool = True,
+        enable_perception: bool = False,
         enable_camera: bool = False,
     ):
         """Initialize the G1 robot.
@@ -144,7 +155,7 @@ def __init__(
             enable_joystick: Enable pygame joystick control
             enable_connection: Enable robot connection module
             enable_ros_bridge: Enable ROS bridge
-            enable_camera: Enable ZED camera module
+            enable_camera: Enable web camera module
         """
         super().__init__()
         self.ip = ip
@@ -154,6 +165,7 @@ def __init__(
         self.enable_joystick = enable_joystick
         self.enable_connection = enable_connection
         self.enable_ros_bridge = enable_ros_bridge
+        self.enable_perception = enable_perception
         self.enable_camera = enable_camera
         self.websocket_port = websocket_port
         self.lcm = LCM()
@@ -176,8 +188,7 @@ def __init__(
         self.spatial_memory_module = None
         self.joystick = None
         self.ros_bridge = None
-        self.zed_camera = None
-
+        self.camera = None
         self._setup_directories()
 
     def _setup_directories(self):
@@ -230,7 +241,7 @@ def _deploy_detection(self, goto):
 
     def start(self):
         """Start the robot system with all modules."""
-        self.dimos = core.start(4)  # 2 workers for connection and visualization
+        self.dimos = core.start(8)  # 2 workers for connection and visualization
 
         if self.enable_connection:
             self._deploy_connection()
@@ -246,7 +257,15 @@ def start(self):
         if self.enable_ros_bridge:
             self._deploy_ros_bridge()
 
-        self._start_modules()
+        self.nav = self.dimos.deploy(NavigationModule)
+        self.nav.goal_reached.transport = core.LCMTransport("/goal_reached", Bool)
+        self.nav.goal_pose.transport = core.LCMTransport("/goal_pose", PoseStamped)
+        self.nav.cancel_goal.transport = core.LCMTransport("/cancel_goal", Bool)
+        self.nav.joy.transport = core.LCMTransport("/joy", Joy)
+        self.nav.start()
+
+        self._deploy_camera()
+        self._deploy_detection(self.nav.go_to)
 
         self.lcm.start()
 
@@ -385,9 +404,30 @@ def _deploy_ros_bridge(self):
             "/tf", TFMessage, ROSTFMessage, direction=BridgeDirection.ROS_TO_DIMOS
         )
 
-        # Add /registered_scan topic from ROS to DIMOS
+        from geometry_msgs.msg import PoseStamped as ROSPoseStamped
+        from std_msgs.msg import Bool as ROSBool
+
+        from dimos.msgs.std_msgs import Bool
+
+        # Navigation control topics from autonomy stack
+        self.ros_bridge.add_topic(
+            "/goal_pose", PoseStamped, ROSPoseStamped, direction=BridgeDirection.DIMOS_TO_ROS
+        )
+        self.ros_bridge.add_topic(
+            "/cancel_goal", Bool, ROSBool, direction=BridgeDirection.DIMOS_TO_ROS
+        )
+        self.ros_bridge.add_topic(
+            "/goal_reached", Bool, ROSBool, direction=BridgeDirection.ROS_TO_DIMOS
+        )
+
+        self.ros_bridge.add_topic("/joy", Joy, ROSJoy, direction=BridgeDirection.DIMOS_TO_ROS)
+
         self.ros_bridge.add_topic(
-            "/registered_scan", PointCloud2, ROSPointCloud2, direction=BridgeDirection.ROS_TO_DIMOS
+            "/registered_scan",
+            PointCloud2,
+            ROSPointCloud2,
+            direction=BridgeDirection.ROS_TO_DIMOS,
+            remap_topic="/map",
         )
 
         logger.info(
@@ -473,9 +513,23 @@ def main():
         enable_camera=args.camera,
         enable_connection=os.getenv("ROBOT_IP") is not None,
         enable_ros_bridge=True,
+        enable_perception=True,
     )
     robot.start()
 
+    # time.sleep(7)
+    # print("Starting navigation...")
+    # print(
+    #     robot.nav.go_to(
+    #         PoseStamped(
+    #             ts=time.time(),
+    #             frame_id="map",
+    #             position=Vector3(0.0, 0.0, 0.03),
+    #             orientation=Quaternion(0, 0, 0, 0),
+    #         ),
+    #         timeout=10,
+    #     ),
+    # )
     try:
         if args.joystick:
             print("\n" + "=" * 50)

From 6b04663b236169371e446abfcc0c135cc5f54a97 Mon Sep 17 00:00:00 2001
From: alexlin2 <alex.lin416@outlook.com>
Date: Fri, 10 Oct 2025 17:13:00 -0400
Subject: [PATCH 15/19] should pass tests

---
 dimos/msgs/std_msgs/Bool.py | 1 +
 dimos/msgs/std_msgs/Int8.py | 8 +++++++-
 2 files changed, 8 insertions(+), 1 deletion(-)

diff --git a/dimos/msgs/std_msgs/Bool.py b/dimos/msgs/std_msgs/Bool.py
index 1303f25da9..6af250277e 100644
--- a/dimos/msgs/std_msgs/Bool.py
+++ b/dimos/msgs/std_msgs/Bool.py
@@ -16,6 +16,7 @@
 """Bool message type."""
 
 from typing import ClassVar
+
 from dimos_lcm.std_msgs import Bool as LCMBool
 
 try:
diff --git a/dimos/msgs/std_msgs/Int8.py b/dimos/msgs/std_msgs/Int8.py
index f87a96a9ad..e4a4a24e17 100644
--- a/dimos/msgs/std_msgs/Int8.py
+++ b/dimos/msgs/std_msgs/Int8.py
@@ -19,7 +19,11 @@
 
 from typing import ClassVar
 from dimos_lcm.std_msgs import Int8 as LCMInt8
-from std_msgs.msg import Int8 as ROSInt8
+
+try:
+    from std_msgs.msg import Int8 as ROSInt8
+except ImportError:
+    ROSInt8 = None
 
 
 class Int8(LCMInt8):
@@ -49,6 +53,8 @@ def to_ros_msg(self) -> ROSInt8:
         Returns:
             ROS Int8 message
         """
+        if ROSInt8 is None:
+            raise ImportError("ROS std_msgs not available")
         ros_msg = ROSInt8()
         ros_msg.data = self.data
         return ros_msg

From 351d167c5cb2d1280979ef52760de1f0533daa95 Mon Sep 17 00:00:00 2001
From: alexlin2 <alex.lin416@outlook.com>
Date: Mon, 13 Oct 2025 15:32:17 -0400
Subject: [PATCH 16/19] made changes given comments

---
 dimos/navigation/rosnav/__init__.py      |  2 ++
 dimos/navigation/{ => rosnav}/nav_bot.py | 15 ++++-----
 dimos/navigation/rosnav/rosnav.py        | 41 ++++++++++++++++++++++++
 3 files changed, 50 insertions(+), 8 deletions(-)
 create mode 100644 dimos/navigation/rosnav/__init__.py
 rename dimos/navigation/{ => rosnav}/nav_bot.py (98%)
 create mode 100644 dimos/navigation/rosnav/rosnav.py

diff --git a/dimos/navigation/rosnav/__init__.py b/dimos/navigation/rosnav/__init__.py
new file mode 100644
index 0000000000..a88bffeb43
--- /dev/null
+++ b/dimos/navigation/rosnav/__init__.py
@@ -0,0 +1,2 @@
+from dimos.navigation.rosnav.rosnav import ROSNav
+from dimos.navigation.rosnav.nav_bot import ROSNavigationModule, NavBot
diff --git a/dimos/navigation/nav_bot.py b/dimos/navigation/rosnav/nav_bot.py
similarity index 98%
rename from dimos/navigation/nav_bot.py
rename to dimos/navigation/rosnav/nav_bot.py
index afbc610548..3e7e973c76 100644
--- a/dimos/navigation/nav_bot.py
+++ b/dimos/navigation/rosnav/nav_bot.py
@@ -35,9 +35,9 @@
 from dimos.msgs.sensor_msgs import PointCloud2, Joy, Image
 from dimos.msgs.std_msgs import Bool
 from dimos.msgs.tf2_msgs.TFMessage import TFMessage
-from dimos.protocol.tf import TF
 from dimos.utils.transform_utils import euler_to_quaternion
 from dimos.utils.logging_config import setup_logger
+from dimos.navigation.rosnav import ROSNav
 
 # ROS2 message imports
 from geometry_msgs.msg import TwistStamped as ROSTwistStamped
@@ -46,7 +46,7 @@
 from nav_msgs.msg import Odometry as ROSOdometry
 from nav_msgs.msg import Path as ROSPath
 from sensor_msgs.msg import PointCloud2 as ROSPointCloud2, Joy as ROSJoy
-from std_msgs.msg import Bool as ROSBool, Header, Int8 as ROSInt8
+from std_msgs.msg import Bool as ROSBool, Int8 as ROSInt8
 from tf2_msgs.msg import TFMessage as ROSTFMessage
 from dimos.utils.logging_config import setup_logger
 from dimos.protocol.pubsub.lcmpubsub import LCM, Topic
@@ -55,7 +55,7 @@
 logger = setup_logger("dimos.robot.unitree_webrtc.nav_bot", level=logging.INFO)
 
 
-class NavigationModule(Module):
+class ROSNavigationModule(ROSNav):
     """
     Handles navigation control and odometry remapping.
     """
@@ -116,6 +116,7 @@ def __init__(self, sensor_to_base_link_transform=None, *args, **kwargs):
             ROSPointCloud2, "/terrain_map_ext", self._on_ros_global_pointcloud, 10
         )
         self.path_sub = self._node.create_subscription(ROSPath, "/path", self._on_ros_path, 10)
+        self.tf_sub = self._node.create_subscription(ROSTFMessage, "/tf", self._on_ros_tf, 10)
 
         logger.info("NavigationModule initialized with ROS2 node")
 
@@ -189,7 +190,6 @@ def _on_ros_path(self, msg: ROSPath):
     def _on_ros_tf(self, msg: ROSTFMessage):
         ros_tf = TFMessage.from_ros_msg(msg)
 
-        # Publish static transforms
         translation = Vector3(
             self.sensor_to_base_link_transform[0],
             self.sensor_to_base_link_transform[1],
@@ -207,7 +207,7 @@ def _on_ros_tf(self, msg: ROSTFMessage):
             rotation=rotation,
             frame_id="sensor",
             child_frame_id="base_link",
-            ts=msg.ts,
+            ts=time.time(),
         )
 
         map_to_world_tf = Transform(
@@ -215,10 +215,10 @@ def _on_ros_tf(self, msg: ROSTFMessage):
             rotation=euler_to_quaternion(Vector3(0.0, 0.0, 0.0)),
             frame_id="map",
             child_frame_id="world",
-            ts=msg.ts,
+            ts=time.time(),
         )
 
-        self.tf.publish(sensor_to_base_link_tf, map_to_world_tf, ros_tf)
+        self.tf.publish(sensor_to_base_link_tf, map_to_world_tf, *ros_tf.transforms)
 
     def _on_goal_pose(self, msg: PoseStamped):
         self.navigate_to(msg)
@@ -456,7 +456,6 @@ def deploy_navigation_modules(self, bridge_name="nav_bot_ros_bridge"):
 
         self.navigation_module.goal_req.transport = core.LCMTransport("/goal", PoseStamped)
         self.navigation_module.cancel_goal.transport = core.LCMTransport("/cancel_goal", Bool)
-        self.navigation_module.soft_stop.transport = core.LCMTransport("/soft_stop", Int8)
 
         self.navigation_module.pointcloud.transport = core.LCMTransport(
             "/pointcloud_map", PointCloud2
diff --git a/dimos/navigation/rosnav/rosnav.py b/dimos/navigation/rosnav/rosnav.py
new file mode 100644
index 0000000000..a80c79e985
--- /dev/null
+++ b/dimos/navigation/rosnav/rosnav.py
@@ -0,0 +1,41 @@
+# Copyright 2025 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 import In, Module, Out
+from dimos.msgs.geometry_msgs import PoseStamped, Twist
+from dimos.msgs.nav_msgs import Path
+from dimos.msgs.sensor_msgs import PointCloud2
+from dimos.msgs.std_msgs import Bool
+
+
+class ROSNav(Module):
+    goal_req: In[PoseStamped] = None  # type: ignore
+    goal_active: Out[PoseStamped] = None  # type: ignore
+    path_active: Out[Path] = None  # type: ignore
+    cancel_goal: In[Bool] = None  # type: ignore
+    cmd_vel: Out[Twist] = None  # type: ignore
+
+    # PointcloudPerception attributes
+    pointcloud: Out[PointCloud2] = None  # type: ignore
+
+    # Global3DMapSpec attributes
+    global_pointcloud: Out[PointCloud2] = None  # type: ignore
+
+    def navigate_to(self, target: PoseStamped) -> None:
+        # TODO: Implement navigation logic
+        pass
+
+    def stop_navigation(self) -> None:
+        # TODO: Implement stop logic
+        pass

From 7ea70460bce0649f43544478381a88251b39b8ca Mon Sep 17 00:00:00 2001
From: alexlin2 <alex.lin416@outlook.com>
Date: Tue, 14 Oct 2025 19:12:12 -0400
Subject: [PATCH 17/19] rebased origin

---
 dimos/navigation/rosnav/nav_bot.py       | 281 ++++-------------------
 dimos/robot/unitree_webrtc/unitree_g1.py |  54 ++---
 2 files changed, 73 insertions(+), 262 deletions(-)

diff --git a/dimos/navigation/rosnav/nav_bot.py b/dimos/navigation/rosnav/nav_bot.py
index 3e7e973c76..3829171183 100644
--- a/dimos/navigation/rosnav/nav_bot.py
+++ b/dimos/navigation/rosnav/nav_bot.py
@@ -28,11 +28,10 @@
 
 from dimos import core
 from dimos.protocol import pubsub
-from dimos.core import Module, In, Out, rpc
-from dimos.core.resource import Resource
-from dimos.msgs.geometry_msgs import PoseStamped, TwistStamped, Transform, Vector3
-from dimos.msgs.nav_msgs import Odometry
-from dimos.msgs.sensor_msgs import PointCloud2, Joy, Image
+from dimos.core import In, Out, rpc
+from dimos.msgs.geometry_msgs import PoseStamped, Twist, Transform, Vector3, Quaternion
+from dimos.msgs.nav_msgs import Odometry, Path
+from dimos.msgs.sensor_msgs import PointCloud2, Joy
 from dimos.msgs.std_msgs import Bool
 from dimos.msgs.tf2_msgs.TFMessage import TFMessage
 from dimos.utils.transform_utils import euler_to_quaternion
@@ -48,9 +47,6 @@
 from sensor_msgs.msg import PointCloud2 as ROSPointCloud2, Joy as ROSJoy
 from std_msgs.msg import Bool as ROSBool, Int8 as ROSInt8
 from tf2_msgs.msg import TFMessage as ROSTFMessage
-from dimos.utils.logging_config import setup_logger
-from dimos.protocol.pubsub.lcmpubsub import LCM, Topic
-from reactivex.disposable import Disposable
 
 logger = setup_logger("dimos.robot.unitree_webrtc.nav_bot", level=logging.INFO)
 
@@ -122,14 +118,12 @@ def __init__(self, sensor_to_base_link_transform=None, *args, **kwargs):
 
     @rpc
     def start(self):
-        super().start()
-        if self.goal_reached:
-            unsub = self.goal_reached.subscribe(self._on_goal_reached)
-            self._disposables.add(Disposable(unsub))
+        self._running = True
+        self.spin_thread = threading.Thread(target=self._spin_node, daemon=True)
+        self.spin_thread.start()
 
-    @rpc
-    def stop(self) -> None:
-        super().stop()
+        self.goal_req.subscribe(self._on_goal_pose)
+        self.cancel_goal.subscribe(self._on_cancel_goal)
 
         logger.info("NavigationModule started with ROS2 spinning")
 
@@ -291,8 +285,7 @@ def navigate_to(self, pose: PoseStamped, timeout: float = 60.0) -> bool:
                 return self.goal_reach
             time.sleep(0.1)
 
-        self.stop_navigation()
-
+        self.stop()
         logger.warning(f"Navigation timed out after {timeout} seconds")
         return False
 
@@ -327,77 +320,7 @@ def stop(self):
             logger.error(f"Error during shutdown: {e}")
 
 
-class TopicRemapModule(Module):
-    """Module that remaps Odometry to PoseStamped and publishes static transforms."""
-
-    odom: In[Odometry] = None
-    odom_pose: Out[PoseStamped] = None
-
-    def __init__(self, sensor_to_base_link_transform=None, *args, **kwargs):
-        Module.__init__(self, *args, **kwargs)
-        self.tf = TF()
-        self.sensor_to_base_link_transform = sensor_to_base_link_transform or [
-            0.0,
-            0.0,
-            0.0,
-            0.0,
-            0.0,
-            0.0,
-        ]
-
-    @rpc
-    def start(self):
-        super().start()
-        unsub = self.odom.subscribe(self._publish_odom_pose)
-        self._disposables.add(Disposable(unsub))
-
-    @rpc
-    def stop(self) -> None:
-        super().stop()
-
-    def _publish_odom_pose(self, msg: Odometry):
-        pose_msg = PoseStamped(
-            ts=msg.ts,
-            frame_id=msg.frame_id,
-            position=msg.pose.pose.position,
-            orientation=msg.pose.pose.orientation,
-        )
-        self.odom_pose.publish(pose_msg)
-
-        # Publish static transform from sensor to base_link
-        translation = Vector3(
-            self.sensor_to_base_link_transform[0],
-            self.sensor_to_base_link_transform[1],
-            self.sensor_to_base_link_transform[2],
-        )
-        euler_angles = Vector3(
-            self.sensor_to_base_link_transform[3],
-            self.sensor_to_base_link_transform[4],
-            self.sensor_to_base_link_transform[5],
-        )
-        rotation = euler_to_quaternion(euler_angles)
-
-        sensor_to_base_link_tf = Transform(
-            translation=translation,
-            rotation=rotation,
-            frame_id="sensor",
-            child_frame_id="base_link",
-            ts=msg.ts,
-        )
-
-        # map to world static transform
-        map_to_world_tf = Transform(
-            translation=Vector3(0.0, 0.0, 0.0),
-            rotation=euler_to_quaternion(Vector3(0.0, 0.0, 0.0)),
-            frame_id="map",
-            child_frame_id="world",
-            ts=msg.ts,
-        )
-
-        self.tf.publish(sensor_to_base_link_tf, map_to_world_tf)
-
-
-class NavBot(Resource):
+class NavBot:
     """
     NavBot wrapper that deploys NavigationModule with proper DIMOS/ROS2 integration.
     """
@@ -426,32 +349,9 @@ def __init__(self, dimos=None, sensor_to_base_link_transform=None):
         self.navigation_module = None
 
     def start(self):
-        super().start()
-
-        if self.topic_remap_module:
-            self.topic_remap_module.start()
-            logger.info("Topic remap module started")
-
-        if self.ros_bridge:
-            logger.info("ROS bridge started")
-
-    def stop(self) -> None:
-        logger.info("Shutting down navigation modules...")
-
-        if self.ros_bridge is not None:
-            try:
-                self.ros_bridge.shutdown()
-                logger.info("ROS bridge shut down successfully")
-            except Exception as e:
-                logger.error(f"Error shutting down ROS bridge: {e}")
-
-        super().stop()
-
-    def deploy_navigation_modules(self, bridge_name="nav_bot_ros_bridge"):
-        # Deploy topic remap module
-        logger.info("Deploying topic remap module...")
-        self.topic_remap_module = self.dimos.deploy(
-            TopicRemapModule, sensor_to_base_link_transform=self.sensor_to_base_link_transform
+        logger.info("Deploying navigation module...")
+        self.navigation_module = self.dimos.deploy(
+            ROSNavigationModule, sensor_to_base_link_transform=self.sensor_to_base_link_transform
         )
 
         self.navigation_module.goal_req.transport = core.LCMTransport("/goal", PoseStamped)
@@ -472,135 +372,52 @@ def deploy_navigation_modules(self, bridge_name="nav_bot_ros_bridge"):
         self.navigation_module.cmd_vel.transport = core.LCMTransport("/cmd_vel", Twist)
         self.navigation_module.odom_pose.transport = core.LCMTransport("/odom_pose", PoseStamped)
 
-        # Deploy ROS bridge
-        logger.info("Deploying ROS bridge...")
-        self.ros_bridge = ROSBridge(bridge_name)
+        self.navigation_module.start()
 
-        # Configure ROS topics
-        self.ros_bridge.add_topic(
-            "/cmd_vel", TwistStamped, ROSTwistStamped, direction=BridgeDirection.ROS_TO_DIMOS
-        )
-        self.ros_bridge.add_topic(
-            "/state_estimation",
-            Odometry,
-            ROSOdometry,
-            direction=BridgeDirection.ROS_TO_DIMOS,
-            remap_topic="/odom",
-        )
-        self.ros_bridge.add_topic(
-            "/tf", TFMessage, ROSTFMessage, direction=BridgeDirection.ROS_TO_DIMOS
-        )
-        self.ros_bridge.add_topic(
-            "/registered_scan", PointCloud2, ROSPointCloud2, direction=BridgeDirection.ROS_TO_DIMOS
-        )
-        self.ros_bridge.add_topic("/joy", Joy, ROSJoy, direction=BridgeDirection.DIMOS_TO_ROS)
-        # Navigation control topics from autonomy stack
-        self.ros_bridge.add_topic(
-            "/goal_pose", PoseStamped, ROSPoseStamped, direction=BridgeDirection.DIMOS_TO_ROS
-        )
-        self.ros_bridge.add_topic(
-            "/cancel_goal", Bool, ROSBool, direction=BridgeDirection.DIMOS_TO_ROS
-        )
-        self.ros_bridge.add_topic(
-            "/goal_reached", Bool, ROSBool, direction=BridgeDirection.ROS_TO_DIMOS
-        )
-
-        # self.ros_bridge.add_topic(
-        #     "/camera/image", Image, ROSImage, direction=BridgeDirection.ROS_TO_DIMOS
-        # )
+    def shutdown(self):
+        logger.info("Shutting down NavBot...")
 
-    def _set_autonomy_mode(self):
-        """
-        Set autonomy mode by publishing Joy message.
-        """
-
-        joy_msg = Joy(
-            frame_id="dimos",
-            axes=[
-                0.0,  # axis 0
-                0.0,  # axis 1
-                -1.0,  # axis 2
-                0.0,  # axis 3
-                1.0,  # axis 4
-                1.0,  # axis 5
-                0.0,  # axis 6
-                0.0,  # axis 7
-            ],
-            buttons=[
-                0,  # button 0
-                0,  # button 1
-                0,  # button 2
-                0,  # button 3
-                0,  # button 4
-                0,  # button 5
-                0,  # button 6
-                1,  # button 7 - controls autonomy mode
-                0,  # button 8
-                0,  # button 9
-                0,  # button 10
-            ],
-        )
-
-        # Start the navigation module
         if self.navigation_module:
-            self.navigation_module.start()
-            logger.info("Navigation module started")
+            self.navigation_module.stop()
 
-    def navigate_to_goal(self, pose: PoseStamped, blocking: bool = True, timeout: float = 30.0):
-        """Navigate to a target pose using ROS topics.
+        if rclpy.ok():
+            rclpy.shutdown()
 
-        Args:
-            pose: Target pose to navigate to
-            blocking: If True, block until goal is reached. If False, return immediately.
-            timeout: Maximum time to wait for goal to be reached (seconds)
-
-        Returns:
-            If blocking=True: True if navigation was successful, False otherwise
-            If blocking=False: True if goal was sent successfully
-        """
-        logger.info(
-            f"Navigating to goal: ({pose.position.x:.2f}, {pose.position.y:.2f}, {pose.position.z:.2f})"
-        )
+        logger.info("NavBot shutdown complete")
 
-        # Publish goal to /goal_pose topic
-        self._set_autonomy_mode()
-        goal_topic = Topic("/goal_pose", PoseStamped)
-        self.lcm.publish(goal_topic, pose)
 
-        if not blocking:
-            return True
+def main():
+    pubsub.lcm.autoconf()
+    nav_bot = NavBot()
+    nav_bot.start()
 
-        # Wait for goal_reached signal
-        goal_reached_topic = Topic("/goal_reached", Bool)
-        start_time = time.time()
+    logger.info("\nTesting navigation in 2 seconds...")
+    time.sleep(2)
 
-        while time.time() - start_time < timeout:
-            try:
-                msg = self.lcm.wait_for_message(goal_reached_topic, timeout=0.5)
-                if msg and msg.data:
-                    logger.info("Navigation goal reached")
-                    return True
-                elif msg and not msg.data:
-                    logger.info("Navigation was cancelled or failed")
-                    return False
-            except Exception:
-                # Timeout on wait_for_message, continue looping
-                pass
+    test_pose = PoseStamped(
+        ts=time.time(),
+        frame_id="map",
+        position=Vector3(1.0, 1.0, 0.0),
+        orientation=Quaternion(0.0, 0.0, 0.0, 0.0),
+    )
 
-        logger.warning(f"Navigation timed out after {timeout} seconds")
-        return False
+    logger.info(f"Sending navigation goal to: (1.0, 1.0, 0.0)")
 
-    def cancel_navigation(self) -> bool:
-        """Cancel the current navigation goal using ROS topics.
+    if nav_bot.navigation_module:
+        success = nav_bot.navigation_module.navigate_to(test_pose, timeout=30.0)
+        if success:
+            logger.info("✓ Navigation goal reached!")
+        else:
+            logger.warning("✗ Navigation failed or timed out")
 
-        Returns:
-            True if cancel command was sent successfully
-        """
-        logger.info("Cancelling navigation goal")
+    try:
+        logger.info("\nNavBot running. Press Ctrl+C to stop.")
+        while True:
+            time.sleep(1)
+    except KeyboardInterrupt:
+        logger.info("\nShutting down...")
+        nav_bot.shutdown()
 
-        # Publish cancel command to /cancel_goal topic
-        cancel_topic = Topic("/cancel_goal", Bool)
-        cancel_msg = Bool(data=True)
-        self.lcm.publish(cancel_topic, cancel_msg)
 
-        return True
+if __name__ == "__main__":
+    main()
diff --git a/dimos/robot/unitree_webrtc/unitree_g1.py b/dimos/robot/unitree_webrtc/unitree_g1.py
index 52f7a67d9d..f319e2c87c 100644
--- a/dimos/robot/unitree_webrtc/unitree_g1.py
+++ b/dimos/robot/unitree_webrtc/unitree_g1.py
@@ -28,6 +28,8 @@
 from dimos.agents2.skills.ros_navigation import RosNavigation
 from dimos.agents2.spec import Model, Provider
 from dimos.core import In, Module, Out, rpc
+from dimos.core.dimos import Dimos
+from dimos.core.resource import Resource
 from dimos.hardware.camera import zed
 from dimos.hardware.camera.module import CameraModule
 from dimos.hardware.camera.webcam import Webcam
@@ -45,7 +47,6 @@
 from dimos.msgs.std_msgs.Bool import Bool
 from dimos.msgs.tf2_msgs.TFMessage import TFMessage
 from dimos.msgs.vision_msgs import Detection2DArray
-from dimos.perception.detection2d import Detection3DModule
 from dimos.perception.detection2d.moduleDB import ObjectDBModule
 from dimos.perception.spatial_perception import SpatialMemory
 from dimos.protocol import pubsub
@@ -99,10 +100,21 @@ def __init__(self, ip: str = None, connection_type: str = "webrtc", *args, **kwa
     @rpc
     def start(self):
         """Start the connection and subscribe to sensor streams."""
+
+        super().start()
+
         # Use the exact same UnitreeWebRTCConnection as Go2
         self.connection = UnitreeWebRTCConnection(self.ip)
-        self.movecmd.subscribe(self.move)
-        self.odom_in.subscribe(self._publish_odom_pose)
+        self.connection.start()
+        unsub = self.movecmd.subscribe(self.move)
+        self._disposables.add(Disposable(unsub))
+        unsub = self.odom_in.subscribe(self._publish_odom_pose)
+        self._disposables.add(Disposable(unsub))
+
+    @rpc
+    def stop(self) -> None:
+        self.connection.stop()
+        super().stop()
 
     def _publish_odom_pose(self, msg: Odometry):
         self.odom_pose.publish(
@@ -126,7 +138,7 @@ def publish_request(self, topic: str, data: dict):
         return self.connection.publish_request(topic, data)
 
 
-class UnitreeG1(Robot):
+class UnitreeG1(Robot, Resource):
     """Unitree G1 humanoid robot."""
 
     def __init__(
@@ -181,7 +193,7 @@ def __init__(
         self.capabilities = [RobotCapability.LOCOMOTION]
 
         # Module references
-        self.dimos = None
+        self._dimos = Dimos(n=4)
         self.connection = None
         self.websocket_vis = None
         self.foxglove_bridge = None
@@ -189,6 +201,7 @@ def __init__(
         self.joystick = None
         self.ros_bridge = None
         self.camera = None
+        self._ros_nav = None
         self._setup_directories()
 
     def _setup_directories(self):
@@ -240,8 +253,8 @@ def _deploy_detection(self, goto):
         self.detection = detection
 
     def start(self):
-        """Start the robot system with all modules."""
-        self.dimos = core.start(8)  # 2 workers for connection and visualization
+        self.lcm.start()
+        self._dimos.start()
 
         if self.enable_connection:
             self._deploy_connection()
@@ -257,7 +270,7 @@ def start(self):
         if self.enable_ros_bridge:
             self._deploy_ros_bridge()
 
-        self.nav = self.dimos.deploy(NavigationModule)
+        self.nav = self._dimos.deploy(NavigationModule)
         self.nav.goal_reached.transport = core.LCMTransport("/goal_reached", Bool)
         self.nav.goal_pose.transport = core.LCMTransport("/goal_pose", PoseStamped)
         self.nav.cancel_goal.transport = core.LCMTransport("/cancel_goal", Bool)
@@ -314,7 +327,7 @@ def stop(self) -> None:
 
     def _deploy_connection(self):
         """Deploy and configure the connection module."""
-        self.connection = self.dimos.deploy(G1ConnectionModule, self.ip)
+        self.connection = self._dimos.deploy(G1ConnectionModule, self.ip)
 
         # Configure LCM transports
         self.connection.movecmd.transport = core.LCMTransport("/cmd_vel", TwistStamped)
@@ -430,6 +443,8 @@ def _deploy_ros_bridge(self):
             remap_topic="/map",
         )
 
+        self.ros_bridge.start()
+
         logger.info(
             "ROS bridge deployed: /cmd_vel, /state_estimation, /tf, /registered_scan (ROS → DIMOS)"
         )
@@ -457,27 +472,6 @@ def get_odom(self) -> PoseStamped:
         # Note: odom functionality removed from G1ConnectionModule
         return None
 
-    def shutdown(self):
-        """Shutdown the robot and clean up resources."""
-        logger.info("Shutting down UnitreeG1...")
-
-        # Shutdown ROS bridge if it exists
-        if self.ros_bridge is not None:
-            try:
-                self.ros_bridge.shutdown()
-                logger.info("ROS bridge shut down successfully")
-            except Exception as e:
-                logger.error(f"Error shutting down ROS bridge: {e}")
-
-        # Stop other modules if needed
-        if self.websocket_vis:
-            try:
-                self.websocket_vis.stop()
-            except Exception as e:
-                logger.error(f"Error stopping websocket vis: {e}")
-
-        logger.info("UnitreeG1 shutdown complete")
-
     @property
     def spatial_memory(self) -> Optional[SpatialMemory]:
         return self.spatial_memory_module

From c5ae2e7ffb8621b886c78e31806a75461de7c5fc Mon Sep 17 00:00:00 2001
From: alexlin2 <alex.lin416@outlook.com>
Date: Tue, 14 Oct 2025 19:19:40 -0400
Subject: [PATCH 18/19] passing tests

---
 dimos/navigation/rosnav/rosnav.py | 6 ++++++
 1 file changed, 6 insertions(+)

diff --git a/dimos/navigation/rosnav/rosnav.py b/dimos/navigation/rosnav/rosnav.py
index a80c79e985..440a0f4269 100644
--- a/dimos/navigation/rosnav/rosnav.py
+++ b/dimos/navigation/rosnav/rosnav.py
@@ -32,6 +32,12 @@ class ROSNav(Module):
     # Global3DMapSpec attributes
     global_pointcloud: Out[PointCloud2] = None  # type: ignore
 
+    def start(self) -> None:
+        pass
+
+    def stop(self) -> None:
+        pass
+
     def navigate_to(self, target: PoseStamped) -> None:
         # TODO: Implement navigation logic
         pass

From 2f03976d5ca207922a5bb10df8ecb163c7e8854f Mon Sep 17 00:00:00 2001
From: alexlin2 <alex.lin416@outlook.com>
Date: Wed, 15 Oct 2025 22:39:38 -0400
Subject: [PATCH 19/19] small bug fix

---
 dimos/navigation/rosnav/nav_bot.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/dimos/navigation/rosnav/nav_bot.py b/dimos/navigation/rosnav/nav_bot.py
index 3829171183..4a5ca0c45a 100644
--- a/dimos/navigation/rosnav/nav_bot.py
+++ b/dimos/navigation/rosnav/nav_bot.py
@@ -285,7 +285,7 @@ def navigate_to(self, pose: PoseStamped, timeout: float = 60.0) -> bool:
                 return self.goal_reach
             time.sleep(0.1)
 
-        self.stop()
+        self.stop_navigation()
         logger.warning(f"Navigation timed out after {timeout} seconds")
         return False