feat(drone): integrate PX4 autonomous navigation drone and SITL simulation

dimos/robot/all_blueprints.py

@@ -61,8 +61,8 @@
     "demo-virtual-mid360-fastlio": "dimos.hardware.sensors.lidar.virtual_mid360.blueprints:demo_virtual_mid360_fastlio",
     "demo-virtual-mid360-pointlio": "dimos.hardware.sensors.lidar.virtual_mid360.blueprints:demo_virtual_mid360_pointlio",
     "desk-marker-tf": "dimos.perception.fiducial.blueprints.desk_marker_tf:desk_marker_tf",
-    "drone-agentic": "dimos.robot.drone.blueprints.agentic.drone_agentic:drone_agentic",
-    "drone-basic": "dimos.robot.drone.blueprints.basic.drone_basic:drone_basic",
+    "drone-agentic": "dimos.robot.drone.dji.blueprints.agentic.drone_agentic:drone_agentic",
+    "drone-basic": "dimos.robot.drone.dji.blueprints.basic.drone_basic:drone_basic",
     "dual-xarm6-planner-coordinator": "dimos.robot.manipulators.xarm.blueprints.basic:dual_xarm6_planner_coordinator",
     "go2-zenoh-basic": "dimos.robot.unitree.go2.zenoh.blueprints:go2_zenoh_basic",
     "go2-zenoh-htc": "dimos.robot.unitree.go2.zenoh.blueprints:go2_zenoh_htc",
@@ -90,6 +90,9 @@
     "openarm-mock-planner-coordinator": "dimos.robot.manipulators.openarm.blueprints.planner:openarm_mock_planner_coordinator",
     "openarm-planner-coordinator": "dimos.robot.manipulators.openarm.blueprints.planner:openarm_planner_coordinator",
     "openyam-planner-coordinator": "dimos.robot.manipulators.openyam.blueprints.basic:openyam_planner_coordinator",
+    "px4-agentic": "dimos.robot.drone.px4.blueprints.agentic.px4_agentic:px4_agentic",
+    "px4-basic": "dimos.robot.drone.px4.blueprints.basic.px4_basic:px4_basic",
+    "px4-gazebo-harmonic": "dimos.robot.drone.px4.blueprints.basic.px4_basic:px4_gazebo_harmonic",
     "spot": "dimos.experimental.robot.bosdyn.spot.blueprints.spot:spot",
     "spot-record": "dimos.experimental.robot.bosdyn.spot.blueprints.spot_record:spot_record",
     "teleop-hosted-go2-multicam": "dimos.teleop.hosted.blueprints.cloudflare:teleop_hosted_go2_multicam",
@@ -191,15 +194,16 @@
     "desk-static-tf-module": "dimos.perception.fiducial.blueprints.desk_marker_tf.DeskStaticTfModule",
     "detection2-d-module": "dimos.perception.detection.module2D.Detection2DModule",
     "detection3-d-module": "dimos.perception.detection.module3D.Detection3DModule",
-    "drone-camera-module": "dimos.robot.drone.camera_module.DroneCameraModule",
-    "drone-connection-module": "dimos.robot.drone.connection_module.DroneConnectionModule",
-    "drone-tracking-module": "dimos.robot.drone.drone_tracking_module.DroneTrackingModule",
+    "drone-camera-module": "dimos.robot.drone.dji.camera_module.DroneCameraModule",
+    "drone-connection-module": "dimos.robot.drone.dji.connection_module.DroneConnectionModule",
+    "drone-tracking-module": "dimos.robot.drone.dji.drone_tracking_module.DroneTrackingModule",
     "emitter-module": "dimos.utils.demo_image_encoding.EmitterModule",
     "episode-monitor-module": "dimos.imitation.collection.episode_monitor.EpisodeMonitorModule",
     "eval-module": "dimos.evals.module.EvalModule",
     "far-planner": "dimos.navigation.cmu_nav.modules.far_planner.far_planner.FarPlanner",
     "fast-lio2": "dimos.hardware.sensors.lidar.fastlio2.module.FastLio2",
     "fast-lio2-recorder": "dimos.hardware.sensors.lidar.fastlio2.recorder.FastLio2Recorder",
+    "flight-controller": "dimos.robot.drone.px4.flight_control.FlightController",
     "front-camera": "dimos.teleop.hosted.blueprints.cloudflare.FrontCamera",
     "g1-connection": "dimos.robot.unitree.g1.connection.G1Connection",
     "g1-connection-base": "dimos.robot.unitree.g1.connection.G1ConnectionBase",
@@ -221,6 +225,7 @@
     "gps-nav-skill-container": "dimos.agents.skills.gps_nav_skill.GpsNavSkillContainer",
     "grasp-gen-x-module": "dimos.manipulation.grasping.grasp_gen_x.GraspGenXModule",
     "grasping-module": "dimos.manipulation.grasping.grasping.GraspingModule",
+    "gs-tee-camera": "dimos.robot.drone.px4.gstreamer_tee_camera.GsTeeCamera",
     "gstreamer-camera-module": "dimos.hardware.sensors.camera.gstreamer.gstreamer_camera.GstreamerCameraModule",
     "hand-teleop-module": "dimos.teleop.quest.quest_extensions.HandTeleopModule",
     "hosted-stats-module": "dimos.teleop.hosted.hosted_stats.HostedStatsModule",
@@ -238,6 +243,7 @@
     "mcp-client": "dimos.agents.mcp.mcp_client.McpClient",
     "mcp-server": "dimos.agents.mcp.mcp_server.McpServer",
     "memory-module": "dimos.memory.module.MemoryModule",
+    "mid360-mount-static-tf": "dimos.robot.drone.px4.mid360_mount_tf.Mid360MountStaticTf",
     "mid360-pcap-recorder": "dimos.hardware.sensors.lidar.virtual_mid360.recorder.Mid360PcapRecorder",
     "mid360-realsense-recorder": "dimos.robot.assembly.mid360_realsense_30.Mid360RealsenseRecorder",
     "mid360-realsense-static-tf": "dimos.robot.assembly.mid360_realsense_30.Mid360RealsenseStaticTf",

dimos/robot/drone/README.md → dimos/robot/drone/dji/README.md

@@ -1,6 +1,6 @@
 # DimOS Drone Module
 
-DJI drone integration via RosettaDrone MAVLink bridge, with visual servoing, autonomous tracking, and LLM agent control.
+DJI drone integration via RosettaDrone MAVLink bridge, with visual servoing, autonomous tracking, and LLM agent control. For the PX4 MAVSDK stack, see [`px4/README.md`](../px4/README.md).
 
 ## Quick Start
 
@@ -148,15 +148,17 @@ DJI Drone ← Wireless → DJI Controller ← USB → Android Device ← WiFi 
 ### Module Structure
 ```
 dimos/robot/drone/
-├── blueprints/
-│   ├── basic/drone_basic.py              # Base blueprint (connection + camera + vis)
-│   └── agentic/drone_agentic.py          # Agentic blueprint (composes on basic)
-├── connection_module.py                   # MAVLink communication & skills
-├── camera_module.py                       # Camera processing & intrinsics
-├── drone_tracking_module.py               # Visual servoing & object tracking
-├── drone_visual_servoing_controller.py    # PID-based visual servoing
-├── mavlink_connection.py                  # Low-level MAVLink protocol
-└── dji_video_stream.py                    # GStreamer video capture + replay
+├── dji/
+│   ├── blueprints/
+│   │   ├── basic/drone_basic.py           # Base blueprint (connection + camera + vis)
+│   │   └── agentic/drone_agentic.py       # Agentic blueprint (composes on basic)
+│   ├── connection_module.py               # MAVLink communication & skills
+│   ├── camera_module.py                   # Camera processing & intrinsics
+│   ├── drone_tracking_module.py           # Visual servoing & object tracking
+│   ├── drone_visual_servoing_controller.py # PID-based visual servoing
+│   ├── mavlink_connection.py              # Low-level MAVLink protocol
+│   └── dji_video_stream.py                # GStreamer video capture + replay
+└── px4/                                    # MAVSDK flight, video, and mapping stack
 ```
 
 ### Communication Flow

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

@@ -26,8 +26,8 @@
 from dimos.agents.skills.osm import OsmSkill
 from dimos.agents.web_human_input import WebInput
 from dimos.core.coordination.blueprints import autoconnect
-from dimos.robot.drone.blueprints.basic.drone_basic import drone_basic
-from dimos.robot.drone.drone_tracking_module import DroneTrackingModule
+from dimos.robot.drone.dji.blueprints.basic.drone_basic import drone_basic
+from dimos.robot.drone.dji.drone_tracking_module import DroneTrackingModule
 
 DRONE_SYSTEM_PROMPT = """\
 You are controlling a DJI drone with MAVLink interface.

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

@@ -20,8 +20,8 @@
 
 from dimos.core.coordination.blueprints import autoconnect
 from dimos.core.global_config import global_config
-from dimos.robot.drone.camera_module import DroneCameraModule
-from dimos.robot.drone.connection_module import DroneConnectionModule
+from dimos.robot.drone.dji.camera_module import DroneCameraModule
+from dimos.robot.drone.dji.connection_module import DroneConnectionModule
 from dimos.visualization.vis_module import vis_module
 
 

dimos/robot/drone/connection_module.py → dimos/robot/drone/dji/connection_module.py

@@ -37,8 +37,8 @@
 from dimos.msgs.geometry_msgs.Vector3 import Vector3
 from dimos.msgs.sensor_msgs.Image import Image
 from dimos.msgs.tf2_msgs.TFMessage import TFMessage
-from dimos.robot.drone.dji_video_stream import DJIDroneVideoStream
-from dimos.robot.drone.mavlink_connection import MavlinkConnection
+from dimos.robot.drone.dji.dji_video_stream import DJIDroneVideoStream
+from dimos.robot.drone.dji.mavlink_connection import MavlinkConnection
 from dimos.utils.logging_config import setup_logger
 
 logger = setup_logger()
@@ -103,8 +103,8 @@ def start(self) -> None:
         """Start the connection and subscribe to sensor streams."""
         # Check for replay mode
         if self.config.connection_string == "replay":
-            from dimos.robot.drone.dji_video_stream import FakeDJIVideoStream
-            from dimos.robot.drone.mavlink_connection import FakeMavlinkConnection
+            from dimos.robot.drone.dji.dji_video_stream import FakeDJIVideoStream
+            from dimos.robot.drone.dji.mavlink_connection import FakeMavlinkConnection
 
             self.connection = FakeMavlinkConnection("replay")
             self.video_stream = FakeDJIVideoStream(port=self.config.video_port)

dimos/robot/drone/drone_tracking_module.py → dimos/robot/drone/dji/drone_tracking_module.py

@@ -32,7 +32,7 @@
 from dimos.msgs.geometry_msgs.Twist import Twist
 from dimos.msgs.geometry_msgs.Vector3 import Vector3
 from dimos.msgs.sensor_msgs.Image import Image, ImageFormat
-from dimos.robot.drone.drone_visual_servoing_controller import (
+from dimos.robot.drone.dji.drone_visual_servoing_controller import (
     DroneVisualServoingController,
     PIDParams,
 )

dimos/robot/drone/test_drone.py → dimos/robot/drone/dji/test_drone.py

@@ -27,13 +27,14 @@
 import pytest
 
 from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped
+from dimos.msgs.geometry_msgs.Quaternion import Quaternion
 from dimos.msgs.geometry_msgs.Vector3 import Vector3
 from dimos.msgs.sensor_msgs.Image import Image, ImageFormat
-from dimos.robot.drone.connection_module import DroneConnectionModule
-from dimos.robot.drone.dji_video_stream import FakeDJIVideoStream
+from dimos.robot.drone.dji.connection_module import DroneConnectionModule
+from dimos.robot.drone.dji.dji_video_stream import FakeDJIVideoStream
 
 # Drone class removed - use blueprints instead
-from dimos.robot.drone.mavlink_connection import FakeMavlinkConnection, MavlinkConnection
+from dimos.robot.drone.dji.mavlink_connection import FakeMavlinkConnection, MavlinkConnection
 
 
 class TestMavlinkProcessing(unittest.TestCase):
@@ -235,8 +236,12 @@ def test_fake_video_stream_no_throttling(self) -> None:
 
     def test_connection_module_replay_mode(self) -> None:
         """Test connection module uses Fake classes in replay mode."""
-        with patch("dimos.robot.drone.mavlink_connection.FakeMavlinkConnection") as mock_fake_conn:
-            with patch("dimos.robot.drone.dji_video_stream.FakeDJIVideoStream") as mock_fake_video:
+        with patch(
+            "dimos.robot.drone.dji.mavlink_connection.FakeMavlinkConnection"
+        ) as mock_fake_conn:
+            with patch(
+                "dimos.robot.drone.dji.dji_video_stream.FakeDJIVideoStream"
+            ) as mock_fake_video:
                 # Mock the fake connection
                 mock_conn_instance = MagicMock()
                 mock_conn_instance.connected = True
@@ -412,6 +417,156 @@ def replay_side_effect(store_name: str):
                 module.stop()
 
 
+@unittest.skip("Skipped: TestDroneFullIntegration tests deprecated Drone class")
+class TestDroneFullIntegration(unittest.TestCase):
+    """Full integration test of Drone class with replay mode."""
+
+    def setUp(self) -> None:
+        """Set up test environment."""
+        # Mock the DimOS core module
+        self.mock_dimos = MagicMock()
+        self.mock_dimos.deploy.return_value = MagicMock()
+
+        # Mock pubsub.lcm.autoconf
+        self.pubsub_patch = patch("dimos.protocol.pubsub.lcm.autoconf")
+        self.pubsub_patch.start()
+
+    def tearDown(self) -> None:
+        """Clean up patches."""
+        self.pubsub_patch.stop()
+
+    @patch("dimos.robot.drone.dji.drone.ModuleCoordinator")
+    @patch("dimos.utils.testing.legacy_pickle.LegacyPickleStore")
+    def test_full_system_with_replay(self, mock_replay, mock_coordinator_class) -> None:
+        """Test full drone system initialization and operation with replay mode."""
+        # Set up mock replay data
+        mavlink_messages = [
+            {"mavpackettype": "HEARTBEAT", "type": 2, "base_mode": 193, "armed": True},
+            {"mavpackettype": "ATTITUDE", "roll": 0.1, "pitch": 0.2, "yaw": 0.3},
+            {
+                "mavpackettype": "GLOBAL_POSITION_INT",
+                "lat": 377810501,
+                "lon": -1224069671,
+                "alt": 5000,
+                "relative_alt": 5000,
+                "vx": 100,  # 1 m/s North
+                "vy": 200,  # 2 m/s East
+                "vz": -50,  # 0.5 m/s Up
+                "hdg": 9000,  # 90 degrees
+            },
+            {
+                "mavpackettype": "BATTERY_STATUS",
+                "voltages": [3800, 3800, 3800, 3800],
+                "battery_remaining": 75,
+            },
+        ]
+
+        video_frames = [
+            Image(
+                data=np.random.randint(0, 255, (360, 640, 3), dtype=np.uint8),
+                format=ImageFormat.BGR,
+            )
+        ]
+
+        def replay_side_effect(store_name: str):
+            mock = MagicMock()
+            if "mavlink" in store_name:
+                # Create stream that emits MAVLink messages
+                stream = MagicMock()
+                stream.subscribe = lambda callback: [callback(msg) for msg in mavlink_messages]
+                mock.stream.return_value = stream
+            elif "video" in store_name:
+                # Create stream that emits video frames
+                stream = MagicMock()
+                stream.subscribe = lambda callback: [callback(frame) for frame in video_frames]
+                mock.stream.return_value = stream
+            return mock
+
+        mock_replay.side_effect = replay_side_effect
+
+        # Mock ModuleCoordinator
+        mock_coordinator_class.return_value = self.mock_dimos
+
+        # Create drone in replay mode
+        drone = Drone(connection_string="replay", video_port=5600)
+
+        # Mock the deployed modules
+        mock_connection = MagicMock()
+        mock_camera = MagicMock()
+
+        # Set up return values for module methods
+        mock_connection.start.return_value = True
+        mock_connection.get_odom.return_value = PoseStamped(
+            position=Vector3(1.0, 2.0, 3.0), orientation=Quaternion(0, 0, 0, 1), frame_id="world"
+        )
+        mock_connection.get_status.return_value = {
+            "armed": True,
+            "battery_voltage": 15.2,
+            "battery_remaining": 75,
+            "altitude": 5.0,
+        }
+
+        mock_camera.start.return_value = True
+
+        # Configure deploy to return our mocked modules
+        def deploy_side_effect(module_class, **kwargs):
+            if "DroneConnectionModule" in str(module_class):
+                return mock_connection
+            elif "DroneCameraModule" in str(module_class):
+                return mock_camera
+            return MagicMock()
+
+        self.mock_dimos.deploy.side_effect = deploy_side_effect
+
+        # Start the drone system
+        drone.start()
+
+        # Verify modules were deployed
+        self.assertEqual(self.mock_dimos.deploy.call_count, 4)
+
+        # Test get_odom
+        odom = drone.get_odom()
+        self.assertIsNotNone(odom)
+        self.assertEqual(odom.position.x, 1.0)
+        self.assertEqual(odom.position.y, 2.0)
+        self.assertEqual(odom.position.z, 3.0)
+
+        # Test get_status
+        status = drone.get_status()
+        self.assertIsNotNone(status)
+        self.assertTrue(status["armed"])
+        self.assertEqual(status["battery_remaining"], 75)
+
+        # Test movement command
+        drone.move(Vector3(1.0, 0.0, 0.5), duration=2.0)
+        mock_connection.move.assert_called_once_with(Vector3(1.0, 0.0, 0.5), 2.0)
+
+        # Test control commands
+        drone.arm()
+        mock_connection.arm.assert_called_once()
+
+        drone.takeoff(altitude=10.0)
+        mock_connection.takeoff.assert_called_once_with(10.0)
+
+        drone.land()
+        mock_connection.land.assert_called_once()
+
+        drone.disarm()
+        mock_connection.disarm.assert_called_once()
+
+        # Test mode setting
+        drone.set_mode("GUIDED")
+        mock_connection.set_mode.assert_called_once_with("GUIDED")
+
+        # Clean up
+        drone.stop()
+
+        # Verify cleanup was called
+        mock_connection.stop.assert_called_once()
+        mock_camera.stop.assert_called_once()
+        self.mock_dimos.stop.assert_called_once()
+
+
 class TestDroneControlCommands(unittest.TestCase):
     """Test drone control commands with FakeMavlinkConnection."""
 
@@ -789,7 +944,7 @@ class TestVisualServoingEdgeCases(unittest.TestCase):
 
     def test_output_clamping(self) -> None:
         """Large errors are clamped to max_velocity."""
-        from dimos.robot.drone.drone_visual_servoing_controller import (
+        from dimos.robot.drone.dji.drone_visual_servoing_controller import (
             DroneVisualServoingController,
         )
 
@@ -809,7 +964,7 @@ def test_output_clamping(self) -> None:
 
     def test_deadband_prevents_integral_windup(self) -> None:
         """Deadband prevents integral accumulation for small errors."""
-        from dimos.robot.drone.drone_visual_servoing_controller import (
+        from dimos.robot.drone.dji.drone_visual_servoing_controller import (
             DroneVisualServoingController,
         )
 
@@ -831,7 +986,7 @@ def test_deadband_prevents_integral_windup(self) -> None:
 
     def test_reset_clears_integral(self) -> None:
         """reset() clears accumulated integral to prevent windup."""
-        from dimos.robot.drone.drone_visual_servoing_controller import (
+        from dimos.robot.drone.dji.drone_visual_servoing_controller import (
             DroneVisualServoingController,
         )
 
@@ -860,7 +1015,7 @@ class TestVisualServoingVelocity(unittest.TestCase):
 
     def test_velocity_from_bbox_center_error(self) -> None:
         """Bbox center offset produces proportional velocity command."""
-        from dimos.robot.drone.drone_visual_servoing_controller import (
+        from dimos.robot.drone.dji.drone_visual_servoing_controller import (
             DroneVisualServoingController,
         )