feat: Ardupilot-Gazebo drone simulation with video stream for agentic tracking, spatial model and warehouse environment

Author: snktshrma

dimos/agents/skills/osm.py

@@ -13,6 +13,8 @@
 # limitations under the License.
 
 
+from typing import Any
+
 from dimos.agents.annotation import skill
 from dimos.core.module import Module
 from dimos.core.stream import In
@@ -31,8 +33,8 @@ class OsmSkill(Module):
 
     gps_location: In[LatLon]
 
-    def __init__(self) -> None:
-        super().__init__()
+    def __init__(self, **kwargs: Any) -> None:
+        super().__init__(**kwargs)
         self._latest_location = None
         self._current_location_map = CurrentLocationMap(QwenVlModel())
 

dimos/robot/all_blueprints.py

@@ -51,7 +51,10 @@
     "demo-osm": "dimos.mapping.osm.demo_osm:demo_osm",
     "demo-skill": "dimos.agents.skills.demo_skill:demo_skill",
     "drone-agentic": "dimos.robot.drone.blueprints.agentic.drone_agentic:drone_agentic",
+    "drone-agentic-gazebo": "dimos.robot.drone.blueprints.agentic.drone_agentic:drone_agentic_gazebo",
     "drone-basic": "dimos.robot.drone.blueprints.basic.drone_basic:drone_basic",
+    "drone-basic-gazebo": "dimos.robot.drone.blueprints.basic.drone_basic:drone_basic_gazebo",
+    "drone-basic-gazebo-spatial": "dimos.robot.drone.blueprints.basic.drone_basic:drone_basic_gazebo_spatial",
     "dual-xarm6-planner": "dimos.manipulation.blueprints:dual_xarm6_planner",
     "keyboard-teleop-piper": "dimos.robot.manipulators.piper.blueprints:keyboard_teleop_piper",
     "keyboard-teleop-xarm6": "dimos.robot.manipulators.xarm.blueprints:keyboard_teleop_xarm6",

dimos/robot/drone/README.md

@@ -19,6 +19,11 @@ dimos run drone-basic --set outdoor=true
 
 # Agentic with LLM control
 dimos run drone-agentic
+
+# Gazebo + ArduPilot SITL
+dimos run drone-basic-gazebo
+dimos run drone-agentic-gazebo
+dimos run drone-basic-gazebo-spatial
 ```
 
 To interact with the agent, run `dimos humancli` in a separate terminal.
@@ -48,6 +53,27 @@ Composes on top of `drone-basic`, adding autonomous capabilities:
 | `Agent` | LLM agent (default: GPT-4o) |
 | `WebInput` | Web/CLI interface for human commands |
 
+### `drone-basic-gazebo` / `drone-agentic-gazebo` / `drone-basic-gazebo-spatial`
+Same as the non-Gazebo blueprints but with video from Gazebo (RTP/H264 on UDP 5600). Odometry from MAVLink; when SITL sends `LOCAL_POSITION_NED` it is used for position. Spatial variant adds the semantic map (camera + TF).
+
+## Running with Gazebo + ArduPilot
+
+1. **Install ArduPilot SITL** and a GCS (e.g. MAVProxy). See the [ArduPilot development wiki](https://ardupilot.org/dev/).
+2. **Install Gazebo and the ArduPilot Gazebo plugin** (Iris quad, camera streaming, etc.): [Using SITL with Gazebo](https://ardupilot.org/dev/docs/sitl-with-gazebo.html). Set `GZ_SIM_SYSTEM_PLUGIN_PATH` and `GZ_SIM_RESOURCE_PATH` as described there.
+3. **Start Gazebo** with the Iris world (camera streams to UDP 5600):
+   ```bash
+   gz sim -v4 -r iris_runway.sdf
+   ```
+   Enable camera streaming if needed:
+   ```bash
+   gz topic -t /world/iris_runway/model/iris_with_gimbal/model/gimbal/link/pitch_link/sensor/camera/image/enable_streaming -m gz.msgs.Boolean -p "data: 1"
+   ```
+4. **Start SITL** in another terminal:
+   ```bash
+   sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --map --console
+   ```
+5. **Run DimOS** (e.g. `dimos run drone-agentic-gazebo`). MAVLink: default UDP 14550. Video: port 5600.
+
 ## Installation
 
 ### Python (included with DimOS)

dimos/robot/drone/blueprints/agentic/drone_agentic.py

@@ -25,14 +25,19 @@
 from dimos.agents.skills.osm import OsmSkill
 from dimos.agents.web_human_input import web_input
 from dimos.core.blueprints import autoconnect
-from dimos.robot.drone.blueprints.basic.drone_basic import drone_basic
+from dimos.robot.drone.blueprints.basic.drone_basic import drone_basic, drone_basic_gazebo
 from dimos.robot.drone.drone_tracking_module import DroneTrackingModule
 
 DRONE_SYSTEM_PROMPT = """\
 You are controlling a DJI drone with MAVLink interface.
 You have access to drone control skills you are already flying so only run move_twist, set_mode, and fly_to.
 When the user gives commands, use the appropriate skills to control the drone.
 Always confirm actions and report results. Send fly_to commands only at above 200 meters altitude to be safe.
+
+Motion skills: move(x, y, z, duration) and move_forward(distance, speed) use velocity commands.
+For direct position use go_to_position(x, y, z, vx_ff, vy_ff, vz_ff): position in local NED (m; z negative = up) with optional velocity feedforward.
+Yaw control: use rotate_to(heading_deg) to turn to a heading (0–360°, 0=North, 90=East). Remember you have this for "turn left/right", "face North", etc.
+
 Here are some GPS locations to remember
 6th and Natoma intersection: 37.78019978319006, -122.40770815020853,
 454 Natoma (Office): 37.780967465525244, -122.40688342010769
@@ -44,7 +49,21 @@
     DroneTrackingModule.blueprint(outdoor=False),
     GoogleMapsSkillContainer.blueprint(),
     OsmSkill.blueprint(),
-    agent(system_prompt=DRONE_SYSTEM_PROMPT, model="gpt-4o"),
+    agent(system_prompt=DRONE_SYSTEM_PROMPT, model="gpt-4o-mini"),
+    web_input(),
+).remappings(
+    [
+        (DroneTrackingModule, "video_input", "video"),
+        (DroneTrackingModule, "cmd_vel", "movecmd_twist"),
+    ]
+)
+
+drone_agentic_gazebo = autoconnect(
+    drone_basic_gazebo,
+    DroneTrackingModule.blueprint(outdoor=False),
+    GoogleMapsSkillContainer.blueprint(),
+    OsmSkill.blueprint(),
+    agent(system_prompt=DRONE_SYSTEM_PROMPT, model="gpt-4o-mini"),
     web_input(),
 ).remappings(
     [
@@ -56,4 +75,5 @@
 __all__ = [
     "DRONE_SYSTEM_PROMPT",
     "drone_agentic",
+    "drone_agentic_gazebo",
 ]

dimos/robot/drone/blueprints/basic/drone_basic.py

@@ -20,6 +20,7 @@
 
 from dimos.core.blueprints import autoconnect
 from dimos.core.global_config import global_config
+from dimos.perception.spatial_perception import spatial_memory
 from dimos.protocol.pubsub.impl.lcmpubsub import LCM
 from dimos.robot.drone.camera_module import DroneCameraModule
 from dimos.robot.drone.connection_module import DroneConnectionModule
@@ -44,7 +45,7 @@ def _drone_rerun_blueprint() -> Any:
 
     return rrb.Blueprint(
         rrb.Horizontal(
-            rrb.Spatial2DView(origin="world/video", name="Camera"),
+            rrb.Spatial2DView(origin="world/color_image", name="Camera"),
             rrb.Spatial3DView(
                 origin="world",
                 name="3D",
@@ -95,6 +96,25 @@ def _drone_rerun_blueprint() -> Any:
     websocket_vis(),
 )
 
+drone_basic_gazebo = autoconnect(
+    _vis,
+    DroneConnectionModule.blueprint(
+        connection_string=connection_string,
+        video_port=video_port,
+        video_source="gazebo",
+        outdoor=False,
+    ),
+    DroneCameraModule.blueprint(camera_intrinsics=[1000.0, 1000.0, 960.0, 540.0]),
+    websocket_vis(),
+)
+
+drone_basic_gazebo_spatial = autoconnect(
+    drone_basic_gazebo,
+    spatial_memory(),
+)
+
 __all__ = [
     "drone_basic",
+    "drone_basic_gazebo",
+    "drone_basic_gazebo_spatial",
 ]

dimos/robot/drone/connection_module.py

@@ -36,6 +36,7 @@
 from dimos.msgs.geometry_msgs.Vector3 import Vector3
 from dimos.msgs.sensor_msgs.Image import Image
 from dimos.robot.drone.dji_video_stream import DJIDroneVideoStream
+from dimos.robot.drone.gazebo_video_stream import GazeboVideoStream
 from dimos.robot.drone.mavlink_connection import MavlinkConnection
 from dimos.utils.logging_config import setup_logger
 
@@ -52,6 +53,7 @@ def _add_disposable(composite: CompositeDisposable, item: Disposable | Any) -> N
 class Config(ModuleConfig):
     connection_string: str = "udp:0.0.0.0:14550"
     video_port: int = 5600
+    video_source: str = "dji"
     outdoor: bool = False
 
 
@@ -92,7 +94,7 @@ def __init__(self, **kwargs: Any) -> None:
         """
         super().__init__(**kwargs)
         self.connection: MavlinkConnection | None = None
-        self.video_stream: DJIDroneVideoStream | None = None
+        self.video_stream: DJIDroneVideoStream | GazeboVideoStream | None = None
         self._latest_video_frame = None
         self._latest_telemetry = None
         self._latest_status = None
@@ -116,7 +118,10 @@ def start(self) -> None:
             )
             self.connection.connect()
 
-            self.video_stream = DJIDroneVideoStream(port=self.config.video_port)
+            if self.config.video_source == "gazebo":
+                self.video_stream = GazeboVideoStream(port=self.config.video_port)
+            else:
+                self.video_stream = DJIDroneVideoStream(port=self.config.video_port)
 
         if not self.connection.connected:
             logger.error("Failed to connect to drone")
@@ -282,6 +287,56 @@ def move(self, x: float = 0.0, y: float = 0.0, z: float = 0.0, duration: float =
         if self.connection:
             self.connection.move(Vector3(x, y, z), duration)
 
+    @skill
+    def move_forward(self, distance: float, speed: float = 0.3) -> None:
+        """Move forward by a given distance in meters.
+
+        Args:
+            distance: Distance to move in meters (positive = forward).
+            speed: Forward speed in m/s. Default 0.3.
+        """
+        if self.connection and abs(distance) >= 0.01:
+            duration = abs(distance) / max(0.01, speed)
+            vx = speed if distance >= 0 else -speed
+            self.connection.move(Vector3(vx, 0.0, 0.0), duration)
+
+    @skill
+    def go_to_position(
+        self, x: float, y: float, z: float,
+        vx_ff: float = 0.0, vy_ff: float = 0.0, vz_ff: float = 0.0,
+    ) -> bool:
+        """Send position target in local NED with optional velocity feedforward.
+
+        Args:
+            x: North position in meters (local NED).
+            y: East position in meters (local NED).
+            z: Down position in meters (negative = up).
+            vx_ff: Velocity feedforward North (m/s). Optional.
+            vy_ff: Velocity feedforward East (m/s). Optional.
+            vz_ff: Velocity feedforward Down (m/s). Optional.
+
+        Returns:
+            True if position target sent successfully
+        """
+        if self.connection:
+            return self.connection.set_position_target(x, y, z, vx_ff, vy_ff, vz_ff)
+        return False
+
+    @skill
+    def rotate_to(self, heading_deg: float, timeout: float = 60.0) -> bool:
+        """Rotate drone to a specific heading (yaw control).
+
+        Args:
+            heading_deg: Target heading in degrees (0–360, 0=North, 90=East).
+            timeout: Max time to rotate in seconds.
+
+        Returns:
+            True if rotation completed successfully
+        """
+        if self.connection:
+            return self.connection.rotate_to(heading_deg, timeout)
+        return False
+
     @skill
     def takeoff(self, altitude: float = 3.0) -> bool:
         """Takeoff to specified altitude.

dimos/robot/drone/drone_tracking_module.py

@@ -63,6 +63,7 @@ def __init__(
         x_pid_params: PIDParams | None = None,
         y_pid_params: PIDParams | None = None,
         z_pid_params: PIDParams | None = None,
+        **kwargs: Any,
     ) -> None:
         """Initialize the drone tracking module.
 
@@ -75,7 +76,7 @@ def __init__(
                           If None, uses preset based on outdoor flag.
             z_pid_params: Optional PID parameters for altitude control.
         """
-        super().__init__()
+        super().__init__(**kwargs)
 
         default_params = OUTDOOR_PID_PARAMS if outdoor else INDOOR_PID_PARAMS
         x_pid_params = x_pid_params if x_pid_params is not None else default_params