From 99a9190b04fa06f613e0b9dbcf3eecb259d25aef Mon Sep 17 00:00:00 2001 From: alexlin2 Date: Thu, 18 Sep 2025 17:34:29 -0400 Subject: [PATCH 01/12] created new navbot class --- dimos/robot/unitree_webrtc/nav_bot.py | 171 +++++++++++++++++++++++ dimos/robot/unitree_webrtc/unitree_g1.py | 85 ++--------- 2 files changed, 181 insertions(+), 75 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 2963c2cfc6..8a06685e7f 100644 --- a/dimos/robot/unitree_webrtc/unitree_g1.py +++ b/dimos/robot/unitree_webrtc/unitree_g1.py @@ -26,7 +26,6 @@ from dimos import core from dimos.core import Module, In, Out, rpc from dimos.msgs.geometry_msgs import PoseStamped, Twist, TwistStamped -from dimos.msgs.nav_msgs.Odometry import Odometry from dimos.msgs.sensor_msgs import Image, CameraInfo, PointCloud2 from dimos.msgs.tf2_msgs.TFMessage import TFMessage from dimos.protocol import pubsub @@ -35,11 +34,7 @@ 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.robot.unitree_webrtc.nav_bot import NavBot from dimos.skills.skills import SkillLibrary from dimos.robot.robot import Robot @@ -66,9 +61,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" @@ -80,35 +72,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.""" - # 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): +class UnitreeG1(Robot, NavBot): """Unitree G1 humanoid robot.""" def __init__( @@ -121,8 +98,8 @@ def __init__( replay_path: str = None, enable_joystick: bool = False, enable_connection: bool = True, - enable_ros_bridge: bool = True, enable_camera: bool = False, + sensor_to_base_link_transform=None, ): """Initialize the G1 robot. @@ -135,17 +112,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 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_camera = enable_camera self.websocket_port = websocket_port self.lcm = LCM() @@ -161,12 +138,10 @@ def __init__( self.capabilities = [RobotCapability.LOCOMOTION] # Module references - self.dimos = None self.connection = None self.websocket_vis = None self.foxglove_bridge = None self.joystick = None - self.ros_bridge = None self.zed_camera = None self._setup_directories() @@ -178,7 +153,6 @@ def _setup_directories(self): 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() @@ -191,8 +165,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._start_modules() @@ -204,11 +177,7 @@ def start(self): 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 the ZED camera module (real or fake based on replay_path).""" @@ -268,34 +237,6 @@ 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.""" if self.connection: @@ -306,6 +247,8 @@ def _start_modules(self): if self.joystick: self.joystick.start() + self.start_navigation_modules() + # Initialize skills after connection is established if self.skill_library is not None: for skill in self.skill_library: @@ -329,15 +272,8 @@ 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}") + self.shutdown_navigation() - # Stop other modules if needed if self.websocket_vis: try: self.websocket_vis.stop() @@ -375,7 +311,6 @@ 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() From 8239fed56e78c5511a3372e2a15ee4f355177377 Mon Sep 17 00:00:00 2001 From: alexlin2 Date: Sat, 20 Sep 2025 04:59:36 -0400 Subject: [PATCH 02/12] 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 8a06685e7f..f1c1fda98d 100644 --- a/dimos/robot/unitree_webrtc/unitree_g1.py +++ b/dimos/robot/unitree_webrtc/unitree_g1.py @@ -99,7 +99,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. @@ -113,10 +112,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 2992a9a47aa6eb64bd61d0f7bdf6efc8c9101c7b Mon Sep 17 00:00:00 2001 From: alexlin2 Date: Mon, 22 Sep 2025 15:22:56 -0400 Subject: [PATCH 03/12] added navigate to goal and cancel navigation to navbot --- dimos/msgs/std_msgs/Bool.py | 58 ++++++++++++++ dimos/msgs/std_msgs/__init__.py | 3 +- dimos/robot/{unitree_webrtc => }/nav_bot.py | 86 +++++++++++++++++---- dimos/robot/unitree_webrtc/unitree_g1.py | 28 ++++--- 4 files changed, 151 insertions(+), 24 deletions(-) create mode 100644 dimos/msgs/std_msgs/Bool.py rename dimos/robot/{unitree_webrtc => }/nav_bot.py (73%) diff --git a/dimos/msgs/std_msgs/Bool.py b/dimos/msgs/std_msgs/Bool.py new file mode 100644 index 0000000000..791b2403c8 --- /dev/null +++ b/dimos/msgs/std_msgs/Bool.py @@ -0,0 +1,58 @@ +#!/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. + +"""Bool message type.""" + +from typing import ClassVar +from dimos_lcm.std_msgs import Bool as LCMBool + +try: + from std_msgs.msg import Bool as ROSBool +except ImportError: + ROSBool = None + + +class Bool(LCMBool): + """ROS-compatible Bool message.""" + + msg_name: ClassVar[str] = "std_msgs.Bool" + + def __init__(self, data: bool = False): + """Initialize Bool with data value.""" + self.data = data + + @classmethod + def from_ros_msg(cls, ros_msg: ROSBool) -> "Bool": + """Create a Bool from a ROS std_msgs/Bool message. + + Args: + ros_msg: ROS Bool message + + Returns: + Bool instance + """ + return cls(data=ros_msg.data) + + def to_ros_msg(self) -> ROSBool: + """Convert to a ROS std_msgs/Bool message. + + Returns: + ROS Bool message + """ + if ROSBool is None: + raise ImportError("ROS std_msgs not available") + ros_msg = ROSBool() + ros_msg.data = bool(self.data) + return ros_msg diff --git a/dimos/msgs/std_msgs/__init__.py b/dimos/msgs/std_msgs/__init__.py index 3ed93fa77d..27061ff6ed 100644 --- a/dimos/msgs/std_msgs/__init__.py +++ b/dimos/msgs/std_msgs/__init__.py @@ -14,5 +14,6 @@ from .Header import Header from .Int32 import Int32 +from .Bool import Bool -__all__ = ["Header", "Int32"] +__all__ = ["Header", "Int32", "Bool"] diff --git a/dimos/robot/unitree_webrtc/nav_bot.py b/dimos/robot/nav_bot.py similarity index 73% rename from dimos/robot/unitree_webrtc/nav_bot.py rename to dimos/robot/nav_bot.py index bdf9d376e4..14fec98baa 100644 --- a/dimos/robot/unitree_webrtc/nav_bot.py +++ b/dimos/robot/nav_bot.py @@ -19,13 +19,14 @@ """ import logging +import time 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.std_msgs import Bool, Header from dimos.msgs.tf2_msgs.TFMessage import TFMessage from dimos.protocol.tf import TF from dimos.robot.ros_bridge import ROSBridge, BridgeDirection @@ -37,6 +38,7 @@ 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 logger = setup_logger("dimos.robot.unitree_webrtc.nav_bot", level=logging.INFO) @@ -119,6 +121,15 @@ def __init__(self, dimos=None, sensor_to_base_link_transform=[0.0, 0.0, 0.0, 0.0 self.ros_bridge = None self.topic_remap_module = None self.tf = TF() + self.lcm = LCM() + + def start(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 deploy_navigation_modules(self, bridge_name="nav_bot_ros_bridge"): # Deploy topic remap module @@ -150,30 +161,77 @@ def deploy_navigation_modules(self, bridge_name="nav_bot_ros_bridge"): 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 + "/goal_pose", PoseStamped, ROSPoseStamped, direction=BridgeDirection.DIMOS_TO_ROS ) self.ros_bridge.add_topic( - "/cancel_goal", Bool, ROSBool, direction=BridgeDirection.ROS_TO_DIMOS + "/cancel_goal", Bool, ROSBool, direction=BridgeDirection.DIMOS_TO_ROS ) self.ros_bridge.add_topic( - "/goal_reached", Bool, ROSBool, direction=BridgeDirection.DIMOS_TO_ROS + "/goal_reached", Bool, ROSBool, direction=BridgeDirection.ROS_TO_DIMOS ) - def start_navigation_modules(self): - if self.topic_remap_module: - self.topic_remap_module.start() - logger.info("Topic remap module started") + def navigate_to_goal(self, pose: PoseStamped, blocking: bool = True, timeout: float = 30.0): + """Navigate to a target pose using ROS topics. - if self.ros_bridge: - logger.info("ROS bridge started") + 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 + 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 - def shutdown_navigation(self): + def shutdown(self): logger.info("Shutting down navigation modules...") if self.ros_bridge is not None: diff --git a/dimos/robot/unitree_webrtc/unitree_g1.py b/dimos/robot/unitree_webrtc/unitree_g1.py index f1c1fda98d..ffbde93fbf 100644 --- a/dimos/robot/unitree_webrtc/unitree_g1.py +++ b/dimos/robot/unitree_webrtc/unitree_g1.py @@ -25,16 +25,15 @@ from dimos import core 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.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 @@ -127,8 +126,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 @@ -166,6 +163,7 @@ def start(self): self.deploy_navigation_modules(bridge_name="g1_ros_bridge") self._start_modules() + NavBot.start(self) self.lcm.start() @@ -245,8 +243,6 @@ def _start_modules(self): if self.joystick: self.joystick.start() - self.start_navigation_modules() - # Initialize skills after connection is established if self.skill_library is not None: for skill in self.skill_library: @@ -270,7 +266,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: @@ -312,6 +309,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() + try: if args.joystick: print("\n" + "=" * 50) From 66e91f3221f331c7870d7e2aa1d28e399db3cfe9 Mon Sep 17 00:00:00 2001 From: alexlin2 Date: Mon, 22 Sep 2025 19:17:49 -0400 Subject: [PATCH 04/12] 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 c676f95129eb760274e84d4460278e875462e196 Mon Sep 17 00:00:00 2001 From: alexlin2 Date: Mon, 22 Sep 2025 19:28:53 -0400 Subject: [PATCH 05/12] added map to world static --- dimos/robot/nav_bot.py | 14 +++++++++++-- dimos/robot/unitree_webrtc/unitree_g1.py | 25 ++++++++++++------------ 2 files changed, 25 insertions(+), 14 deletions(-) diff --git a/dimos/robot/nav_bot.py b/dimos/robot/nav_bot.py index 14fec98baa..29e053533f 100644 --- a/dimos/robot/nav_bot.py +++ b/dimos/robot/nav_bot.py @@ -88,14 +88,24 @@ def _publish_odom_pose(self, msg: Odometry): ) rotation = euler_to_quaternion(euler_angles) - static_tf = Transform( + sensor_to_base_link_tf = Transform( translation=translation, rotation=rotation, frame_id="sensor", child_frame_id="base_link", ts=msg.ts, ) - self.tf.publish(static_tf) + + # 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: diff --git a/dimos/robot/unitree_webrtc/unitree_g1.py b/dimos/robot/unitree_webrtc/unitree_g1.py index ffbde93fbf..2d35cf90f0 100644 --- a/dimos/robot/unitree_webrtc/unitree_g1.py +++ b/dimos/robot/unitree_webrtc/unitree_g1.py @@ -309,18 +309,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 4d383861c754756e7311e427a3665c8315f63068 Mon Sep 17 00:00:00 2001 From: alexlin2 Date: Mon, 22 Sep 2025 23:16:50 -0400 Subject: [PATCH 06/12] To be dropped, temp nav module for launch --- dimos/robot/nav_bot.py | 79 ++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 79 insertions(+) diff --git a/dimos/robot/nav_bot.py b/dimos/robot/nav_bot.py index 29e053533f..f2db39c3fe 100644 --- a/dimos/robot/nav_bot.py +++ b/dimos/robot/nav_bot.py @@ -42,6 +42,85 @@ 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 + + def __init__(self, *args, **kwargs): + """Initialize NavigationModule.""" + Module.__init__(self, *args, **kwargs) + self.goal_reach = None + + @rpc + def start(self): + """Start the navigation module.""" + if self.goal_reached: + self.goal_reached.subscribe(self._on_goal_reached) + logger.info("NavigationModule started") + + def _on_goal_reached(self, msg: Bool): + """Handle goal reached status messages.""" + self.goal_reach = msg.data + + @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.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() + + logger.warning(f"Navigation timed out after {timeout} seconds") + return False + + @rpc + def stop(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.""" From b70940177b3c66762b89120cd43df869faceee43 Mon Sep 17 00:00:00 2001 From: alexlin2 Date: Tue, 23 Sep 2025 20:16:45 -0400 Subject: [PATCH 07/12] 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 1bf68c226c5d9ac10029bf4c9e89e9a0bc22ecd7 Mon Sep 17 00:00:00 2001 From: alexlin2 Date: Wed, 24 Sep 2025 20:33:07 -0400 Subject: [PATCH 08/12] added joy message type to dimos --- dimos/msgs/sensor_msgs/Joy.py | 181 +++++++++++++++++++++ dimos/msgs/sensor_msgs/__init__.py | 1 + dimos/msgs/sensor_msgs/test_Joy.py | 243 +++++++++++++++++++++++++++++ 3 files changed, 425 insertions(+) create mode 100644 dimos/msgs/sensor_msgs/Joy.py create mode 100644 dimos/msgs/sensor_msgs/test_Joy.py diff --git a/dimos/msgs/sensor_msgs/Joy.py b/dimos/msgs/sensor_msgs/Joy.py new file mode 100644 index 0000000000..e528b304b6 --- /dev/null +++ b/dimos/msgs/sensor_msgs/Joy.py @@ -0,0 +1,181 @@ +# 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 __future__ import annotations + +import time +from typing import List, TypeAlias + +from dimos_lcm.sensor_msgs import Joy as LCMJoy + +try: + from sensor_msgs.msg import Joy as ROSJoy +except ImportError: + ROSJoy = None + +from plum import dispatch + +from dimos.types.timestamped import Timestamped + +# Types that can be converted to/from Joy +JoyConvertable: TypeAlias = ( + tuple[List[float], List[int]] | dict[str, List[float] | List[int]] | LCMJoy +) + + +def sec_nsec(ts): + s = int(ts) + return [s, int((ts - s) * 1_000_000_000)] + + +class Joy(Timestamped): + msg_name = "sensor_msgs.Joy" + ts: float + frame_id: str + axes: List[float] + buttons: List[int] + + @dispatch + def __init__( + self, + ts: float = 0.0, + frame_id: str = "", + axes: List[float] | None = None, + buttons: List[int] | None = None, + ) -> None: + """Initialize a Joy message. + + Args: + ts: Timestamp in seconds + frame_id: Frame ID for the message + axes: List of axis values (typically -1.0 to 1.0) + buttons: List of button states (0 or 1) + """ + self.ts = ts if ts != 0 else time.time() + self.frame_id = frame_id + self.axes = axes if axes is not None else [] + self.buttons = buttons if buttons is not None else [] + + @dispatch + def __init__(self, joy_tuple: tuple[List[float], List[int]]) -> None: + """Initialize from a tuple of (axes, buttons).""" + self.ts = time.time() + self.frame_id = "" + self.axes = list(joy_tuple[0]) + self.buttons = list(joy_tuple[1]) + + @dispatch + def __init__(self, joy_dict: dict[str, List[float] | List[int]]) -> None: + """Initialize from a dictionary with 'axes' and 'buttons' keys.""" + self.ts = joy_dict.get("ts", time.time()) + self.frame_id = joy_dict.get("frame_id", "") + self.axes = list(joy_dict.get("axes", [])) + self.buttons = list(joy_dict.get("buttons", [])) + + @dispatch + def __init__(self, joy: Joy) -> None: + """Initialize from another Joy (copy constructor).""" + self.ts = joy.ts + self.frame_id = joy.frame_id + self.axes = list(joy.axes) + self.buttons = list(joy.buttons) + + @dispatch + def __init__(self, lcm_joy: LCMJoy) -> None: + """Initialize from an LCM Joy message.""" + self.ts = lcm_joy.header.stamp.sec + (lcm_joy.header.stamp.nsec / 1_000_000_000) + self.frame_id = lcm_joy.header.frame_id + self.axes = list(lcm_joy.axes) + self.buttons = list(lcm_joy.buttons) + + def lcm_encode(self) -> bytes: + lcm_msg = LCMJoy() + [lcm_msg.header.stamp.sec, lcm_msg.header.stamp.nsec] = sec_nsec(self.ts) + lcm_msg.header.frame_id = self.frame_id + lcm_msg.axes_length = len(self.axes) + lcm_msg.axes = self.axes + lcm_msg.buttons_length = len(self.buttons) + lcm_msg.buttons = self.buttons + return lcm_msg.lcm_encode() + + @classmethod + def lcm_decode(cls, data: bytes) -> Joy: + lcm_msg = LCMJoy.lcm_decode(data) + return cls( + ts=lcm_msg.header.stamp.sec + (lcm_msg.header.stamp.nsec / 1_000_000_000), + frame_id=lcm_msg.header.frame_id, + axes=list(lcm_msg.axes) if lcm_msg.axes else [], + buttons=list(lcm_msg.buttons) if lcm_msg.buttons else [], + ) + + def __str__(self) -> str: + return ( + f"Joy(axes={len(self.axes)} values, buttons={len(self.buttons)} values, " + f"frame_id='{self.frame_id}')" + ) + + def __repr__(self) -> str: + return ( + f"Joy(ts={self.ts}, frame_id='{self.frame_id}', " + f"axes={self.axes}, buttons={self.buttons})" + ) + + def __eq__(self, other) -> bool: + """Check if two Joy messages are equal.""" + if not isinstance(other, Joy): + return False + return ( + self.axes == other.axes + and self.buttons == other.buttons + and self.frame_id == other.frame_id + ) + + @classmethod + def from_ros_msg(cls, ros_msg: ROSJoy) -> "Joy": + """Create a Joy from a ROS sensor_msgs/Joy message. + + Args: + ros_msg: ROS Joy message + + Returns: + Joy instance + """ + # Convert timestamp from ROS header + ts = ros_msg.header.stamp.sec + (ros_msg.header.stamp.nanosec / 1_000_000_000) + + return cls( + ts=ts, + frame_id=ros_msg.header.frame_id, + axes=list(ros_msg.axes), + buttons=list(ros_msg.buttons), + ) + + def to_ros_msg(self) -> ROSJoy: + """Convert to a ROS sensor_msgs/Joy message. + + Returns: + ROS Joy message + """ + ros_msg = ROSJoy() + + # Set header + ros_msg.header.frame_id = self.frame_id + ros_msg.header.stamp.sec = int(self.ts) + ros_msg.header.stamp.nanosec = int((self.ts - int(self.ts)) * 1_000_000_000) + + # Set axes and buttons + ros_msg.axes = self.axes + ros_msg.buttons = self.buttons + + return ros_msg diff --git a/dimos/msgs/sensor_msgs/__init__.py b/dimos/msgs/sensor_msgs/__init__.py index a7afafe2f2..9a8a7b54fe 100644 --- a/dimos/msgs/sensor_msgs/__init__.py +++ b/dimos/msgs/sensor_msgs/__init__.py @@ -1,3 +1,4 @@ from dimos.msgs.sensor_msgs.Image import Image, ImageFormat from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo +from dimos.msgs.sensor_msgs.Joy import Joy diff --git a/dimos/msgs/sensor_msgs/test_Joy.py b/dimos/msgs/sensor_msgs/test_Joy.py new file mode 100644 index 0000000000..fd11624b08 --- /dev/null +++ b/dimos/msgs/sensor_msgs/test_Joy.py @@ -0,0 +1,243 @@ +#!/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. + +import pytest +import time + +try: + from sensor_msgs.msg import Joy as ROSJoy + from std_msgs.msg import Header as ROSHeader + + ROS_AVAILABLE = True +except ImportError: + ROSJoy = None + ROSHeader = None + ROS_AVAILABLE = False + +from dimos.msgs.sensor_msgs.Joy import Joy + + +def test_lcm_encode_decode(): + """Test LCM encode/decode preserves Joy data.""" + print("Testing Joy LCM encode/decode...") + + # Create test joy message with sample gamepad data + original = Joy( + ts=1234567890.123456789, + frame_id="gamepad", + axes=[0.5, -0.25, 1.0, -1.0, 0.0, 0.75], # 6 axes (e.g., left/right sticks + triggers) + buttons=[1, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0], # 12 buttons + ) + + # Encode to LCM bytes + encoded = original.lcm_encode() + assert isinstance(encoded, bytes) + assert len(encoded) > 0 + + # Decode back + decoded = Joy.lcm_decode(encoded) + + # Verify all fields match + assert abs(decoded.ts - original.ts) < 1e-9 + assert decoded.frame_id == original.frame_id + assert decoded.axes == original.axes + assert decoded.buttons == original.buttons + + print("✓ Joy LCM encode/decode test passed") + + +def test_initialization_methods(): + """Test various initialization methods for Joy.""" + print("Testing Joy initialization methods...") + + # Test default initialization + joy1 = Joy() + assert joy1.axes == [] + assert joy1.buttons == [] + assert joy1.frame_id == "" + assert joy1.ts > 0 # Should have current time + + # Test full initialization + joy2 = Joy(ts=1234567890.0, frame_id="xbox_controller", axes=[0.1, 0.2, 0.3], buttons=[1, 0, 1]) + assert joy2.ts == 1234567890.0 + assert joy2.frame_id == "xbox_controller" + assert joy2.axes == [0.1, 0.2, 0.3] + assert joy2.buttons == [1, 0, 1] + + # Test tuple initialization + joy3 = Joy(([0.5, -0.5], [1, 1, 0])) + assert joy3.axes == [0.5, -0.5] + assert joy3.buttons == [1, 1, 0] + + # Test dict initialization + joy4 = Joy({"axes": [0.7, 0.8], "buttons": [0, 1], "frame_id": "ps4_controller"}) + assert joy4.axes == [0.7, 0.8] + assert joy4.buttons == [0, 1] + assert joy4.frame_id == "ps4_controller" + + # Test copy constructor + joy5 = Joy(joy2) + assert joy5.ts == joy2.ts + assert joy5.frame_id == joy2.frame_id + assert joy5.axes == joy2.axes + assert joy5.buttons == joy2.buttons + assert joy5 is not joy2 # Different objects + + print("✓ Joy initialization methods test passed") + + +def test_equality(): + """Test Joy equality comparison.""" + print("Testing Joy equality...") + + joy1 = Joy(ts=1000.0, frame_id="controller1", axes=[0.5, -0.5], buttons=[1, 0, 1]) + + joy2 = Joy(ts=1000.0, frame_id="controller1", axes=[0.5, -0.5], buttons=[1, 0, 1]) + + joy3 = Joy( + ts=1000.0, + frame_id="controller2", # Different frame_id + axes=[0.5, -0.5], + buttons=[1, 0, 1], + ) + + joy4 = Joy( + ts=1000.0, + frame_id="controller1", + axes=[0.6, -0.5], # Different axes + buttons=[1, 0, 1], + ) + + # Same content should be equal + assert joy1 == joy2 + + # Different frame_id should not be equal + assert joy1 != joy3 + + # Different axes should not be equal + assert joy1 != joy4 + + # Different type should not be equal + assert joy1 != "not a joy" + assert joy1 != 42 + + print("✓ Joy equality test passed") + + +def test_string_representation(): + """Test Joy string representations.""" + print("Testing Joy string representations...") + + joy = Joy( + ts=1234567890.123, + frame_id="test_controller", + axes=[0.1, -0.2, 0.3, 0.4], + buttons=[1, 0, 1, 0, 0, 1], + ) + + # Test __str__ + str_repr = str(joy) + assert "Joy" in str_repr + assert "axes=4 values" in str_repr + assert "buttons=6 values" in str_repr + assert "test_controller" in str_repr + + # Test __repr__ + repr_str = repr(joy) + assert "Joy" in repr_str + assert "1234567890.123" in repr_str + assert "test_controller" in repr_str + assert "[0.1, -0.2, 0.3, 0.4]" in repr_str + assert "[1, 0, 1, 0, 0, 1]" in repr_str + + print("✓ Joy string representation test passed") + + +@pytest.mark.skipif(not ROS_AVAILABLE, reason="ROS not available") +def test_ros_conversion(): + """Test conversion to/from ROS Joy messages.""" + print("Testing Joy ROS conversion...") + + # Create a ROS Joy message + ros_msg = ROSJoy() + ros_msg.header = ROSHeader() + ros_msg.header.stamp.sec = 1234567890 + ros_msg.header.stamp.nanosec = 123456789 + ros_msg.header.frame_id = "ros_gamepad" + ros_msg.axes = [0.25, -0.75, 0.0, 1.0, -1.0] + ros_msg.buttons = [1, 1, 0, 0, 1, 0, 1, 0] + + # Convert from ROS + joy = Joy.from_ros_msg(ros_msg) + assert abs(joy.ts - 1234567890.123456789) < 1e-9 + assert joy.frame_id == "ros_gamepad" + assert joy.axes == [0.25, -0.75, 0.0, 1.0, -1.0] + assert joy.buttons == [1, 1, 0, 0, 1, 0, 1, 0] + + # Convert back to ROS + ros_msg2 = joy.to_ros_msg() + assert ros_msg2.header.frame_id == "ros_gamepad" + assert ros_msg2.header.stamp.sec == 1234567890 + assert abs(ros_msg2.header.stamp.nanosec - 123456789) < 100 # Allow small rounding + assert list(ros_msg2.axes) == [0.25, -0.75, 0.0, 1.0, -1.0] + assert list(ros_msg2.buttons) == [1, 1, 0, 0, 1, 0, 1, 0] + + print("✓ Joy ROS conversion test passed") + + +def test_edge_cases(): + """Test Joy with edge cases.""" + print("Testing Joy edge cases...") + + # Empty axes and buttons + joy1 = Joy(axes=[], buttons=[]) + assert joy1.axes == [] + assert joy1.buttons == [] + encoded = joy1.lcm_encode() + decoded = Joy.lcm_decode(encoded) + assert decoded.axes == [] + assert decoded.buttons == [] + + # Large number of axes and buttons + many_axes = [float(i) / 100.0 for i in range(20)] + many_buttons = [i % 2 for i in range(32)] + joy2 = Joy(axes=many_axes, buttons=many_buttons) + assert len(joy2.axes) == 20 + assert len(joy2.buttons) == 32 + encoded = joy2.lcm_encode() + decoded = Joy.lcm_decode(encoded) + # Check axes with floating point tolerance + assert len(decoded.axes) == len(many_axes) + for i, (a, b) in enumerate(zip(decoded.axes, many_axes)): + assert abs(a - b) < 1e-6, f"Axis {i}: {a} != {b}" + assert decoded.buttons == many_buttons + + # Extreme axis values + extreme_axes = [-1.0, 1.0, 0.0, -0.999999, 0.999999] + joy3 = Joy(axes=extreme_axes) + 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! ✓") From 1f5c6354076d565c598e62ab17e8d323fe3ba4d6 Mon Sep 17 00:00:00 2001 From: alexlin2 Date: Wed, 24 Sep 2025 20:34:00 -0400 Subject: [PATCH 09/12] added set autonomy mode to nav module --- dimos/robot/nav_bot.py | 81 +++++++++++++++++++++++++++++++++++++++--- 1 file changed, 77 insertions(+), 4 deletions(-) diff --git a/dimos/robot/nav_bot.py b/dimos/robot/nav_bot.py index f2db39c3fe..d2964b1259 100644 --- a/dimos/robot/nav_bot.py +++ b/dimos/robot/nav_bot.py @@ -25,8 +25,8 @@ 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.std_msgs import Bool, Header +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.protocol.tf import TF from dimos.robot.ros_bridge import ROSBridge, BridgeDirection @@ -34,7 +34,7 @@ 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 sensor_msgs.msg import PointCloud2 as ROSPointCloud2, Joy as ROSJoy from std_msgs.msg import Bool as ROSBool from tf2_msgs.msg import TFMessage as ROSTFMessage from dimos.utils.logging_config import setup_logger @@ -56,6 +56,7 @@ class NavigationModule(Module): goal_pose: Out[PoseStamped] = None cancel_goal: Out[Bool] = None + joy: Out[Joy] = None def __init__(self, *args, **kwargs): """Initialize NavigationModule.""" @@ -73,6 +74,42 @@ 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: """ @@ -91,6 +128,7 @@ def go_to(self, pose: PoseStamped, timeout: float = 60.0) -> bool: ) self.goal_reach = None + self._set_autonomy_mode() self.goal_pose.publish(pose) start_time = time.time() @@ -250,7 +288,7 @@ def deploy_navigation_modules(self, bridge_name="nav_bot_ros_bridge"): 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 @@ -262,6 +300,40 @@ def deploy_navigation_modules(self, bridge_name="nav_bot_ros_bridge"): "/goal_reached", Bool, ROSBool, 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. @@ -279,6 +351,7 @@ def navigate_to_goal(self, pose: PoseStamped, blocking: bool = True, timeout: fl ) # Publish goal to /goal_pose topic + self._set_autonomy_mode() goal_topic = Topic("/goal_pose", PoseStamped) self.lcm.publish(goal_topic, pose) From 0f5e1cdb6f92a4d773d099b7baea6e798547c8a6 Mon Sep 17 00:00:00 2001 From: Paul Nechifor Date: Wed, 22 Oct 2025 07:57:07 +0300 Subject: [PATCH 10/12] etst --- dimos/robot/unitree_webrtc/unitree_g1.py | 26 ++++++++++++------------ 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/dimos/robot/unitree_webrtc/unitree_g1.py b/dimos/robot/unitree_webrtc/unitree_g1.py index 2d35cf90f0..165d6ee49a 100644 --- a/dimos/robot/unitree_webrtc/unitree_g1.py +++ b/dimos/robot/unitree_webrtc/unitree_g1.py @@ -309,19 +309,19 @@ def main(): ) 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() + # 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 450662cfb25cba60e029b1c11716e8687344dba4 Mon Sep 17 00:00:00 2001 From: Paul Nechifor Date: Sat, 25 Oct 2025 04:32:29 +0300 Subject: [PATCH 11/12] dimos and ros nav stack in docker --- dimos/robot/unitree_webrtc/unitree_g1.py | 4 +- ros_docker_integration/.gitignore | 20 +++ ros_docker_integration/Dockerfile | 116 ++++++++++++ ros_docker_integration/README.md | 178 +++++++++++++++++++ ros_docker_integration/build.sh | 70 ++++++++ ros_docker_integration/docker-compose.yml | 66 +++++++ ros_docker_integration/ros_launch_wrapper.py | 132 ++++++++++++++ ros_docker_integration/run_both.sh | 109 ++++++++++++ ros_docker_integration/run_command.sh | 46 +++++ ros_docker_integration/shell.sh | 42 +++++ ros_docker_integration/start.sh | 88 +++++++++ ros_docker_integration/start_clean.sh | 90 ++++++++++ ros_docker_integration/test_integration.sh | 121 +++++++++++++ 13 files changed, 1080 insertions(+), 2 deletions(-) create mode 100644 ros_docker_integration/.gitignore create mode 100644 ros_docker_integration/Dockerfile create mode 100644 ros_docker_integration/README.md create mode 100755 ros_docker_integration/build.sh create mode 100644 ros_docker_integration/docker-compose.yml create mode 100755 ros_docker_integration/ros_launch_wrapper.py create mode 100755 ros_docker_integration/run_both.sh create mode 100755 ros_docker_integration/run_command.sh create mode 100755 ros_docker_integration/shell.sh create mode 100755 ros_docker_integration/start.sh create mode 100755 ros_docker_integration/start_clean.sh create mode 100755 ros_docker_integration/test_integration.sh diff --git a/dimos/robot/unitree_webrtc/unitree_g1.py b/dimos/robot/unitree_webrtc/unitree_g1.py index 165d6ee49a..8c7548e7dc 100644 --- a/dimos/robot/unitree_webrtc/unitree_g1.py +++ b/dimos/robot/unitree_webrtc/unitree_g1.py @@ -313,11 +313,11 @@ def main(): pose = PoseStamped( ts=time.time(), frame_id="map", - position=Vector3(1.0, 1.0, 0.0), + position=Vector3(0.2, 0.2, 0.0), orientation=Quaternion(0.0, 0.0, 0.0, 1.0), ) - time.sleep(2) + time.sleep(20) robot.navigate_to_goal(pose, blocking=False) time.sleep(5) diff --git a/ros_docker_integration/.gitignore b/ros_docker_integration/.gitignore new file mode 100644 index 0000000000..4933cec4ed --- /dev/null +++ b/ros_docker_integration/.gitignore @@ -0,0 +1,20 @@ +# Cloned repository +autonomy_stack_mecanum_wheel_platform/ + +# Unity models (large binary files) +unity_models/ + +# ROS bag files +bagfiles/ + +# Config files (may contain local settings) +config/ + +# Docker volumes +.docker/ + +# Temporary files +*.tmp +*.log +*.swp +*~ \ No newline at end of file diff --git a/ros_docker_integration/Dockerfile b/ros_docker_integration/Dockerfile new file mode 100644 index 0000000000..0502ba6e11 --- /dev/null +++ b/ros_docker_integration/Dockerfile @@ -0,0 +1,116 @@ +# Base image with ROS Jazzy desktop full +FROM osrf/ros:jazzy-desktop-full + +# Set environment variables +ENV DEBIAN_FRONTEND=noninteractive +ENV ROS_DISTRO=jazzy +ENV WORKSPACE=/ros2_ws +ENV DIMOS_PATH=/home/p/pro/dimensional/dimos + +# Install system dependencies +RUN apt-get update && apt-get install -y \ + # ROS packages + ros-jazzy-pcl-ros \ + # Development tools + git \ + cmake \ + build-essential \ + python3-colcon-common-extensions \ + # PCL and system libraries + libpcl-dev \ + libgoogle-glog-dev \ + libgflags-dev \ + libatlas-base-dev \ + libeigen3-dev \ + libsuitesparse-dev \ + # X11 and GUI support for RVIZ + x11-apps \ + xorg \ + openbox \ + # Networking tools + iputils-ping \ + net-tools \ + iproute2 \ + # Editor (optional but useful) + nano \ + vim \ + # Python tools + python3-pip \ + python3-setuptools \ + python3-venv \ + # Additional dependencies for dimos + ffmpeg \ + portaudio19-dev \ + libsndfile1 \ + # For OpenCV + libgl1 \ + libglib2.0-0 \ + # For Open3D + libgomp1 \ + # Clean up + && rm -rf /var/lib/apt/lists/* + +# Create workspace directory +RUN mkdir -p ${WORKSPACE}/src + +# Copy the autonomy stack repository (should be cloned by build.sh) +COPY ros_docker_integration/autonomy_stack_mecanum_wheel_platform ${WORKSPACE}/src/autonomy_stack_mecanum_wheel_platform + +# Set working directory +WORKDIR ${WORKSPACE} + +# Set up ROS environment +RUN echo "source /opt/ros/${ROS_DISTRO}/setup.bash" >> ~/.bashrc + +# Build the autonomy stack workspace (simulation mode - skipping SLAM and Mid-360 driver) +RUN /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash && \ + cd ${WORKSPACE} && \ + colcon build --symlink-install \ + --cmake-args -DCMAKE_BUILD_TYPE=Release \ + --packages-skip arise_slam_mid360 arise_slam_mid360_msgs livox_ros_driver2" + +# Source the workspace setup +RUN echo "source ${WORKSPACE}/install/setup.bash" >> ~/.bashrc + +# Create directory for Unity environment models +RUN mkdir -p ${WORKSPACE}/src/autonomy_stack_mecanum_wheel_platform/src/base_autonomy/vehicle_simulator/mesh/unity + +# Copy the dimos repository +RUN mkdir -p ${DIMOS_PATH} +COPY . ${DIMOS_PATH}/ + +# Install Python dependencies for dimos +WORKDIR ${DIMOS_PATH} +RUN python3 -m venv .venv && \ + /bin/bash -c "source .venv/bin/activate && \ + pip install --upgrade pip setuptools wheel && \ + pip install -e .[cpu,dev] 'mmengine>=0.10.3' 'mmcv>=2.1.0'" + +# Copy helper scripts +COPY ros_docker_integration/run_both.sh /usr/local/bin/run_both.sh +COPY ros_docker_integration/ros_launch_wrapper.py /usr/local/bin/ros_launch_wrapper.py +RUN chmod +x /usr/local/bin/run_both.sh /usr/local/bin/ros_launch_wrapper.py + +# Set up entrypoint script +RUN echo '#!/bin/bash\n\ +set -e\n\ +\n\ +# Source ROS setup\n\ +source /opt/ros/${ROS_DISTRO}/setup.bash\n\ +source ${WORKSPACE}/install/setup.bash\n\ +\n\ +# Activate Python virtual environment for dimos\n\ +source ${DIMOS_PATH}/.venv/bin/activate\n\ +\n\ +# Export ROBOT_CONFIG_PATH for autonomy stack\n\ +export ROBOT_CONFIG_PATH="unitree/unitree_g1"\n\ +\n\ +# Execute the command\n\ +exec "$@"' > /ros_entrypoint.sh && \ + chmod +x /ros_entrypoint.sh + +# Set the entrypoint +ENTRYPOINT ["/ros_entrypoint.sh"] + +# Default command +CMD ["bash"] \ No newline at end of file diff --git a/ros_docker_integration/README.md b/ros_docker_integration/README.md new file mode 100644 index 0000000000..b976057e6d --- /dev/null +++ b/ros_docker_integration/README.md @@ -0,0 +1,178 @@ +# ROS Docker Integration for DimOS + +This directory contains Docker configuration files to run DimOS and the ROS autonomy stack in the same container, enabling communication between the two systems. + +## Prerequisites + +- Docker with `docker compose` support +- NVIDIA GPU with drivers installed +- NVIDIA Container Toolkit (nvidia-docker2) +- X11 server for GUI applications (RVIZ, Unity simulator) + +## Quick Start + +1. **Build the Docker image:** + ```bash + ./build.sh + ``` + This will: + - Clone the autonomy_stack_mecanum_wheel_platform repository (jazzy branch) + - Build a Docker image with both ROS and DimOS dependencies + - Set up the environment for both systems + +2. **Run the container:** + ```bash + # Interactive bash shell (default) + ./start.sh + + # Start with ROS route planner + ./start.sh --ros-planner + + # Start with DimOS Unitree G1 controller + ./start.sh --dimos + + # Start both systems (basic) + ./start.sh --all + + # Start both systems with improved shutdown handling (recommended) + ./start_clean.sh --all + ``` + +## Directory Structure + +``` +ros_docker_integration/ +├── Dockerfile # Combined Dockerfile for ROS + DimOS +├── docker-compose.yml # Docker Compose configuration +├── build.sh # Script to clone repos and build image +├── start.sh # Script to run the container (basic) +├── start_clean.sh # Script with improved shutdown handling +├── run_both.sh # Bash helper to run both ROS and DimOS +├── ros_launch_wrapper.py # Python wrapper for clean ROS shutdown +├── run_command.sh # Helper script for running custom commands +├── shell.sh # Quick access to interactive shell +├── test_integration.sh # Integration test script +├── README.md # This file +├── autonomy_stack_mecanum_wheel_platform/ # (Created by build.sh) +├── unity_models/ # (Optional) Unity environment models +├── bagfiles/ # (Optional) ROS bag files +└── config/ # (Optional) Configuration files +``` + +## Unity Models (Optional) + +For the Unity simulator to work properly, download the Unity environment models from: +https://drive.google.com/drive/folders/1G1JYkccvoSlxyySuTlPfvmrWoJUO8oSs + +Extract them to: `ros_docker_integration/unity_models/` + +## Manual Commands + +Once inside the container, you can manually run: + +### ROS Autonomy Stack +```bash +cd /ros2_ws/src/autonomy_stack_mecanum_wheel_platform +./system_simulation_with_route_planner.sh +``` + +### DimOS +```bash +# Activate virtual environment +source /home/p/pro/dimensional/dimos/.venv/bin/activate + +# Run Unitree G1 controller +python /home/p/pro/dimensional/dimos/dimos/robot/unitree_webrtc/unitree_g1.py + +# Or run other DimOS scripts +python /home/p/pro/dimensional/dimos/dimos/your_script.py +``` + +### ROS Commands +```bash +# List ROS topics +ros2 topic list + +# Send navigation goal +ros2 topic pub /way_point geometry_msgs/msg/PointStamped "{ + header: {frame_id: 'map'}, + point: {x: 5.0, y: 3.0, z: 0.0} +}" --once + +# Monitor robot state +ros2 topic echo /state_estimation +``` + +## Custom Commands + +Use the `run_command.sh` helper script to run custom commands: +```bash +./run_command.sh "ros2 topic list" +./run_command.sh "python /path/to/your/script.py" +``` + +## Development + +The docker-compose.yml mounts the following directories for live development: +- DimOS source: `../dimos` → `/home/p/pro/dimensional/dimos/dimos` +- Autonomy stack source: `./autonomy_stack_mecanum_wheel_platform/src` → `/ros2_ws/src/autonomy_stack_mecanum_wheel_platform/src` + +Changes to these files will be reflected in the container without rebuilding. + +## Environment Variables + +The container sets: +- `ROS_DISTRO=jazzy` +- `ROBOT_CONFIG_PATH=unitree/unitree_g1` +- `ROS_DOMAIN_ID=0` +- GPU and display variables for GUI support + +## Shutdown Handling + +The integration provides two methods for running both systems together: + +### Basic Method (`./start.sh --all`) +Uses the bash script `run_both.sh` with signal trapping and process group management. + +### Improved Method (`./start_clean.sh --all`) +Uses the Python wrapper `ros_launch_wrapper.py` which provides: +- Proper signal forwarding to ROS launch system +- Graceful shutdown with timeouts +- Automatic cleanup of orphaned ROS nodes +- Better handling of ROS2's complex process hierarchy + +**Recommended**: Use `./start_clean.sh --all` for the cleanest shutdown experience. + +## Troubleshooting + +### ROS Nodes Not Shutting Down Cleanly +If you experience issues with ROS nodes hanging during shutdown: +1. Use `./start_clean.sh --all` instead of `./start.sh --all` +2. The improved handler will automatically clean up remaining processes +3. If issues persist, you can manually clean up with: + ```bash + docker compose -f ros_docker_integration/docker-compose.yml down + ``` + +### X11 Display Issues +If you get display errors: +```bash +xhost +local:docker +``` + +### GPU Not Available +Ensure NVIDIA Container Toolkit is installed: +```bash +sudo apt-get install nvidia-container-toolkit +sudo systemctl restart docker +``` + +### Permission Issues +The container runs with `--privileged` and `--network=host` for hardware access. + +## Notes + +- The container uses `--network=host` for ROS communication +- GPU passthrough is enabled via `runtime: nvidia` +- X11 forwarding is configured for GUI applications +- The ROS workspace is built without SLAM and Mid-360 packages (simulation mode) \ No newline at end of file diff --git a/ros_docker_integration/build.sh b/ros_docker_integration/build.sh new file mode 100755 index 0000000000..603a841324 --- /dev/null +++ b/ros_docker_integration/build.sh @@ -0,0 +1,70 @@ +#!/bin/bash + +set -e + +# Colors for output +RED='\033[0;31m' +GREEN='\033[0;32m' +YELLOW='\033[1;33m' +NC='\033[0m' # No Color + +echo -e "${GREEN}=========================================${NC}" +echo -e "${GREEN}Building DimOS + ROS Autonomy Stack Docker Image${NC}" +echo -e "${GREEN}=========================================${NC}" +echo "" + +# Get the directory where the script is located +SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" +cd "$SCRIPT_DIR" + +# Check if autonomy stack already exists +if [ ! -d "autonomy_stack_mecanum_wheel_platform" ]; then + echo -e "${YELLOW}Cloning autonomy_stack_mecanum_wheel_platform repository...${NC}" + git clone https://github.com/alexlin2/autonomy_stack_mecanum_wheel_platform.git + echo -e "${GREEN}Repository cloned successfully!${NC}" +fi + +# Check if Unity models directory exists (warn if not) +if [ ! -d "unity_models" ]; then + echo "" + echo -e "${YELLOW}WARNING: Unity models directory not found!${NC}" + echo "If you want to use the Unity simulator, please download Unity environment models" + echo "and extract them to: ${SCRIPT_DIR}/unity_models/" + echo "Download from: https://drive.google.com/drive/folders/1G1JYkccvoSlxyySuTlPfvmrWoJUO8oSs" + echo "" + read -p "Continue building without Unity models? (y/n) " -n 1 -r + echo + if [[ ! $REPLY =~ ^[Yy]$ ]]; then + exit 1 + fi +fi + +# Build the Docker image using docker compose +echo "" +echo -e "${YELLOW}Building Docker image with docker compose...${NC}" +echo "This may take a while as it needs to:" +echo " - Download base ROS Jazzy image" +echo " - Install ROS packages and dependencies" +echo " - Build the autonomy stack" +echo " - Install Python dependencies for DimOS" +echo "" + +# Go to the dimos directory (parent of ros_docker_integration) for the build context +cd .. + +# Build using docker compose +docker compose -f ros_docker_integration/docker-compose.yml build + +echo "" +echo -e "${GREEN}=========================================${NC}" +echo -e "${GREEN}Docker image built successfully!${NC}" +echo -e "${GREEN}=========================================${NC}" +echo "" +echo "You can now run the container using:" +echo -e "${YELLOW} ./start.sh${NC}" +echo "" +echo "Available options:" +echo " - Run with default configuration (starts bash): ./start.sh" +echo " - Run with ROS route planner: ./start.sh --ros-planner" +echo " - Run with DimOS only: ./start.sh --dimos" +echo " - Run with both: ./start.sh --all" diff --git a/ros_docker_integration/docker-compose.yml b/ros_docker_integration/docker-compose.yml new file mode 100644 index 0000000000..fc411c7400 --- /dev/null +++ b/ros_docker_integration/docker-compose.yml @@ -0,0 +1,66 @@ +services: + dimos_autonomy_stack: + build: + context: .. + dockerfile: ros_docker_integration/Dockerfile + image: dimos_autonomy_stack:jazzy + container_name: dimos_autonomy_container + + # Enable interactive terminal + stdin_open: true + tty: true + + # Network configuration - required for ROS communication + network_mode: host + + # Privileged mode for hardware access (required for real robot and some devices) + privileged: true + + # GPU support (NVIDIA runtime) + runtime: nvidia + + # Environment variables for display and ROS + environment: + - DISPLAY=${DISPLAY} + - QT_X11_NO_MITSHM=1 + - NVIDIA_VISIBLE_DEVICES=all + - NVIDIA_DRIVER_CAPABILITIES=all + - ROS_DOMAIN_ID=0 + - ROBOT_CONFIG_PATH=unitree/unitree_g1 + + # Volume mounts + volumes: + # X11 socket for GUI + - /tmp/.X11-unix:/tmp/.X11-unix:rw + - ${HOME}/.Xauthority:/root/.Xauthority:rw + + # Mount Unity environment models (if available) + - ./unity_models:/ros2_ws/src/autonomy_stack_mecanum_wheel_platform/src/base_autonomy/vehicle_simulator/mesh/unity:rw + + # Mount the autonomy stack source for development (optional - comment out if not needed) + - ./autonomy_stack_mecanum_wheel_platform/src:/ros2_ws/src/autonomy_stack_mecanum_wheel_platform/src:rw + + # Mount dimos source code for live development + - ../dimos:/home/p/pro/dimensional/dimos/dimos:rw + + # Mount bagfiles directory (optional) + - ./bagfiles:/ros2_ws/bagfiles:rw + + # Mount config files for easy editing + - ./config:/ros2_ws/config:rw + + # Device access (for joystick controllers, serial devices, and cameras) + devices: + - /dev/input:/dev/input + - /dev/dri:/dev/dri + # Uncomment if using real robot with motor controller + # - /dev/ttyACM0:/dev/ttyACM0 + # - /dev/ttyUSB0:/dev/ttyUSB0 + # Uncomment if using cameras + # - /dev/video0:/dev/video0 + + # Working directory + working_dir: /home/p/pro/dimensional/dimos + + # Command to run (can be overridden) + command: bash \ No newline at end of file diff --git a/ros_docker_integration/ros_launch_wrapper.py b/ros_docker_integration/ros_launch_wrapper.py new file mode 100755 index 0000000000..42f59fe606 --- /dev/null +++ b/ros_docker_integration/ros_launch_wrapper.py @@ -0,0 +1,132 @@ +#!/usr/bin/env python3 +""" +Wrapper script to properly handle ROS2 launch file shutdown. +This script ensures clean shutdown of all ROS nodes when receiving SIGINT. +""" + +import os +import sys +import signal +import subprocess +import time +import threading + +class ROSLaunchWrapper: + def __init__(self): + self.ros_process = None + self.dimos_process = None + self.shutdown_in_progress = False + + def signal_handler(self, signum, frame): + """Handle shutdown signals gracefully""" + if self.shutdown_in_progress: + return + + self.shutdown_in_progress = True + print("\n\nShutdown signal received. Stopping services gracefully...") + + # Stop DimOS first (it typically shuts down cleanly) + if self.dimos_process and self.dimos_process.poll() is None: + print("Stopping DimOS...") + self.dimos_process.terminate() + try: + self.dimos_process.wait(timeout=5) + print("DimOS stopped cleanly.") + except subprocess.TimeoutExpired: + print("Force stopping DimOS...") + self.dimos_process.kill() + self.dimos_process.wait() + + # Stop ROS - send SIGINT first for graceful shutdown + if self.ros_process and self.ros_process.poll() is None: + print("Stopping ROS nodes (this may take a moment)...") + + # Send SIGINT to trigger graceful ROS shutdown + self.ros_process.send_signal(signal.SIGINT) + + # Wait for graceful shutdown with timeout + try: + self.ros_process.wait(timeout=15) + print("ROS stopped cleanly.") + except subprocess.TimeoutExpired: + print("ROS is taking too long to stop. Sending SIGTERM...") + self.ros_process.terminate() + try: + self.ros_process.wait(timeout=5) + except subprocess.TimeoutExpired: + print("Force stopping ROS...") + self.ros_process.kill() + self.ros_process.wait() + + # Clean up any remaining processes + print("Cleaning up any remaining processes...") + cleanup_commands = [ + "pkill -f 'ros2' || true", + "pkill -f 'localPlanner' || true", + "pkill -f 'pathFollower' || true", + "pkill -f 'terrainAnalysis' || true", + "pkill -f 'sensorScanGeneration' || true", + "pkill -f 'vehicleSimulator' || true", + "pkill -f 'visualizationTools' || true", + "pkill -f 'far_planner' || true", + "pkill -f 'graph_decoder' || true", + ] + + for cmd in cleanup_commands: + subprocess.run(cmd, shell=True, stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL) + + print("All services stopped.") + sys.exit(0) + + def run(self): + """Main execution function""" + # Register signal handlers + signal.signal(signal.SIGINT, self.signal_handler) + signal.signal(signal.SIGTERM, self.signal_handler) + + print("Starting ROS route planner and DimOS...") + + # Change to the ROS workspace directory + os.chdir("/ros2_ws/src/autonomy_stack_mecanum_wheel_platform") + + # Start ROS route planner + print("Starting ROS route planner...") + self.ros_process = subprocess.Popen( + ["bash", "./system_simulation_with_route_planner.sh"], + preexec_fn=os.setsid # Create new process group + ) + + # Wait for ROS to initialize + print("Waiting for ROS to initialize...") + time.sleep(5) + + # Start DimOS + print("Starting DimOS Unitree G1 controller...") + self.dimos_process = subprocess.Popen( + [sys.executable, "/home/p/pro/dimensional/dimos/dimos/robot/unitree_webrtc/unitree_g1.py"] + ) + + print("Both systems are running. Press Ctrl+C to stop.") + print("") + + # Wait for processes + try: + # Monitor both processes + while True: + # Check if either process has died + if self.ros_process.poll() is not None: + print("ROS process has stopped unexpectedly.") + self.signal_handler(signal.SIGTERM, None) + break + if self.dimos_process.poll() is not None: + print("DimOS process has stopped.") + # DimOS stopping is less critical, but we should still clean up ROS + self.signal_handler(signal.SIGTERM, None) + break + time.sleep(1) + except KeyboardInterrupt: + pass # Signal handler will take care of cleanup + +if __name__ == "__main__": + wrapper = ROSLaunchWrapper() + wrapper.run() \ No newline at end of file diff --git a/ros_docker_integration/run_both.sh b/ros_docker_integration/run_both.sh new file mode 100755 index 0000000000..878022c28a --- /dev/null +++ b/ros_docker_integration/run_both.sh @@ -0,0 +1,109 @@ +#!/bin/bash +# Script to run both ROS route planner and DimOS together + +echo "Starting ROS route planner and DimOS..." + +# Variables for process IDs +ROS_PID="" +DIMOS_PID="" +SHUTDOWN_IN_PROGRESS=false + +# Function to handle cleanup +cleanup() { + if [ "$SHUTDOWN_IN_PROGRESS" = true ]; then + return + fi + SHUTDOWN_IN_PROGRESS=true + + echo "" + echo "Shutdown initiated. Stopping services..." + + # First, try to gracefully stop DimOS + if [ -n "$DIMOS_PID" ] && kill -0 $DIMOS_PID 2>/dev/null; then + echo "Stopping DimOS..." + kill -TERM $DIMOS_PID 2>/dev/null || true + + # Wait up to 5 seconds for DimOS to stop + for i in {1..10}; do + if ! kill -0 $DIMOS_PID 2>/dev/null; then + echo "DimOS stopped cleanly." + break + fi + sleep 0.5 + done + + # Force kill if still running + if kill -0 $DIMOS_PID 2>/dev/null; then + echo "Force stopping DimOS..." + kill -9 $DIMOS_PID 2>/dev/null || true + fi + fi + + # Then handle ROS - send SIGINT to the launch process group + if [ -n "$ROS_PID" ] && kill -0 $ROS_PID 2>/dev/null; then + echo "Stopping ROS nodes (this may take a moment)..." + + # Send SIGINT to the process group to properly trigger ROS shutdown + kill -INT -$ROS_PID 2>/dev/null || kill -INT $ROS_PID 2>/dev/null || true + + # Wait up to 15 seconds for graceful shutdown + for i in {1..30}; do + if ! kill -0 $ROS_PID 2>/dev/null; then + echo "ROS stopped cleanly." + break + fi + sleep 0.5 + done + + # If still running, send SIGTERM + if kill -0 $ROS_PID 2>/dev/null; then + echo "Sending SIGTERM to ROS..." + kill -TERM -$ROS_PID 2>/dev/null || kill -TERM $ROS_PID 2>/dev/null || true + sleep 2 + fi + + # Final resort: SIGKILL + if kill -0 $ROS_PID 2>/dev/null; then + echo "Force stopping ROS..." + kill -9 -$ROS_PID 2>/dev/null || kill -9 $ROS_PID 2>/dev/null || true + fi + fi + + # Clean up any remaining ROS2 processes + echo "Cleaning up any remaining processes..." + pkill -f "ros2" 2>/dev/null || true + pkill -f "localPlanner" 2>/dev/null || true + pkill -f "pathFollower" 2>/dev/null || true + pkill -f "terrainAnalysis" 2>/dev/null || true + pkill -f "sensorScanGeneration" 2>/dev/null || true + pkill -f "vehicleSimulator" 2>/dev/null || true + pkill -f "visualizationTools" 2>/dev/null || true + pkill -f "far_planner" 2>/dev/null || true + pkill -f "graph_decoder" 2>/dev/null || true + + echo "All services stopped." +} + +# Set up trap to call cleanup on exit +trap cleanup EXIT INT TERM + +# Start ROS route planner in background (in new process group) +echo "Starting ROS route planner..." +cd /ros2_ws/src/autonomy_stack_mecanum_wheel_platform +setsid bash -c './system_simulation_with_route_planner.sh' & +ROS_PID=$! + +# Wait a bit for ROS to initialize +echo "Waiting for ROS to initialize..." +sleep 5 + +# Start DimOS +echo "Starting DimOS Unitree G1 controller..." +python /home/p/pro/dimensional/dimos/dimos/robot/unitree_webrtc/unitree_g1.py & +DIMOS_PID=$! + +echo "Both systems are running. Press Ctrl+C to stop." +echo "" + +# Wait for both processes +wait $ROS_PID $DIMOS_PID 2>/dev/null || true \ No newline at end of file diff --git a/ros_docker_integration/run_command.sh b/ros_docker_integration/run_command.sh new file mode 100755 index 0000000000..36187ac30d --- /dev/null +++ b/ros_docker_integration/run_command.sh @@ -0,0 +1,46 @@ +#!/bin/bash + +set -e + +# Colors for output +RED='\033[0;31m' +GREEN='\033[0;32m' +YELLOW='\033[1;33m' +NC='\033[0m' # No Color + +# Get the directory where the script is located +SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" +cd "$SCRIPT_DIR" + +# Check if command was provided +if [ $# -eq 0 ]; then + echo -e "${RED}Error: No command provided${NC}" + echo "" + echo "Usage: $0 \"command to run\"" + echo "" + echo "Examples:" + echo " $0 \"ros2 topic list\"" + echo " $0 \"ros2 launch base_autonomy unity_simulation_bringup.launch.py\"" + echo " $0 \"python /home/p/pro/dimensional/dimos/dimos/robot/unitree_webrtc/unitree_g1.py\"" + echo " $0 \"bash\" # For interactive shell" + exit 1 +fi + +# Allow X server connection from Docker +xhost +local:docker 2>/dev/null || true + +echo -e "${GREEN}=========================================${NC}" +echo -e "${GREEN}Running command in DimOS + ROS Container${NC}" +echo -e "${GREEN}=========================================${NC}" +echo "" +echo -e "${YELLOW}Command: $@${NC}" +echo "" + +# Go to dimos directory (parent of ros_docker_integration) for docker compose context +cd .. + +# Run the command in the container +docker compose -f ros_docker_integration/docker-compose.yml run --rm dimos_autonomy_stack bash -c "$@" + +# Revoke X server access when done +xhost -local:docker 2>/dev/null || true \ No newline at end of file diff --git a/ros_docker_integration/shell.sh b/ros_docker_integration/shell.sh new file mode 100755 index 0000000000..70898400e8 --- /dev/null +++ b/ros_docker_integration/shell.sh @@ -0,0 +1,42 @@ +#!/bin/bash + +# Quick script to enter an interactive shell in the container + +set -e + +# Colors for output +GREEN='\033[0;32m' +YELLOW='\033[1;33m' +NC='\033[0m' # No Color + +# Get the directory where the script is located +SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" +cd "$SCRIPT_DIR" + +# Allow X server connection from Docker +xhost +local:docker 2>/dev/null || true + +echo -e "${GREEN}=========================================${NC}" +echo -e "${GREEN}Entering DimOS + ROS Container Shell${NC}" +echo -e "${GREEN}=========================================${NC}" +echo "" +echo -e "${YELLOW}Environment:${NC}" +echo " - ROS workspace: /ros2_ws" +echo " - DimOS path: /home/p/pro/dimensional/dimos" +echo " - Python venv: /home/p/pro/dimensional/dimos/.venv" +echo "" +echo -e "${YELLOW}Useful commands:${NC}" +echo " - ros2 topic list" +echo " - ros2 launch base_autonomy unity_simulation_bringup.launch.py" +echo " - source /home/p/pro/dimensional/dimos/.venv/bin/activate" +echo " - python /home/p/pro/dimensional/dimos/dimos/robot/unitree_webrtc/unitree_g1.py" +echo "" + +# Go to dimos directory (parent of ros_docker_integration) for docker compose context +cd .. + +# Enter interactive shell +docker compose -f ros_docker_integration/docker-compose.yml run --rm dimos_autonomy_stack bash + +# Revoke X server access when done +xhost -local:docker 2>/dev/null || true \ No newline at end of file diff --git a/ros_docker_integration/start.sh b/ros_docker_integration/start.sh new file mode 100755 index 0000000000..900729fbff --- /dev/null +++ b/ros_docker_integration/start.sh @@ -0,0 +1,88 @@ +#!/bin/bash + +set -e + +# Colors for output +RED='\033[0;31m' +GREEN='\033[0;32m' +YELLOW='\033[1;33m' +NC='\033[0m' # No Color + +# Get the directory where the script is located +SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" +cd "$SCRIPT_DIR" + +# Allow X server connection from Docker +xhost +local:docker 2>/dev/null || true + +echo -e "${GREEN}=========================================${NC}" +echo -e "${GREEN}Starting DimOS + ROS Autonomy Stack Container${NC}" +echo -e "${GREEN}=========================================${NC}" +echo "" + +# Check if Unity models exist (warn if not) +if [ ! -d "unity_models" ] && [[ "$*" == *"--ros-planner"* || "$*" == *"--all"* ]]; then + echo -e "${YELLOW}WARNING: Unity models directory not found!${NC}" + echo "The Unity simulator may not work properly." + echo "Download from: https://drive.google.com/drive/folders/1G1JYkccvoSlxyySuTlPfvmrWoJUO8oSs" + echo "" +fi + +# Parse command line arguments +MODE="default" +if [[ "$1" == "--ros-planner" ]]; then + MODE="ros-planner" +elif [[ "$1" == "--dimos" ]]; then + MODE="dimos" +elif [[ "$1" == "--all" ]]; then + MODE="all" +elif [[ "$1" == "--help" || "$1" == "-h" ]]; then + echo "Usage: $0 [OPTIONS]" + echo "" + echo "Options:" + echo " --ros-planner Start with ROS route planner" + echo " --dimos Start with DimOS Unitree G1 controller" + echo " --all Start both ROS planner and DimOS" + echo " --help Show this help message" + echo "" + echo "Without options, starts an interactive bash shell" + exit 0 +fi + +# Go to dimos directory (parent of ros_docker_integration) for docker compose context +cd .. + +# Set the command based on mode +case $MODE in + "ros-planner") + echo -e "${YELLOW}Starting with ROS route planner...${NC}" + CMD="bash -c 'cd /ros2_ws/src/autonomy_stack_mecanum_wheel_platform && ./system_simulation_with_route_planner.sh'" + ;; + "dimos") + echo -e "${YELLOW}Starting with DimOS Unitree G1 controller...${NC}" + CMD="python /home/p/pro/dimensional/dimos/dimos/robot/unitree_webrtc/unitree_g1.py" + ;; + "all") + echo -e "${YELLOW}Starting both ROS planner and DimOS...${NC}" + # Use the helper script to run both + CMD="/usr/local/bin/run_both.sh" + ;; + "default") + echo -e "${YELLOW}Starting interactive bash shell...${NC}" + echo "" + echo "You can manually run:" + echo " ROS planner: cd /ros2_ws/src/autonomy_stack_mecanum_wheel_platform && ./system_simulation_with_route_planner.sh" + echo " DimOS: python /home/p/pro/dimensional/dimos/dimos/robot/unitree_webrtc/unitree_g1.py" + echo "" + CMD="bash" + ;; +esac + +# Run the container +docker compose -f ros_docker_integration/docker-compose.yml run --rm dimos_autonomy_stack $CMD + +# Revoke X server access when done +xhost -local:docker 2>/dev/null || true + +echo "" +echo -e "${GREEN}Container stopped.${NC}" \ No newline at end of file diff --git a/ros_docker_integration/start_clean.sh b/ros_docker_integration/start_clean.sh new file mode 100755 index 0000000000..af1e8de11b --- /dev/null +++ b/ros_docker_integration/start_clean.sh @@ -0,0 +1,90 @@ +#!/bin/bash + +set -e + +# Colors for output +RED='\033[0;31m' +GREEN='\033[0;32m' +YELLOW='\033[1;33m' +NC='\033[0m' # No Color + +# Get the directory where the script is located +SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" +cd "$SCRIPT_DIR" + +# Allow X server connection from Docker +xhost +local:docker 2>/dev/null || true + +echo -e "${GREEN}=========================================${NC}" +echo -e "${GREEN}Starting DimOS + ROS Autonomy Stack Container${NC}" +echo -e "${GREEN}(Using improved signal handling)${NC}" +echo -e "${GREEN}=========================================${NC}" +echo "" + +# Check if Unity models exist (warn if not) +if [ ! -d "unity_models" ] && [[ "$*" == *"--ros-planner"* || "$*" == *"--all"* ]]; then + echo -e "${YELLOW}WARNING: Unity models directory not found!${NC}" + echo "The Unity simulator may not work properly." + echo "Download from: https://drive.google.com/drive/folders/1G1JYkccvoSlxyySuTlPfvmrWoJUO8oSs" + echo "" +fi + +# Parse command line arguments +MODE="default" +if [[ "$1" == "--ros-planner" ]]; then + MODE="ros-planner" +elif [[ "$1" == "--dimos" ]]; then + MODE="dimos" +elif [[ "$1" == "--all" ]]; then + MODE="all" +elif [[ "$1" == "--help" || "$1" == "-h" ]]; then + echo "Usage: $0 [OPTIONS]" + echo "" + echo "Options:" + echo " --ros-planner Start with ROS route planner" + echo " --dimos Start with DimOS Unitree G1 controller" + echo " --all Start both ROS planner and DimOS (with clean shutdown)" + echo " --help Show this help message" + echo "" + echo "Without options, starts an interactive bash shell" + exit 0 +fi + +# Go to dimos directory (parent of ros_docker_integration) for docker compose context +cd .. + +# Set the command based on mode +case $MODE in + "ros-planner") + echo -e "${YELLOW}Starting with ROS route planner...${NC}" + CMD="bash -c 'cd /ros2_ws/src/autonomy_stack_mecanum_wheel_platform && ./system_simulation_with_route_planner.sh'" + ;; + "dimos") + echo -e "${YELLOW}Starting with DimOS Unitree G1 controller...${NC}" + CMD="python /home/p/pro/dimensional/dimos/dimos/robot/unitree_webrtc/unitree_g1.py" + ;; + "all") + echo -e "${YELLOW}Starting both ROS planner and DimOS with improved signal handling...${NC}" + # Use the Python wrapper for better signal handling + CMD="python3 /usr/local/bin/ros_launch_wrapper.py" + ;; + "default") + echo -e "${YELLOW}Starting interactive bash shell...${NC}" + echo "" + echo "You can manually run:" + echo " ROS planner: cd /ros2_ws/src/autonomy_stack_mecanum_wheel_platform && ./system_simulation_with_route_planner.sh" + echo " DimOS: python /home/p/pro/dimensional/dimos/dimos/robot/unitree_webrtc/unitree_g1.py" + echo " Both (clean shutdown): python3 /usr/local/bin/ros_launch_wrapper.py" + echo "" + CMD="bash" + ;; +esac + +# Run the container +docker compose -f ros_docker_integration/docker-compose.yml run --rm dimos_autonomy_stack $CMD + +# Revoke X server access when done +xhost -local:docker 2>/dev/null || true + +echo "" +echo -e "${GREEN}Container stopped.${NC}" \ No newline at end of file diff --git a/ros_docker_integration/test_integration.sh b/ros_docker_integration/test_integration.sh new file mode 100755 index 0000000000..8df94e77bb --- /dev/null +++ b/ros_docker_integration/test_integration.sh @@ -0,0 +1,121 @@ +#!/bin/bash + +# Test script to verify ROS-DimOS integration + +set -e + +# Colors for output +RED='\033[0;31m' +GREEN='\033[0;32m' +YELLOW='\033[1;33m' +CYAN='\033[0;36m' +NC='\033[0m' # No Color + +echo -e "${CYAN}=========================================${NC}" +echo -e "${CYAN}Testing ROS-DimOS Integration${NC}" +echo -e "${CYAN}=========================================${NC}" +echo "" + +# Get the directory where the script is located +SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" +cd "$SCRIPT_DIR" + +# Allow X server connection from Docker +xhost +local:docker 2>/dev/null || true + +# Go to dimos directory (parent of ros_docker_integration) for docker compose context +cd .. + +echo -e "${YELLOW}Test 1: Checking ROS environment...${NC}" +docker compose -f ros_docker_integration/docker-compose.yml run --rm dimos_autonomy_stack bash -c " + source /opt/ros/jazzy/setup.bash && + source /ros2_ws/install/setup.bash && + echo 'ROS_DISTRO: \$ROS_DISTRO' && + echo 'ROS workspace: /ros2_ws' && + ros2 pkg list | grep -E '(base_autonomy|vehicle_simulator)' | head -5 +" + +if [ $? -eq 0 ]; then + echo -e "${GREEN}✓ ROS environment is properly configured${NC}" +else + echo -e "${RED}✗ ROS environment check failed${NC}" +fi + +echo "" +echo -e "${YELLOW}Test 2: Checking DimOS Python environment...${NC}" +docker compose -f ros_docker_integration/docker-compose.yml run --rm dimos_autonomy_stack bash -c " + source /home/p/pro/dimensional/dimos/.venv/bin/activate && + python -c ' +import sys +print(f\"Python: {sys.version}\") +try: + import dimos + print(\"✓ DimOS package is importable\") +except ImportError as e: + print(f\"✗ DimOS import failed: {e}\") + sys.exit(1) + +try: + import cv2 + print(\"✓ OpenCV is available\") +except ImportError: + print(\"✗ OpenCV import failed\") + +try: + import torch + print(f\"✓ PyTorch is available\") +except ImportError: + print(\"✓ PyTorch not installed (CPU mode)\") +' +" + +if [ $? -eq 0 ]; then + echo -e "${GREEN}✓ DimOS Python environment is properly configured${NC}" +else + echo -e "${RED}✗ DimOS Python environment check failed${NC}" +fi + +echo "" +echo -e "${YELLOW}Test 3: Checking GPU access...${NC}" +docker compose -f ros_docker_integration/docker-compose.yml run --rm dimos_autonomy_stack bash -c " + if command -v nvidia-smi &> /dev/null; then + nvidia-smi --query-gpu=name,driver_version,memory.total --format=csv,noheader + echo '✓ GPU is accessible' + else + echo '✗ nvidia-smi not found - GPU may not be accessible' + fi +" + +echo "" +echo -e "${YELLOW}Test 4: Checking network configuration...${NC}" +docker compose -f ros_docker_integration/docker-compose.yml run --rm dimos_autonomy_stack bash -c " + echo 'Network mode: host' + echo -n 'Can access localhost: ' + if ping -c 1 localhost &> /dev/null; then + echo '✓ Yes' + else + echo '✗ No' + fi +" + +echo "" +echo -e "${YELLOW}Test 5: Checking ROS topic availability...${NC}" +docker compose -f ros_docker_integration/docker-compose.yml run --rm dimos_autonomy_stack bash -c " + source /opt/ros/jazzy/setup.bash && + source /ros2_ws/install/setup.bash && + timeout 2 ros2 topic list 2>/dev/null | head -10 || echo 'Note: No ROS nodes running (this is expected)' +" + +# Revoke X server access when done +xhost -local:docker 2>/dev/null || true + +echo "" +echo -e "${CYAN}=========================================${NC}" +echo -e "${CYAN}Integration Test Complete${NC}" +echo -e "${CYAN}=========================================${NC}" +echo "" +echo "To run the full system:" +echo -e "${YELLOW} ./start.sh --all${NC}" +echo "" +echo "For interactive testing:" +echo -e "${YELLOW} ./shell.sh${NC}" \ No newline at end of file From 5cdb84eea062e33ec25dcf3bc046a6730602cbb6 Mon Sep 17 00:00:00 2001 From: paul-nechifor <1262969+paul-nechifor@users.noreply.github.com> Date: Sat, 25 Oct 2025 04:48:55 +0000 Subject: [PATCH 12/12] CI code cleanup --- ros_docker_integration/ros_launch_wrapper.py | 25 +++++++++++++++++--- 1 file changed, 22 insertions(+), 3 deletions(-) diff --git a/ros_docker_integration/ros_launch_wrapper.py b/ros_docker_integration/ros_launch_wrapper.py index 42f59fe606..38da967100 100755 --- a/ros_docker_integration/ros_launch_wrapper.py +++ b/ros_docker_integration/ros_launch_wrapper.py @@ -1,4 +1,18 @@ #!/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. + """ Wrapper script to properly handle ROS2 launch file shutdown. This script ensures clean shutdown of all ROS nodes when receiving SIGINT. @@ -11,6 +25,7 @@ import time import threading + class ROSLaunchWrapper: def __init__(self): self.ros_process = None @@ -93,7 +108,7 @@ def run(self): print("Starting ROS route planner...") self.ros_process = subprocess.Popen( ["bash", "./system_simulation_with_route_planner.sh"], - preexec_fn=os.setsid # Create new process group + preexec_fn=os.setsid, # Create new process group ) # Wait for ROS to initialize @@ -103,7 +118,10 @@ def run(self): # Start DimOS print("Starting DimOS Unitree G1 controller...") self.dimos_process = subprocess.Popen( - [sys.executable, "/home/p/pro/dimensional/dimos/dimos/robot/unitree_webrtc/unitree_g1.py"] + [ + sys.executable, + "/home/p/pro/dimensional/dimos/dimos/robot/unitree_webrtc/unitree_g1.py", + ] ) print("Both systems are running. Press Ctrl+C to stop.") @@ -127,6 +145,7 @@ def run(self): except KeyboardInterrupt: pass # Signal handler will take care of cleanup + if __name__ == "__main__": wrapper = ROSLaunchWrapper() - wrapper.run() \ No newline at end of file + wrapper.run()