diff --git a/AUTONOMY_STACK_README.md b/AUTONOMY_STACK_README.md new file mode 100644 index 0000000000..70eff131ce --- /dev/null +++ b/AUTONOMY_STACK_README.md @@ -0,0 +1,284 @@ +# Autonomy Stack API Documentation + +## 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: + +```bash +# 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/` + +## 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 + +```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) + +| 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 | +| `/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 | + +### 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/msgs/sensor_msgs/test_Joy.py b/dimos/msgs/sensor_msgs/test_Joy.py index fd11624b08..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...") @@ -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/msgs/std_msgs/Int8.py b/dimos/msgs/std_msgs/Int8.py new file mode 100644 index 0000000000..e4a4a24e17 --- /dev/null +++ b/dimos/msgs/std_msgs/Int8.py @@ -0,0 +1,60 @@ +#!/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 + +try: + from std_msgs.msg import Int8 as ROSInt8 +except ImportError: + ROSInt8 = None + + +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 + """ + if ROSInt8 is None: + raise ImportError("ROS std_msgs not available") + 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 898b1035b5..d46e2ce9a3 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 .Int8 import Int8 -__all__ = ["Bool", "Header", "Int32"] +__all__ = ["Bool", "Header", "Int32", "Int8"] 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/rosnav/nav_bot.py b/dimos/navigation/rosnav/nav_bot.py new file mode 100644 index 0000000000..4a5ca0c45a --- /dev/null +++ b/dimos/navigation/rosnav/nav_bot.py @@ -0,0 +1,423 @@ +#!/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 +import time +import threading + +import rclpy +from rclpy.node import Node +from rclpy.executors import SingleThreadedExecutor + +from dimos import core +from dimos.protocol import pubsub +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 +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 +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 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, Int8 as ROSInt8 +from tf2_msgs.msg import TFMessage as ROSTFMessage + +logger = setup_logger("dimos.robot.unitree_webrtc.nav_bot", level=logging.INFO) + + +class ROSNavigationModule(ROSNav): + """ + Handles navigation control and odometry remapping. + """ + + goal_req: In[PoseStamped] = None + cancel_goal: In[Bool] = 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[Twist] = None + odom_pose: Out[PoseStamped] = None + + def __init__(self, sensor_to_base_link_transform=None, *args, **kwargs): + super().__init__(*args, **kwargs) + if not rclpy.ok(): + rclpy.init() + self._node = Node("navigation_module") + + self.goal_reach = 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.spin_thread = None + + # ROS2 Publishers + 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._node.create_subscription( + ROSBool, "/goal_reached", self._on_ros_goal_reached, 10 + ) + self.odom_sub = self._node.create_subscription( + ROSOdometry, "/state_estimation", self._on_ros_odom, 10 + ) + self.cmd_vel_sub = self._node.create_subscription( + ROSTwistStamped, "/cmd_vel", self._on_ros_cmd_vel, 10 + ) + self.goal_waypoint_sub = self._node.create_subscription( + ROSPointStamped, "/way_point", self._on_ros_goal_waypoint, 10 + ) + self.registered_scan_sub = self._node.create_subscription( + ROSPointCloud2, "/registered_scan", self._on_ros_registered_scan, 10 + ) + self.global_pointcloud_sub = self._node.create_subscription( + 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") + + @rpc + def start(self): + self._running = True + self.spin_thread = threading.Thread(target=self._spin_node, daemon=True) + self.spin_thread.start() + + self.goal_req.subscribe(self._on_goal_pose) + self.cancel_goal.subscribe(self._on_cancel_goal) + + logger.info("NavigationModule started with ROS2 spinning") + + def _spin_node(self): + while self._running and rclpy.ok(): + try: + rclpy.spin_once(self._node, 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): + 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): + 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): + # 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): + 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 _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) + + 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=time.time(), + ) + + 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=time.time(), + ) + + 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) + + def _on_cancel_goal(self, msg: Bool): + if msg.data: + self.stop() + + def _set_autonomy_mode(self): + 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 ROS topics. + + Args: + pose: Target pose to navigate to + timeout: Maximum time to wait for goal (seconds) + + Returns: + True if navigation was successful + """ + logger.info( + f"Navigating to goal: ({pose.position.x:.2f}, {pose.position.y:.2f}, {pose.position.z:.2f})" + ) + + self.goal_reach = None + self._set_autonomy_mode() + + # 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: + soft_stop_msg.data = 2 + self.soft_stop_pub.publish(soft_stop_msg) + return self.goal_reach + time.sleep(0.1) + + self.stop_navigation() + logger.warning(f"Navigation timed out after {timeout} seconds") + return False + + @rpc + def stop_navigation(self) -> bool: + """ + Stop current navigation by publishing to ROS topics. + + Returns: + True if stop command was sent successfully + """ + logger.info("Stopping navigation") + + cancel_msg = ROSBool() + cancel_msg.data = True + self.cancel_goal_pub.publish(cancel_msg) + + soft_stop_msg = ROSInt8() + soft_stop_msg.data = 2 + self.soft_stop_pub.publish(soft_stop_msg) + + return True + + @rpc + def stop(self): + try: + self._running = False + if self.spin_thread: + self.spin_thread.join(timeout=1) + self._node.destroy_node() + except Exception as e: + logger.error(f"Error during shutdown: {e}") + + +class NavBot: + """ + NavBot wrapper that deploys NavigationModule with proper DIMOS/ROS2 integration. + """ + + 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 + """ + if dimos is None: + self.dimos = core.start(2) + else: + self.dimos = dimos + + 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 + + def start(self): + 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) + self.navigation_module.cancel_goal.transport = core.LCMTransport("/cancel_goal", Bool) + + 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", Twist) + self.navigation_module.odom_pose.transport = core.LCMTransport("/odom_pose", PoseStamped) + + self.navigation_module.start() + + def shutdown(self): + logger.info("Shutting down NavBot...") + + if self.navigation_module: + self.navigation_module.stop() + + if rclpy.ok(): + rclpy.shutdown() + + logger.info("NavBot shutdown complete") + + +def main(): + pubsub.lcm.autoconf() + nav_bot = NavBot() + nav_bot.start() + + logger.info("\nTesting navigation in 2 seconds...") + time.sleep(2) + + 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.info(f"Sending navigation goal to: (1.0, 1.0, 0.0)") + + 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") + + try: + logger.info("\nNavBot running. Press Ctrl+C to stop.") + while True: + time.sleep(1) + except KeyboardInterrupt: + logger.info("\nShutting down...") + nav_bot.shutdown() + + +if __name__ == "__main__": + main() diff --git a/dimos/navigation/rosnav/rosnav.py b/dimos/navigation/rosnav/rosnav.py new file mode 100644 index 0000000000..440a0f4269 --- /dev/null +++ b/dimos/navigation/rosnav/rosnav.py @@ -0,0 +1,47 @@ +# 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 start(self) -> None: + pass + + def stop(self) -> None: + pass + + def navigate_to(self, target: PoseStamped) -> None: + # TODO: Implement navigation logic + pass + + def stop_navigation(self) -> None: + # TODO: Implement stop logic + pass diff --git a/dimos/robot/nav_bot.py b/dimos/robot/nav_bot.py deleted file mode 100644 index e65ed8214b..0000000000 --- a/dimos/robot/nav_bot.py +++ /dev/null @@ -1,423 +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 -import time - -from dimos import core -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.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 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, Joy as ROSJoy, Image as ROSImage -from std_msgs.msg import Bool as ROSBool -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) - -############################################################ -# 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): - goal_reached: In[Bool] = None - - goal_pose: Out[PoseStamped] = None - cancel_goal: Out[Bool] = None - joy: Out[Joy] = None - - def __init__(self, *args, **kwargs): - """Initialize NavigationModule.""" - Module.__init__(self, *args, **kwargs) - self.goal_reach = None - - @rpc - def start(self): - super().start() - if self.goal_reached: - unsub = self.goal_reached.subscribe(self._on_goal_reached) - self._disposables.add(Disposable(unsub)) - - @rpc - def stop(self) -> None: - super().stop() - - def _on_goal_reached(self, msg: Bool): - """Handle goal reached status messages.""" - self.goal_reach = msg.data - - 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 - ], - ) - - if self.joy: - self.joy.publish(joy_msg) - logger.info(f"Setting autonomy mode via Joy message") - - @rpc - def go_to(self, pose: PoseStamped, timeout: float = 60.0) -> bool: - """ - Navigate to a target pose by publishing to LCM 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) - """ - logger.info( - f"Navigating to goal: ({pose.position.x:.2f}, {pose.position.y:.2f}, {pose.position.z:.2f})" - ) - - self.goal_reach = None - self._set_autonomy_mode() - self.goal_pose.publish(pose) - - start_time = time.time() - while time.time() - start_time < timeout: - if self.goal_reach is not None: - return self.goal_reach - time.sleep(0.1) - - self.stop_navigation() - - logger.warning(f"Navigation timed out after {timeout} seconds") - return False - - @rpc - def stop_navigation(self) -> bool: - """ - Cancel current navigation by publishing to cancel_goal. - - 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) - return True - - return False - - -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): - """ - 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() - self.lcm = LCM() - - 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 - ) - 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("/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 _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 - ], - ) - - self.lcm.publish(Topic("/joy", Joy), joy_msg) - - def navigate_to_goal(self, pose: PoseStamped, blocking: bool = True, timeout: float = 30.0): - """Navigate to a target pose using ROS topics. - - 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})" - ) - - # 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 - - # Wait for goal_reached signal - goal_reached_topic = Topic("/goal_reached", Bool) - start_time = time.time() - - 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 - - logger.warning(f"Navigation timed out after {timeout} seconds") - return False - - def cancel_navigation(self) -> bool: - """Cancel the current navigation goal using ROS topics. - - Returns: - True if cancel command was sent successfully - """ - logger.info("Cancelling navigation goal") - - # 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