diff --git a/dimos/hardware/camera/module.py b/dimos/hardware/camera/module.py index 2b2880b80a..561ce7508c 100644 --- a/dimos/hardware/camera/module.py +++ b/dimos/hardware/camera/module.py @@ -125,3 +125,6 @@ def stop(self): if self.hardware and hasattr(self.hardware, "stop"): self.hardware.stop() super().stop() + + +camera_module = CameraModule.blueprint diff --git a/dimos/navigation/rosnav/nav_bot.py b/dimos/navigation/rosnav/nav_bot.py index 4a5ca0c45a..c6c8016273 100644 --- a/dimos/navigation/rosnav/nav_bot.py +++ b/dimos/navigation/rosnav/nav_bot.py @@ -28,8 +28,15 @@ from dimos import core from dimos.protocol import pubsub -from dimos.core import In, Out, rpc -from dimos.msgs.geometry_msgs import PoseStamped, Twist, Transform, Vector3, Quaternion +from dimos.core import In, Out, rpc, DimosCluster, LCMTransport, pSHMTransport +from dimos.msgs.geometry_msgs import ( + PoseStamped, + Twist, + TwistStamped, + Transform, + Vector3, + Quaternion, +) from dimos.msgs.nav_msgs import Odometry, Path from dimos.msgs.sensor_msgs import PointCloud2, Joy from dimos.msgs.std_msgs import Bool @@ -320,6 +327,9 @@ def stop(self): logger.error(f"Error during shutdown: {e}") +navigation_module = ROSNavigationModule.blueprint + + class NavBot: """ NavBot wrapper that deploys NavigationModule with proper DIMOS/ROS2 integration. @@ -419,5 +429,20 @@ def main(): nav_bot.shutdown() +def deploy(dimos: DimosCluster): + nav = dimos.deploy(ROSNav) + nav.pointcloud.transport = pSHMTransport("/lidar") + nav.global_pointcloud.transport = pSHMTransport("/map") + # nav.pointcloud.transport = LCMTransport("/lidar", PointCloud2) + # nav.global_pointcloud.transport = LCMTransport("/map", PointCloud2) + + nav.goal_req.transport = LCMTransport("/goal_req", PoseStamped) + nav.goal_active.transport = LCMTransport("/goal_active", PoseStamped) + nav.path_active.transport = LCMTransport("/path_active", Path) + nav.cmd_vel.transport = LCMTransport("/cmd_vel", TwistStamped) + nav.start() + return nav + + if __name__ == "__main__": main() diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 2eef48855f..77414fc93e 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -21,6 +21,12 @@ "unitree-go2-basic": "dimos.robot.unitree_webrtc.unitree_go2_blueprints:basic", "unitree-go2-shm": "dimos.robot.unitree_webrtc.unitree_go2_blueprints:standard_with_shm", "unitree-go2-agentic": "dimos.robot.unitree_webrtc.unitree_go2_blueprints:agentic", + "unitree-g1": "dimos.robot.unitree_webrtc.unitree_g1_blueprints:standard", + "unitree-g1-basic": "dimos.robot.unitree_webrtc.unitree_g1_blueprints:basic", + "unitree-g1-shm": "dimos.robot.unitree_webrtc.unitree_g1_blueprints:standard_with_shm", + "unitree-g1-agentic": "dimos.robot.unitree_webrtc.unitree_g1_blueprints:agentic", + "unitree-g1-joystick": "dimos.robot.unitree_webrtc.unitree_g1_blueprints:with_joystick", + "unitree-g1-full": "dimos.robot.unitree_webrtc.unitree_g1_blueprints:full_featured", "demo-osm": "dimos.mapping.osm.demo_osm:demo_osm", } diff --git a/dimos/robot/unitree_webrtc/g1_joystick_module.py b/dimos/robot/unitree_webrtc/g1_joystick_module.py index 156a0891a2..eaf1ade576 100644 --- a/dimos/robot/unitree_webrtc/g1_joystick_module.py +++ b/dimos/robot/unitree_webrtc/g1_joystick_module.py @@ -179,3 +179,7 @@ def _update_display(self, twist): y_pos += 25 pygame.display.flip() + + +# Create blueprint function for easy instantiation +g1_joystick = G1JoystickModule.blueprint diff --git a/dimos/robot/unitree_webrtc/rosnav.py b/dimos/robot/unitree_webrtc/rosnav.py index 969ddad950..55a97edf2a 100644 --- a/dimos/robot/unitree_webrtc/rosnav.py +++ b/dimos/robot/unitree_webrtc/rosnav.py @@ -39,6 +39,7 @@ logger = setup_logger("dimos.robot.unitree_webrtc.nav_bot", level=logging.INFO) +# TODO: Remove, deprecated class NavigationModule(Module): goal_pose: Out[PoseStamped] = None goal_reached: In[Bool] = None diff --git a/dimos/robot/unitree_webrtc/unitree_g1.py b/dimos/robot/unitree_webrtc/unitree_g1.py index d8f6975d27..22a8426da7 100644 --- a/dimos/robot/unitree_webrtc/unitree_g1.py +++ b/dimos/robot/unitree_webrtc/unitree_g1.py @@ -31,6 +31,7 @@ from sensor_msgs.msg import Joy as ROSJoy from sensor_msgs.msg import PointCloud2 as ROSPointCloud2 from tf2_msgs.msg import TFMessage as ROSTFMessage +from dimos.core.global_config import GlobalConfig from dimos import core from dimos.agents2 import Agent @@ -86,16 +87,24 @@ class G1ConnectionModule(Module): """Simplified connection module for G1 - uses WebRTC for control.""" - movecmd: In[TwistStamped] = None + movecmd: In[Twist] = None odom_in: In[Odometry] = None odom_pose: Out[PoseStamped] = None ip: str connection_type: str = "webrtc" - def __init__(self, ip: str = None, connection_type: str = "webrtc", *args, **kwargs): - self.ip = ip - self.connection_type = connection_type + def __init__( + self, + ip: str = None, + connection_type: str = "webrtc", + global_config: GlobalConfig | None = None, + *args, + **kwargs, + ): + cfg = global_config or GlobalConfig() + self.ip = ip if ip is not None else cfg.robot_ip + self.connection_type = connection_type or cfg.unitree_connection_type self.connection = None Module.__init__(self, *args, **kwargs) @@ -129,9 +138,8 @@ def _publish_odom_pose(self, msg: Odometry): ) @rpc - def move(self, twist_stamped: TwistStamped, duration: float = 0.0): + def move(self, twist: Twist, 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 @@ -140,6 +148,9 @@ def publish_request(self, topic: str, data: dict): return self.connection.publish_request(topic, data) +connection = G1ConnectionModule.blueprint + + class UnitreeG1(Robot, Resource): """Unitree G1 humanoid robot.""" @@ -362,11 +373,12 @@ def _deploy_camera(self): def _deploy_visualization(self): """Deploy and configure visualization modules.""" - # Deploy WebSocket visualization module - self.websocket_vis = self._dimos.deploy(WebsocketVisModule, port=self.websocket_port) - self.websocket_vis.movecmd_stamped.transport = core.LCMTransport("/cmd_vel", TwistStamped) + # Deploy WebSocket visualization module - COMMENTED OUT DUE TO TRANSPORT ISSUES + # self.websocket_vis = self._dimos.deploy(WebsocketVisModule, port=self.websocket_port) + # self.websocket_vis.movecmd_stamped.transport = core.LCMTransport("/cmd_vel", TwistStamped) - # Note: robot_pose connection removed since odom was removed from G1ConnectionModule + # Connect odometry to websocket visualization + # self.websocket_vis.odom.transport = core.LCMTransport("/odom", PoseStamped) # Deploy Foxglove bridge self.foxglove_bridge = FoxgloveBridge( diff --git a/dimos/robot/unitree_webrtc/unitree_g1_blueprints.py b/dimos/robot/unitree_webrtc/unitree_g1_blueprints.py new file mode 100644 index 0000000000..cb9be0e8df --- /dev/null +++ b/dimos/robot/unitree_webrtc/unitree_g1_blueprints.py @@ -0,0 +1,173 @@ +#!/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. + +"""Blueprint configurations for Unitree G1 humanoid robot. + +This module provides pre-configured blueprints for various G1 robot setups, +from basic teleoperation to full autonomous agent configurations. +""" + +from dimos.constants import DEFAULT_CAPACITY_COLOR_IMAGE, DEFAULT_CAPACITY_DEPTH_IMAGE +from dimos.core.blueprints import autoconnect +from dimos.core.transport import LCMTransport, pSHMTransport +from dimos.msgs.geometry_msgs import ( + PoseStamped, + Twist, + TwistStamped, + Transform, + Vector3, + Quaternion, +) +from dimos.msgs.sensor_msgs import Image, PointCloud2 +from dimos.msgs.nav_msgs import Odometry, Path +from dimos_lcm.sensor_msgs import CameraInfo +from dimos.msgs.std_msgs import Bool +from dimos.perception.spatial_perception import spatial_memory +from dimos.robot.foxglove_bridge import foxglove_bridge +from dimos.robot.unitree_webrtc.unitree_g1 import connection +from dimos.navigation.rosnav.nav_bot import navigation_module +from dimos.utils.monitoring import utilization +from dimos.web.websocket_vis.websocket_vis_module import websocket_vis +from dimos.navigation.global_planner import astar_planner +from dimos.navigation.local_planner.holonomic_local_planner import ( + holonomic_local_planner, +) +from dimos.navigation.bt_navigator.navigator import ( + behavior_tree_navigator, +) +from dimos.navigation.frontier_exploration import ( + wavefront_frontier_explorer, +) +from dimos.robot.unitree_webrtc.type.map import mapper +from dimos.robot.unitree_webrtc.depth_module import depth_module +from dimos.perception.object_tracker import object_tracking +from dimos.agents2.agent import llm_agent +from dimos.agents2.cli.human import human_input +from dimos.agents2.skills.navigation import navigation_skill +from dimos.robot.unitree_webrtc.g1_joystick_module import g1_joystick +from dimos.robot.unitree_webrtc.unitree_g1_skill_container import g1_skills +from dimos.hardware.camera.module import camera_module +from dimos.hardware.camera.webcam import Webcam +from dimos.hardware.camera import zed + + +# Basic configuration with navigation and visualization +basic = ( + autoconnect( + # Core connection module for G1 + connection(), + # Camera module + camera_module( + transform=Transform( + translation=Vector3(0.05, 0.0, 0.0), + rotation=Quaternion.from_euler(Vector3(0.0, 0.2, 0.0)), + frame_id="sensor", + child_frame_id="camera_link", + ), + hardware=lambda: Webcam( + camera_index=0, + frequency=15, + stereo_slice="left", + camera_info=zed.CameraInfo.SingleWebcam, + ), + ), + # SLAM and mapping + mapper(voxel_size=0.5, global_publish_interval=2.5), + # Navigation stack + astar_planner(), + holonomic_local_planner(), + behavior_tree_navigator(), + wavefront_frontier_explorer(), + navigation_module(), # G1-specific ROS navigation + # Visualization + websocket_vis(), + foxglove_bridge(), + ) + .with_global_config(n_dask_workers=4) + .with_transports( + { + # G1 uses Twist for movement commands + ("cmd_vel", Twist): LCMTransport("/cmd_vel", Twist), + ("movecmd", Twist): LCMTransport("/cmd_vel", Twist), + # State estimation from ROS + ("state_estimation", Odometry): LCMTransport("/state_estimation", Odometry), + # Odometry output from ROSNavigationModule + ("odom_pose", PoseStamped): LCMTransport("/odom", PoseStamped), + # Navigation module topics from nav_bot + ("goal_req", PoseStamped): LCMTransport("/goal_req", PoseStamped), + ("goal_active", PoseStamped): LCMTransport("/goal_active", PoseStamped), + ("path_active", Path): LCMTransport("/path_active", Path), + ("pointcloud", PointCloud2): LCMTransport("/lidar", PointCloud2), + ("global_pointcloud", PointCloud2): LCMTransport("/map", PointCloud2), + # Original navigation topics for backwards compatibility + ("goal_pose", PoseStamped): LCMTransport("/goal_pose", PoseStamped), + ("goal_reached", Bool): LCMTransport("/goal_reached", Bool), + ("cancel_goal", Bool): LCMTransport("/cancel_goal", Bool), + # Camera topics (if camera module is added) + ("image", Image): LCMTransport("/g1/color_image", Image), + ("color_image", Image): LCMTransport("/g1/color_image", Image), + ("camera_info", CameraInfo): LCMTransport("/g1/camera_info", CameraInfo), + } + ) +) + +# Standard configuration with perception and memory +standard = autoconnect( + basic, + spatial_memory(), + object_tracking(frame_id="camera_link"), + utilization(), +).with_global_config(n_dask_workers=8) + +# Optimized configuration using shared memory for images +standard_with_shm = autoconnect( + standard.with_transports( + { + ("color_image", Image): pSHMTransport( + "/g1/color_image", default_capacity=DEFAULT_CAPACITY_COLOR_IMAGE + ), + } + ), + foxglove_bridge( + shm_channels=[ + "/g1/color_image#sensor_msgs.Image", + ] + ), +) + +# Full agentic configuration with LLM and skills +agentic = autoconnect( + standard, + llm_agent(), + human_input(), + navigation_skill(), + g1_skills(), # G1-specific arm and movement mode skills +) + +# Configuration with joystick control for teleoperation +with_joystick = autoconnect( + basic, + g1_joystick(), # Pygame-based joystick control +) + +# Full featured configuration with everything +full_featured = autoconnect( + standard_with_shm, + llm_agent(), + human_input(), + navigation_skill(), + g1_skills(), + g1_joystick(), +) diff --git a/dimos/robot/unitree_webrtc/unitree_g1_skill_container.py b/dimos/robot/unitree_webrtc/unitree_g1_skill_container.py index 2ca937dde3..ef0bb1df0c 100644 --- a/dimos/robot/unitree_webrtc/unitree_g1_skill_container.py +++ b/dimos/robot/unitree_webrtc/unitree_g1_skill_container.py @@ -232,3 +232,7 @@ def _execute_mode_command(self, data_value: int, name: str) -> str: error_msg = f"Failed to execute G1 mode {name}: {e}" logger.error(error_msg) return error_msg + + +# Create blueprint function for easy instantiation +g1_skills = UnitreeG1SkillContainer.blueprint