diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 03a4ddc2b8..c920ed27f1 100644 --- a/dimos/robot/all_blueprints.py +++ b/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", diff --git a/dimos/robot/drone/README.md b/dimos/robot/drone/dji/README.md similarity index 90% rename from dimos/robot/drone/README.md rename to dimos/robot/drone/dji/README.md index d88d2ce1b8..b5146beeb1 100644 --- a/dimos/robot/drone/README.md +++ b/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 diff --git a/dimos/robot/drone/blueprints/agentic/drone_agentic.py b/dimos/robot/drone/dji/blueprints/agentic/drone_agentic.py similarity index 93% rename from dimos/robot/drone/blueprints/agentic/drone_agentic.py rename to dimos/robot/drone/dji/blueprints/agentic/drone_agentic.py index 5fbce4ef72..7f92b64292 100644 --- a/dimos/robot/drone/blueprints/agentic/drone_agentic.py +++ b/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. diff --git a/dimos/robot/drone/blueprints/basic/drone_basic.py b/dimos/robot/drone/dji/blueprints/basic/drone_basic.py similarity index 94% rename from dimos/robot/drone/blueprints/basic/drone_basic.py rename to dimos/robot/drone/dji/blueprints/basic/drone_basic.py index 4fbee93840..1b6bad4a9e 100644 --- a/dimos/robot/drone/blueprints/basic/drone_basic.py +++ b/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 diff --git a/dimos/robot/drone/camera_module.py b/dimos/robot/drone/dji/camera_module.py similarity index 100% rename from dimos/robot/drone/camera_module.py rename to dimos/robot/drone/dji/camera_module.py diff --git a/dimos/robot/drone/connection_module.py b/dimos/robot/drone/dji/connection_module.py similarity index 98% rename from dimos/robot/drone/connection_module.py rename to dimos/robot/drone/dji/connection_module.py index 21519fcc84..56eb5aca4e 100644 --- a/dimos/robot/drone/connection_module.py +++ b/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) diff --git a/dimos/robot/drone/dji_video_stream.py b/dimos/robot/drone/dji/dji_video_stream.py similarity index 100% rename from dimos/robot/drone/dji_video_stream.py rename to dimos/robot/drone/dji/dji_video_stream.py diff --git a/dimos/robot/drone/drone_tracking_module.py b/dimos/robot/drone/dji/drone_tracking_module.py similarity index 99% rename from dimos/robot/drone/drone_tracking_module.py rename to dimos/robot/drone/dji/drone_tracking_module.py index ec18323ad3..60dbcb50bc 100644 --- a/dimos/robot/drone/drone_tracking_module.py +++ b/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, ) diff --git a/dimos/robot/drone/drone_visual_servoing_controller.py b/dimos/robot/drone/dji/drone_visual_servoing_controller.py similarity index 100% rename from dimos/robot/drone/drone_visual_servoing_controller.py rename to dimos/robot/drone/dji/drone_visual_servoing_controller.py diff --git a/dimos/robot/drone/mavlink_connection.py b/dimos/robot/drone/dji/mavlink_connection.py similarity index 100% rename from dimos/robot/drone/mavlink_connection.py rename to dimos/robot/drone/dji/mavlink_connection.py diff --git a/dimos/robot/drone/test_drone.py b/dimos/robot/drone/dji/test_drone.py similarity index 84% rename from dimos/robot/drone/test_drone.py rename to dimos/robot/drone/dji/test_drone.py index 2e7336c2c7..573bcd7bf7 100644 --- a/dimos/robot/drone/test_drone.py +++ b/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, ) diff --git a/dimos/robot/drone/px4/README.md b/dimos/robot/drone/px4/README.md new file mode 100644 index 0000000000..b883d36856 --- /dev/null +++ b/dimos/robot/drone/px4/README.md @@ -0,0 +1,291 @@ +# PX4 Module + +## Demo + +| Hardware: camera, MID360 point cloud, and voxel map | Simulation: Gazebo gimbal camera | +|---|---| +| ![PX4 hardware frontend](https://github.com/user-attachments/assets/adaaa34b-1ced-4229-98da-02f1e9f4e9f5) | ![PX4 Gazebo Harmonic simulation frontend](https://github.com/user-attachments/assets/19358c12-3d02-4c18-b7b1-687e05fb6f2c) | + +DimOS integration for PX4 drones using MAVSDK. The module combines flight control, MID360 localization, voxel mapping, GStreamer video distribution, Rerun visualization, Gazebo Harmonic simulation, and optional LLM agent control. The hardware stack currently targets a PX4 flight controller connected at `serial:///dev/ttyTHS3:921600` and a Livox MID360 at `192.168.1.3`. + +## Quick Start + +The following commands assume that the [installation](#installation) is complete. They explicitly provide the minimum hardware inputs used by the current setup: MID360 network addresses, MAVSDK serial connection, and V4L2 camera pipeline. Replace the device paths and local host address when the hardware differs. + +### Hardware stack: `px4-basic` + +```bash +dimos run px4-basic \ + --pointlio.lidar-ip=192.168.1.3 \ + --pointlio.host-ip=192.168.1.50 \ + --flightcontroller.connection-url='serial:///dev/ttyTHS3:921600' \ + --flightcontroller.connection-timeout-s=20 \ + --gsteecamera.input-pipeline='v4l2src device=/dev/video0 ! video/x-raw,width=640,height=480,framerate=30/1' \ + --gsteecamera.input-format=raw +``` + +For a headless onboard computer that only accepts a remote `dimos-viewer`, use: + +```bash +dimos --viewer rerun --rerun-open none --no-rerun-web --rerun-host 0.0.0.0 run px4-basic \ + --pointlio.lidar-ip=192.168.1.3 \ + --pointlio.host-ip=192.168.1.50 \ + --flightcontroller.connection-url='serial:///dev/ttyTHS3:921600' \ + --flightcontroller.connection-timeout-s=20 \ + --gsteecamera.input-pipeline='v4l2src device=/dev/video0 ! video/x-raw,width=640,height=480,framerate=30/1' \ + --gsteecamera.input-format=raw +``` + +Connect from the remote computer, replacing `` with the address of the computer running DimOS: + +```bash +dimos-viewer \ + --connect rerun+http://:9877/proxy \ + --ws-url ws://:3030/ws +``` + +`dimos-viewer` currently sends directional commands only. It does not arm, take off, or enter Offboard mode. Before its direction controls can work, open another terminal on the onboard computer, enter `dimos shell`, verify that the flight area is safe, and run these commands in order: + +```bash +dimos shell +``` + +```python +app.FlightController.arm() +app.FlightController.takeoff(3.0) +# Wait until the vehicle reaches a safe altitude and stabilizes, then run: +app.FlightController.enter_offboard() +``` + +The remote viewer controls become active only after `enter_offboard()` returns `offboard mode entered`. Run `app.FlightController.hold()` to stop directional control, then `app.FlightController.land()` after confirming that the landing area is safe. Do not call `disarm()` while airborne. + +### Hardware stack with agent: `px4-agentic` + +```bash +export OPENAI_API_KEY=sk-... + +dimos run px4-agentic \ + --pointlio.lidar-ip=192.168.1.3 \ + --pointlio.host-ip=192.168.1.50 \ + --flightcontroller.connection-url='serial:///dev/ttyTHS3:921600' \ + --flightcontroller.connection-timeout-s=20 \ + --gsteecamera.input-pipeline='v4l2src device=/dev/video0 ! video/x-raw,width=640,height=480,framerate=30/1' \ + --gsteecamera.input-format=raw +``` + +In another terminal, inspect the available tools or send a request: + +```bash +dimos mcp list-tools +dimos agent-send "take off to 2 meters" +``` + +### Simulation: `px4-gazebo-harmonic` + +The simulation scene comes from [PX4-AeroFusion-Sim](https://github.com/weyne-Jiang/PX4-AeroFusion-Sim). Start PX4 SITL and Gazebo in terminal 1: + +```bash +cd /path/to/PX4-Autopilot +PX4_GZ_NO_FOLLOW=1 make px4_sitl gz_x500_gimbal_windy +``` + +After PX4 reports its MAVLink endpoint, start DimOS in terminal 2. Use `dimos status`, `dimos log -f`, and `dimos stop` to inspect or stop the running stack. + +```bash +cd /path/to/dimos +dimos run px4-gazebo-harmonic +``` + +## Blueprints and Modules + +The PX4 integration provides three runnable blueprints. `px4-basic` is the complete hardware stack for flight, localization, mapping, video, and visualization; `px4-agentic` adds an LLM agent; and `px4-gazebo-harmonic` targets Gazebo SITL without PointLIO or voxel mapping. + +| Blueprint | Composition and data flow | +|-----------|---------------------------| +| `px4-basic` | Connects real PX4 hardware, MID360, and a camera, providing external vision, voxel mapping, and Rerun visualization | +| `px4-agentic` | Adds `McpServer`, `McpClient`, and `WebInput` to `px4-basic`, exposing `@skill` flight methods to the LLM agent | +| `px4-gazebo-harmonic` | Connects to PX4 SITL at `udpin://0.0.0.0:14540`, receives Gazebo H.264 video on UDP `5600`, and shows only the camera view in Rerun | + +`px4-basic` and `px4-agentic` use these core modules: + +| Module | Purpose | +|--------|---------| +| `FlightController` | Direct MAVSDK connection, telemetry, flight skills, Offboard velocity control, and external-vision forwarding | +| `PointLio` | MID360 point-cloud processing and lidar odometry | +| `Mid360MountStaticTf` | Calibrated static transform from `mid360_link` to `base_link` | +| `RayTracingVoxelMap` | Local and global voxel maps with ray-traced clearing | +| `GsTeeCamera` | One GStreamer input split into raw BGR and Annex-B H.264 streams | +| Rerun visualization modules | Camera, point-cloud, map, TF, and teleoperation visualization | + +## Available Skills + +`FlightController` provides the flight skills below. They can be called through `dimos shell` in every PX4 blueprint. `px4-agentic` also exposes them to the agent through MCP. + +| Skill | Parameters | Purpose and prerequisites | +|-------|------------|---------------------------| +| `arm()` | None | Arm the vehicle | +| `disarm()` | None | Disarm the vehicle | +| `takeoff(altitude=3.0)` | `altitude`: takeoff altitude in meters | Take off to the specified altitude | +| `land()` | None | Land at the current position | +| `enter_offboard()` | None | Enter Offboard mode | +| `exit_offboard()` | None | Exit Offboard mode | +| `hold()` | None | Enter position-hold mode | +| `move(forward=0, left=0, up=0, yaw_rate=0)` | Body-FLU velocity in m/s; counter-clockwise `yaw_rate` in rad/s | Move briefly; repeat calls to continue moving; requires Offboard mode | +| `goto(north, east, down, yaw=0)` | NED position in meters; clockwise `yaw` in degrees | Fly to the specified local position; requires Offboard mode | +| `hover()` | None | Hover at the current position; requires Offboard mode | + +For example, run the following basic flight sequence in `dimos shell`. A successful RPC return means that the command was sent, not that the maneuver has completed, so check PX4 telemetry between steps. In particular, wait until the vehicle reaches a safe altitude and stabilizes before entering Offboard mode. + +```python +app.FlightController.arm() +app.FlightController.takeoff(3.0) +# Wait until the vehicle reaches a safe altitude and stabilizes. +app.FlightController.enter_offboard() +app.FlightController.move(forward=0.5) +app.FlightController.hold() +app.FlightController.land() +``` + +With `px4-agentic`, list the skills actually registered with MCP using: + +```bash +dimos mcp list-tools +``` + +## Installation + +### Python dependencies + +On Ubuntu 24.04, install PyGObject and GStreamer from apt. `python3-gi` +provides the Python bindings, while the GIR packages expose the `Gst` and +`GstApp` APIs used by `GsTeeCamera`. The plugin packages provide camera input, +conversion, H.264 decoding, and X264 encoding: + +```bash +sudo apt-get update +sudo apt-get install -y \ + python3-gi \ + gir1.2-gstreamer-1.0 gir1.2-gst-plugins-base-1.0 \ + gstreamer1.0-tools gstreamer1.0-plugins-base \ + gstreamer1.0-plugins-good gstreamer1.0-plugins-bad \ + gstreamer1.0-plugins-ugly gstreamer1.0-libav +``` + +Create the project environment with access to Ubuntu's system Python packages, +then install the DimOS drone dependencies. A default isolated uv environment +cannot import apt's `python3-gi` package: + +```bash +uv venv --system-site-packages --python /usr/bin/python3 +uv sync --extra drone +``` + +The `drone` extra installs MAVSDK and pymavlink. PyGObject and pycairo are not +installed from PyPI. Jetson hardware encoding additionally requires the NVIDIA +GStreamer plugins provided by JetPack, including `nvv4l2h264enc` and +`nvvidconv`. + +### Native PointLIO and ray-tracing modules + +Nix with flakes enabled is required. Build both native executables once: + +```bash +cd dimos/hardware/sensors/lidar/pointlio/cpp +nix build -L .#pointlio_native + +cd ../../../../../mapping/ray_tracing/rust +nix build -L path:. +``` + +Each build creates a `result` link in its module directory. The corresponding DimOS `NativeModule` starts `result/bin/pointlio_native` or `result/bin/voxel_ray_tracing` from that directory. + +## Hardware Configuration + +### MAVLink + +The default MAVLink endpoint is `serial:///dev/ttyTHS3:921600`, with a 10-second connection timeout. Override the endpoint with `--flightcontroller.connection-url` when the flight controller uses another serial device, and adjust the timeout with `--flightcontroller.connection-timeout-s`. The following complete hardware example changes only the serial device to `/dev/ttyUSB0`: + +```bash +dimos run px4-basic \ + --flightcontroller.connection-url='serial:///dev/ttyUSB0:921600' \ + --pointlio.lidar-ip=192.168.1.3 \ + --pointlio.host-ip=192.168.1.50 \ + --gsteecamera.input-pipeline='v4l2src device=/dev/video0 ! video/x-raw,width=640,height=480,framerate=30/1' \ + --gsteecamera.input-format=raw +``` + +The SITL endpoint `udpin://0.0.0.0:14540` belongs to `px4-gazebo-harmonic` and should not be mixed into a `px4-basic` hardware configuration that also enables PointLIO. + +### MID360 and external vision + +The MID360 address is `192.168.1.3`. `--pointlio.host-ip` must be an address on the local lidar-facing interface in the same subnet. It may be omitted when PointLIO can select that interface automatically. + +Point-LIO publishes the sensor pose and the mount publisher completes this TF chain: + +```text +odom -> mid360_link -> base_link +``` + +`mid360_link -> base_link` applies the inverse of the calibrated 15-degree sensor mount transform. `FlightController` uses the same conversion to derive the body pose, converts FLU to FRD, and sends it to PX4 through MAVSDK `set_vision_position_estimate`. + +## Video Pipeline + +`GsTeeCamera` accepts a trusted GStreamer pipeline ending in raw video or H.264 and always publishes both outputs. Rerun records H.264 at `drone/video`; raw lidar visualization is limited to 5 Hz, while camera calibration and camera TF publication remain outside this stack. + +| Stream | Transport | Content | +|--------|-----------|---------| +| `color_image` at `/color_image` | pSHM | Raw BGR frames | +| `video_h264` at `/video_h264` | Typed LCM | Annex-B H.264 access units | + +Raw input is split before conversion and encoding. `bitrate` is measured in bits per second and `gop` is the maximum keyframe interval in frames; both NVV4L2 and X264 use these values: + +```bash +dimos run px4-basic \ + --gsteecamera.input-pipeline='videotestsrc is-live=true' \ + --gsteecamera.input-format=raw \ + --gsteecamera.encoder=x264enc \ + --gsteecamera.bitrate=2000000 \ + --gsteecamera.gop=30 \ + --pointlio.lidar-ip=192.168.1.3 \ + --pointlio.host-ip=192.168.1.50 +``` + +H.264 input is forwarded without re-encoding and decoded only for the BGR branch. This mode rejects explicit `encoder`, `bitrate`, or `gop` settings because they would be unused: + +```bash +dimos run px4-basic \ + --gsteecamera.input-pipeline='rtspsrc location=rtsp://camera/stream latency=50 ! rtph264depay ! h264parse config-interval=-1' \ + --gsteecamera.input-format=h264 \ + --pointlio.lidar-ip=192.168.1.3 \ + --pointlio.host-ip=192.168.1.50 +``` + +## File Structure + +PX4 blueprints and dedicated modules live under `dimos/robot/drone/px4/`. PointLIO and ray tracing are shared DimOS modules under `dimos/hardware/sensors/lidar/pointlio/` and `dimos/mapping/ray_tracing/`, respectively: + +```text +dimos/robot/drone/px4/ +├── blueprints/ +│ ├── basic/ +│ │ ├── px4_basic.py # Hardware and Gazebo blueprints +│ │ └── test_px4_basic.py +│ └── agentic/ +│ └── px4_agentic.py # MCP and LLM agent composition +├── flight_control.py # MAVSDK flight control and external vision +├── gstreamer_tee_camera.py # Raw BGR and H.264 GStreamer tee +├── mid360_mount_tf.py # Calibrated MID360-to-body transform +├── test_external_vision.py +├── test_gstreamer_tee_camera.py +├── test_mid360_mount_tf.py +└── README.md +``` + +## Validation and Safety + +Validate command paths against PX4 SITL before connecting real hardware, and do not arm or enter Offboard mode during telemetry-only checks. Run the following code checks: + +```bash +uv run pytest dimos/robot/drone/px4 -q +uv run ruff check dimos/robot/drone/px4 +``` diff --git a/dimos/robot/drone/px4/blueprints/agentic/px4_agentic.py b/dimos/robot/drone/px4/blueprints/agentic/px4_agentic.py new file mode 100644 index 0000000000..60879bf3c3 --- /dev/null +++ b/dimos/robot/drone/px4/blueprints/agentic/px4_agentic.py @@ -0,0 +1,38 @@ +# Copyright 2026 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. + +"""PX4 stack with MCP-based agent control.""" + +from dimos.agents.mcp.mcp_client import McpClient +from dimos.agents.mcp.mcp_server import McpServer +from dimos.agents.web_human_input import WebInput +from dimos.core.coordination.blueprints import autoconnect +from dimos.robot.drone.px4.blueprints.basic.px4_basic import px4_basic + +PX4_SYSTEM_PROMPT = """\ +You control a PX4 drone through MAVSDK. +Use the available flight skills only when the user explicitly requests a flight action. +Never arm, take off, land, disarm, or move based on an assumption or an ambiguous request. +Use enter_offboard before move or goto. The move skill is watchdog-protected: call it repeatedly +to maintain velocity, or once for a brief adjustment that automatically returns to zero velocity. +Use hold to stop Offboard control and hold position. +Report command failures exactly and do not claim that a command succeeded when a tool reports failure. +""" + +px4_agentic = autoconnect( + px4_basic, + McpServer.blueprint(), + McpClient.blueprint(system_prompt=PX4_SYSTEM_PROMPT), + WebInput.blueprint(), +).global_config(n_workers=11, robot_model="px4_agentic") diff --git a/dimos/robot/drone/px4/blueprints/basic/px4_basic.py b/dimos/robot/drone/px4/blueprints/basic/px4_basic.py new file mode 100644 index 0000000000..4c40871c8e --- /dev/null +++ b/dimos/robot/drone/px4/blueprints/basic/px4_basic.py @@ -0,0 +1,161 @@ +# Copyright 2026 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. + +"""PX4 hardware and Gazebo Harmonic blueprints.""" + +from typing import Any + +from dimos.constants import DEFAULT_CAPACITY_COLOR_IMAGE +from dimos.core.coordination.blueprints import autoconnect +from dimos.core.global_config import global_config +from dimos.core.transport import LCMTransport, pSHMTransport +from dimos.hardware.sensors.lidar.pointlio.module import PointLio +from dimos.mapping.ray_tracing.module import RayTracingVoxelMap +from dimos.msgs.foxglove_msgs.CompressedVideo import CompressedVideo +from dimos.msgs.sensor_msgs.Image import Image +from dimos.robot.drone.px4.flight_control import FlightController +from dimos.robot.drone.px4.gstreamer_tee_camera import GsTeeCamera, GstInputFormat +from dimos.robot.drone.px4.mid360_mount_tf import Mid360MountStaticTf +from dimos.visualization.rerun.websocket_server import RerunWebSocketServer +from dimos.visualization.vis_module import vis_module + +RERUN_VIDEO_ENTITY = "drone/video" + + +def _video_h264_to_rerun(video: CompressedVideo) -> Any: + return [(RERUN_VIDEO_ENTITY, video.to_rerun())] + + +def _rerun_layout() -> Any: + import rerun as rr + import rerun.blueprint as rrb + + return rrb.Blueprint( + rrb.Horizontal( + rrb.Spatial2DView(origin=RERUN_VIDEO_ENTITY, name="Camera"), + rrb.Spatial3DView( + origin="world", + name="3D", + background=rrb.Background(kind="SolidColor", color=[0, 0, 0]), + line_grid=rrb.LineGrid3D( + plane=rr.components.Plane3D.XY.with_distance(0.5), + ), + ), + column_shares=[1, 2], + ), + rrb.TimePanel(state="hidden"), + rrb.SelectionPanel(state="hidden"), + ) + + +def _gazebo_rerun_layout() -> Any: + import rerun.blueprint as rrb + + return rrb.Blueprint( + rrb.Spatial2DView(origin=RERUN_VIDEO_ENTITY, name="Camera"), + rrb.TimePanel(state="hidden"), + rrb.SelectionPanel(state="hidden"), + ) + + +def _static_drone_body(rr: Any) -> list[Any]: + return [ + rr.Boxes3D( + half_sizes=[0.2, 0.2, 0.1], + colors=[(255, 100, 0)], + ), + rr.Transform3D(parent_frame="tf#/base_link"), + ] + + +rerun_config: dict[str, Any] = { + "blueprint": _rerun_layout, + "visual_override": { + "world/color_image": None, + "world/video_h264": _video_h264_to_rerun, + "world/region_bounds": None, + }, + "max_hz": { + "world/global_map": 0, + "world/local_map": 0, + "world/lidar": 5.0, + }, + "static": {"world/drone/body": _static_drone_body}, +} + +gazebo_rerun_config: dict[str, Any] = { + "blueprint": _gazebo_rerun_layout, + "visual_override": { + "world/color_image": None, + "world/video_h264": _video_h264_to_rerun, + }, +} + + +px4_basic = ( + autoconnect( + PointLio.blueprint(sensor_frame_id="mid360_link"), + Mid360MountStaticTf.blueprint(), + FlightController.blueprint(), + GsTeeCamera.blueprint(), + RayTracingVoxelMap.blueprint( + voxel_size=0.1, + max_range=5.0, + shadow_depth=0.1, + min_health=0, + max_health=5, + emit_every=2, + ray_subsample=1, + global_emit_every=10, + ), + vis_module( + global_config.viewer, + rerun_config=rerun_config, + ), + ) + .transports( + { + ("color_image", Image): pSHMTransport.spec( + "/color_image", default_capacity=DEFAULT_CAPACITY_COLOR_IMAGE + ), + ("video_h264", CompressedVideo): LCMTransport.spec("/video_h264", CompressedVideo), + } + ) + .remappings([(RerunWebSocketServer, "tele_cmd_vel", "cmd_vel")]) + .global_config(n_workers=8, robot_model="px4_basic") +) + +px4_gazebo_harmonic = ( + autoconnect( + FlightController.blueprint(connection_url="udpin://0.0.0.0:14540"), + GsTeeCamera.blueprint( + input_pipeline=( + "udpsrc port=5600 caps=application/x-rtp,media=video," + "encoding-name=H264,payload=96 ! rtph264depay ! h264parse config-interval=-1" + ), + input_format=GstInputFormat.H264, + ), + vis_module(global_config.viewer, rerun_config=gazebo_rerun_config), + ) + .transports( + { + ("color_image", Image): pSHMTransport.spec( + "/color_image", default_capacity=DEFAULT_CAPACITY_COLOR_IMAGE + ), + ("video_h264", CompressedVideo): LCMTransport.spec("/video_h264", CompressedVideo), + } + ) + .remappings([(RerunWebSocketServer, "tele_cmd_vel", "cmd_vel")]) + .global_config(n_workers=4, robot_model="px4_gazebo_harmonic") +) diff --git a/dimos/robot/drone/px4/flight_control.py b/dimos/robot/drone/px4/flight_control.py new file mode 100644 index 0000000000..2f247eca84 --- /dev/null +++ b/dimos/robot/drone/px4/flight_control.py @@ -0,0 +1,488 @@ +# Copyright 2026 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. + +"""Direct MAVSDK gRPC flight control and external-vision forwarding.""" + +import asyncio +from collections.abc import Sequence +from dataclasses import dataclass +from logging import Logger +import math +from typing import Final, TypeAlias + +from mavsdk import System # type: ignore[import-not-found,import-untyped] +from mavsdk.action import ActionError # type: ignore[import-not-found,import-untyped] +from mavsdk.mocap import ( # type: ignore[import-not-found,import-untyped] + AngleBody, + Covariance, + MocapError, + PositionBody, + VisionPositionEstimate, +) +from mavsdk.offboard import ( # type: ignore[import-not-found,import-untyped] + OffboardError, + PositionNedYaw, + VelocityBodyYawspeed, +) +import numpy as np +from numpy.typing import NDArray +from typing_extensions import override + +from dimos.agents.annotation import skill +from dimos.agents.capabilities import CAP_MOVEMENT +from dimos.core.core import rpc +from dimos.core.module import Module, ModuleConfig +from dimos.core.stream import In +from dimos.msgs.geometry_msgs.Quaternion import Quaternion +from dimos.msgs.geometry_msgs.Transform import Transform +from dimos.msgs.geometry_msgs.Twist import Twist +from dimos.msgs.nav_msgs.Odometry import Odometry +from dimos.robot.drone.px4.mid360_mount_tf import BASE_TO_MID360 +from dimos.utils.logging_config import setup_logger + +# MAVSDK installs uvloop as the process-wide policy during import. DimOS Modules +# use Python's default policy; restoring it prevents uvloop and native LCM +# lifecycle teardown from corrupting later Module instances in the same worker. +asyncio.set_event_loop_policy(asyncio.DefaultEventLoopPolicy()) + +logger: Logger = setup_logger() + +_VisionQuaternion: TypeAlias = tuple[float, float, float, float] +_VisionCovariance: TypeAlias = tuple[float, ...] +_FLU_TO_FRD: Final[NDArray[np.float64]] = np.diag((1.0, -1.0, -1.0)) +_UNKNOWN_COVARIANCE: Final[_VisionCovariance] = (float("nan"),) + + +class FlightControllerConfig(ModuleConfig): + connection_url: str = "serial:///dev/ttyTHS3:921600" + connection_timeout_s: float = 10.0 + move_watchdog_timeout_s: float = 0.5 + + +@dataclass(frozen=True, slots=True) +class MavsdkConnectionTimeoutError(TimeoutError): + connection_url: str + timeout_s: float + + @override + def __str__(self) -> str: + return f"MAVSDK did not connect to {self.connection_url} within {self.timeout_s:g} seconds" + + +class _InvalidExternalVisionSampleError(Exception): + """Raised when a Point-LIO sample cannot be forwarded safely.""" + + +def _require_finite(values: Sequence[float], detail: str) -> None: + if not all(math.isfinite(value) for value in values): + raise _InvalidExternalVisionSampleError(detail) + + +def _quaternion_from_hamilton(quaternion: _VisionQuaternion) -> Quaternion: + result = Quaternion.from_rotation_matrix(np.eye(3)) + result.x, result.y, result.z, result.w = ( + quaternion[1], + quaternion[2], + quaternion[3], + quaternion[0], + ) + return result + + +def _validated_quaternion(quaternion: Quaternion) -> Quaternion: + values = quaternion.to_tuple() + _require_finite(values, "orientation quaternion must be finite") + norm = math.sqrt(sum(value * value for value in values)) + if not math.isclose(norm, 1.0, abs_tol=1e-6): + raise _InvalidExternalVisionSampleError("orientation quaternion must be normalized") + return _quaternion_from_hamilton((values[3], values[0], values[1], values[2])).normalize() + + +def _transform_pose_covariance_to_frd( + covariance: Sequence[float] | NDArray[np.float64], + odom_mid360_quaternion: _VisionQuaternion, +) -> _VisionCovariance: + if len(covariance) != 36 or not all(math.isfinite(value) for value in covariance): + return _UNKNOWN_COVARIANCE + w, x, y, z = odom_mid360_quaternion + rotation = _validated_quaternion(_quaternion_from_hamilton((w, x, y, z))) + lever = rotation.rotate_vector(BASE_TO_MID360.inverse().translation) + skew = np.array( + ( + (0.0, -lever.z, lever.y), + (lever.z, 0.0, -lever.x), + (-lever.y, lever.x, 0.0), + ) + ) + jacobian = np.zeros((6, 6)) + jacobian[:3, :3] = _FLU_TO_FRD + jacobian[:3, 3:] = -_FLU_TO_FRD @ skew + jacobian[3:, 3:] = _FLU_TO_FRD + transformed = jacobian @ np.asarray(covariance, dtype=float).reshape((6, 6)) @ jacobian.T + return tuple(float(transformed[row, column]) for row in range(6) for column in range(row, 6)) + + +def _build_vision_position_estimate(message: Odometry) -> VisionPositionEstimate: + if message.frame_id != "odom" or message.child_frame_id != "mid360_link": + raise _InvalidExternalVisionSampleError("PointLIO odometry must be odom -> mid360_link") + if not math.isfinite(message.ts) or message.ts <= 0.0: + raise _InvalidExternalVisionSampleError("timestamp must be finite and positive") + odom_to_mid360 = Transform.from_pose("mid360_link", message.to_pose_stamped()) + odom_to_mid360.rotation = _validated_quaternion(message.orientation) + odom_to_base = odom_to_mid360 + BASE_TO_MID360.inverse() + quaternion = message.orientation + rotation = odom_to_base.rotation + frd_orientation = _quaternion_from_hamilton((rotation.w, rotation.x, -rotation.y, -rotation.z)) + euler = frd_orientation.to_euler() + position = odom_to_base.translation + _require_finite((position.x, position.y, position.z), "position must be finite") + return VisionPositionEstimate( + time_usec=round(message.ts * 1_000_000.0), + position_body=PositionBody(position.x, -position.y, -position.z), + angle_body=AngleBody(euler.roll, euler.pitch, euler.yaw), + pose_covariance=Covariance( + list( + _transform_pose_covariance_to_frd( + message.pose.covariance.tolist(), + (quaternion.w, quaternion.x, quaternion.y, quaternion.z), + ) + ) + ), + reset_counter=0, + ) + + +class FlightController(Module): + """Expose MAVSDK gRPC flight skills, velocity control, and vision pose.""" + + config: FlightControllerConfig + cmd_vel: In[Twist] + odometry: In[Odometry] + + # RPC handlers are registered before start(); callers must not invoke skills until startup completes. + def __init__(self, **kwargs: object) -> None: + super().__init__(**kwargs) + self._system: System + self._connected: bool = False + self._armed: bool | None = None + self._in_air: bool | None = None + self._flight_mode: str | None = None + self._offboard_active: bool = False + self._position_ned: tuple[float, float, float] | None = None + self._yaw_deg: float | None = None + self._telemetry_tasks: list[asyncio.Task[None]] = [] + self._move_watchdog_task: asyncio.Task[None] | None = None + self._move_watchdog_lock = asyncio.Lock() + + def _auto_bind_handlers(self) -> None: + """Bind only inputs connected by the selected PX4 blueprint.""" + for stream, handler in ( + (self.cmd_vel, self.handle_cmd_vel), + (self.odometry, self.handle_odometry), + ): + if getattr(stream, "_transport", None) is not None: + self.process_observable(stream.pure_observable(), handler) + + @skill(uses=[CAP_MOVEMENT]) + async def arm(self) -> str: + """Arm the vehicle motors.""" + try: + await self._system.action.arm() + except ActionError as error: + return f"arm failed: {error}" + return "arm command sent" + + @skill(uses=[CAP_MOVEMENT]) + async def disarm(self) -> str: + """Disarm the vehicle motors when the autopilot permits it.""" + try: + await self._system.action.disarm() + except ActionError as error: + return f"disarm failed: {error}" + return "disarm command sent" + + @skill(uses=[CAP_MOVEMENT]) + async def takeoff(self, altitude: float = 3.0) -> str: + """Set a positive takeoff altitude and command takeoff. + + Args: + altitude: Target altitude above the takeoff point in meters. + """ + if not math.isfinite(altitude) or altitude <= 0.0: + return "takeoff failed: altitude must be a finite positive number" + try: + await self._system.action.set_takeoff_altitude(altitude) + await self._system.action.takeoff() + except ActionError as error: + return f"takeoff failed: {error}" + return f"takeoff command sent for {altitude} m" + + @skill(uses=[CAP_MOVEMENT]) + async def land(self) -> str: + """Command the vehicle to land at its current position.""" + try: + await self._system.action.land() + except ActionError as error: + return f"land failed: {error}" + return "land command sent" + + async def _set_body_velocity( + self, forward: float, left: float, up: float, yaw_rate: float + ) -> None: + if not all(math.isfinite(value) for value in (forward, left, up, yaw_rate)): + raise ValueError("frame conversion inputs must be finite") + await self._system.offboard.set_velocity_body( + VelocityBodyYawspeed(forward, -left, -up, -math.degrees(yaw_rate)) + ) + + async def _cancel_move_watchdog(self) -> None: + task = self._move_watchdog_task + self._move_watchdog_task = None + if task is not None and not task.done(): + task.cancel() + await asyncio.gather(task, return_exceptions=True) + + @skill(uses=[CAP_MOVEMENT]) + async def move( + self, forward: float = 0.0, left: float = 0.0, up: float = 0.0, yaw_rate: float = 0.0 + ) -> str: + """Set a temporary body-FLU velocity, returning to zero when calls stop. + + Args: + forward: Forward velocity in meters per second. + left: Left velocity in meters per second. + up: Up velocity in meters per second. + yaw_rate: Counter-clockwise yaw rate in radians per second. + """ + timeout = self.config.move_watchdog_timeout_s + if not math.isfinite(timeout) or timeout <= 0.0: + return "move failed: watchdog timeout must be a finite positive number" + + async def _watchdog() -> None: + try: + await asyncio.sleep(timeout) + async with self._move_watchdog_lock: + await self._set_body_velocity(0.0, 0.0, 0.0, 0.0) + except asyncio.CancelledError: + raise + except OffboardError as error: + logger.warning("PX4 rejected move watchdog stop: %s", error) + finally: + if asyncio.current_task() is self._move_watchdog_task: + self._move_watchdog_task = None + + try: + async with self._move_watchdog_lock: + await self._set_body_velocity(forward, left, up, yaw_rate) + await self._cancel_move_watchdog() + self._move_watchdog_task = asyncio.create_task(_watchdog()) + except (ValueError, OffboardError) as error: + return f"move failed: {error}" + return "move command sent" + + @skill(uses=[CAP_MOVEMENT]) + async def goto(self, north: float, east: float, down: float, yaw: float = 0.0) -> str: + """Set a local-NED position and yaw target. + + Args: + north: Target north position in meters. + east: Target east position in meters. + down: Target down position in meters. + yaw: Target clockwise yaw in degrees. + """ + values = (north, east, down, yaw) + if not all(math.isfinite(value) for value in values): + return "goto failed: position and yaw must be finite" + try: + async with self._move_watchdog_lock: + await self._system.offboard.set_position_ned(PositionNedYaw(*values)) + await self._cancel_move_watchdog() + except OffboardError as error: + return f"goto failed: {error}" + return "goto command sent" + + @skill(uses=[CAP_MOVEMENT]) + async def enter_offboard(self) -> str: + """Enter Offboard mode with a zero body-velocity setpoint.""" + await self._cancel_move_watchdog() + try: + await self._system.offboard.set_velocity_body(VelocityBodyYawspeed(0.0, 0.0, 0.0, 0.0)) + await self._system.offboard.start() + except OffboardError as error: + return f"enter offboard failed: {error}" + self._offboard_active = True + return "offboard mode entered" + + @skill(uses=[CAP_MOVEMENT]) + async def exit_offboard(self) -> str: + """Exit Offboard mode.""" + try: + await self._system.offboard.stop() + except OffboardError as error: + return f"exit offboard failed: {error}" + await self._cancel_move_watchdog() + self._offboard_active = False + return "offboard mode exited" + + @skill(uses=[CAP_MOVEMENT]) + async def hold(self) -> str: + """Exit Offboard control and command the PX4 Hold flight mode.""" + try: + await self._system.action.hold() + except ActionError as error: + return f"hold failed: {error}" + self._offboard_active = False + return "hold mode entered" + + @skill(uses=[CAP_MOVEMENT]) + async def hover(self) -> str: + """Hold the latest local-NED position and yaw target.""" + position = self._position_ned + yaw = self._yaw_deg + if position is None or yaw is None: + return "hover failed: local NED position and yaw are unavailable" + try: + async with self._move_watchdog_lock: + await self._system.offboard.set_position_ned(PositionNedYaw(*position, yaw)) + await self._cancel_move_watchdog() + except OffboardError as error: + return f"hover failed: {error}" + return "hover command sent" + + @rpc + def start(self) -> None: + """Connect MAVSDK and start telemetry before accepting stream input.""" + loop = self._loop + if loop is None or not loop.is_running(): + raise RuntimeError("FlightController event loop is not running") + started = False + try: + asyncio.run_coroutine_threadsafe(self._start_mavsdk(), loop).result() + super().start() + started = True + finally: + if not started: + asyncio.run_coroutine_threadsafe(self._stop_mavsdk(), loop).result() + + @rpc + def stop(self) -> None: + """Stop telemetry and MAVSDK before closing the Module event loop.""" + loop = self._loop + try: + if loop is not None: + asyncio.run_coroutine_threadsafe(self._stop_mavsdk(), loop).result() + finally: + super().stop() + + async def _start_mavsdk(self) -> None: + system = System() + await system.connect(system_address=self.config.connection_url) + try: + await asyncio.wait_for( + self._wait_until_connected(system), + timeout=self.config.connection_timeout_s, + ) + except TimeoutError as error: + raise MavsdkConnectionTimeoutError( + connection_url=self.config.connection_url, + timeout_s=self.config.connection_timeout_s, + ) from error + self._system = system + self._connected = True + self._telemetry_tasks = [ + asyncio.create_task(self._watch_armed()), + asyncio.create_task(self._watch_in_air()), + asyncio.create_task(self._watch_flight_mode()), + asyncio.create_task(self._watch_position_velocity_ned()), + asyncio.create_task(self._watch_attitude_euler()), + ] + + @staticmethod + async def _wait_until_connected(system: System) -> None: + async for state in system.core.connection_state(): + if state.is_connected: + return + + @rpc + def get_status(self) -> dict[str, bool | str | None]: + """Return the latest MAVSDK connection and telemetry state.""" + return { + "connected": self._connected, + "armed": self._armed, + "in_air": self._in_air, + "flight_mode": self._flight_mode, + } + + async def handle_cmd_vel(self, twist: Twist) -> None: + try: + async with self._move_watchdog_lock: + await self._set_body_velocity( + twist.linear.x, twist.linear.y, twist.linear.z, twist.angular.z + ) + await self._cancel_move_watchdog() + except (ValueError, OffboardError) as error: + logger.warning("PX4 rejected cmd_vel: %s", error) + + async def handle_odometry(self, message: Odometry) -> None: + try: + estimate = _build_vision_position_estimate(message) + await self._system.mocap.set_vision_position_estimate(estimate) + except (_InvalidExternalVisionSampleError, MocapError) as error: + logger.warning("External vision odometry rejected: %s", error) + + async def _watch_armed(self) -> None: + async for value in self._system.telemetry.armed(): + self._armed = value + + async def _watch_in_air(self) -> None: + async for value in self._system.telemetry.in_air(): + self._in_air = value + + async def _watch_flight_mode(self) -> None: + async for value in self._system.telemetry.flight_mode(): + if value is None: + continue + self._flight_mode = value.name + self._offboard_active = value.name == "OFFBOARD" + + async def _watch_position_velocity_ned(self) -> None: + async for value in self._system.telemetry.position_velocity_ned(): + position = value.position + self._position_ned = (position.north_m, position.east_m, position.down_m) + + async def _watch_attitude_euler(self) -> None: + async for value in self._system.telemetry.attitude_euler(): + self._yaw_deg = value.yaw_deg + + async def _stop_mavsdk(self) -> None: + await self._cancel_move_watchdog() + system = getattr(self, "_system", None) + if system is None: + return + for task in self._telemetry_tasks: + task.cancel() + if self._telemetry_tasks: + await asyncio.gather(*self._telemetry_tasks, return_exceptions=True) + self._telemetry_tasks.clear() + if self._offboard_active: + try: + await system.offboard.stop() + except OffboardError as error: + logger.warning("Failed to stop Offboard control during shutdown: %s", error) + self._connected = False + self._offboard_active = False + del self._system + self._armed = None + self._in_air = None + self._flight_mode = None diff --git a/dimos/robot/drone/px4/gstreamer_tee_camera.py b/dimos/robot/drone/px4/gstreamer_tee_camera.py new file mode 100644 index 0000000000..e3acbdccd2 --- /dev/null +++ b/dimos/robot/drone/px4/gstreamer_tee_camera.py @@ -0,0 +1,242 @@ +# Copyright 2026 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. + +"""GStreamer tee camera with raw BGR and Annex-B H.264 outputs.""" + +from enum import Enum +import logging +from threading import Thread, current_thread +from typing import Any, ClassVar + +import gi # type: ignore[import-not-found,import-untyped] +import numpy as np +from pydantic import Field, model_validator +from typing_extensions import Self + +gi.require_version("Gst", "1.0") +gi.require_version("GstApp", "1.0") +from gi.repository import GLib, Gst # type: ignore[import-not-found,import-untyped] + +from dimos.core.core import rpc +from dimos.core.module import Module, ModuleConfig +from dimos.core.stream import Out +from dimos.msgs.foxglove_msgs.CompressedVideo import CompressedVideo +from dimos.msgs.sensor_msgs.Image import Image, ImageFormat +from dimos.utils.logging_config import setup_logger + +Gst.init(None) +logger = setup_logger(level=logging.INFO) + + +class GstInputFormat(str, Enum): + RAW = "raw" + H264 = "h264" + + +class GstEncoder(str, Enum): + NVV4L2 = "nvv4l2h264enc" + X264 = "x264enc" + + +DEFAULT_INPUT_PIPELINE = ( + "v4l2src device=/dev/video0 ! video/x-raw,width=640,height=480,framerate=30/1" +) + + +class GsTeeCameraConfig(ModuleConfig): + input_pipeline: str = DEFAULT_INPUT_PIPELINE + input_format: GstInputFormat = GstInputFormat.RAW + encoder: GstEncoder = GstEncoder.NVV4L2 + bitrate: int = Field(default=4_000_000, ge=1_000) + gop: int = Field(default=30, gt=0) + + @model_validator(mode="after") + def reject_unused_encoder_options(self) -> Self: + unused = {"encoder", "bitrate", "gop"} & self.model_fields_set + if self.input_format is GstInputFormat.H264 and unused: + names = ", ".join(sorted(unused)) + raise ValueError(f"{names} cannot be set when input_format is h264") + return self + + +class GsTeeCamera(Module): + """Own one live source pipeline and publish its raw and encoded tee branches.""" + + config: GsTeeCameraConfig + dedicated_worker: ClassVar[bool] = True + + color_image: Out[Image] + video_h264: Out[CompressedVideo] + + def __init__(self, **kwargs: Any) -> None: + kwargs.setdefault("frame_id", "camera_optical") + super().__init__(**kwargs) + self._pipeline: Any = None + self._bus: Any = None + self._main_loop: Any = None + self._main_loop_thread: Thread | None = None + + @rpc + def start(self) -> None: + """Create, bind, and start the one owned tee pipeline.""" + if self._pipeline is not None: + raise RuntimeError("camera is already running") + super().start() + + if self.config.input_format is GstInputFormat.H264: + raw_branch = ( + "t. ! queue max-size-buffers=2 leaky=downstream ! avdec_h264 ! " + "videoconvert ! video/x-raw,format=BGR ! appsink name=raw_sink " + "emit-signals=true sync=false max-buffers=1 drop=true" + ) + h264_branch = ( + "t. ! queue max-size-buffers=2 ! " + "video/x-h264,stream-format=byte-stream,alignment=au ! appsink " + "name=h264_sink emit-signals=true sync=false max-buffers=1 drop=false" + ) + else: + raw_branch = ( + "t. ! queue max-size-buffers=2 leaky=downstream ! videoconvert ! " + "video/x-raw,format=BGR ! appsink name=raw_sink emit-signals=true " + "sync=false max-buffers=1 drop=true" + ) + match self.config.encoder: + case GstEncoder.NVV4L2: + encoder_input = "nvvidconv ! video/x-raw(memory:NVMM),format=NV12" + encoder = ( + f"nvv4l2h264enc bitrate={self.config.bitrate} " + f"iframeinterval={self.config.gop} idrinterval={self.config.gop} " + "insert-sps-pps=true maxperf-enable=true" + ) + case GstEncoder.X264: + encoder_input = "videoconvert" + encoder = ( + f"x264enc bitrate={self.config.bitrate // 1000} " + f"key-int-max={self.config.gop} bframes=0 tune=zerolatency" + ) + h264_branch = ( + f"t. ! queue max-size-buffers=2 ! {encoder_input} ! {encoder} ! " + "h264parse config-interval=-1 ! " + "video/x-h264,stream-format=byte-stream,alignment=au ! appsink " + "name=h264_sink emit-signals=true sync=false max-buffers=1 drop=false" + ) + + pipeline = Gst.parse_launch( + f"{self.config.input_pipeline} ! tee name=t {raw_branch} {h264_branch}" + ) + raw_sink = pipeline.get_by_name("raw_sink") + h264_sink = pipeline.get_by_name("h264_sink") + if raw_sink is None or h264_sink is None: + pipeline.set_state(Gst.State.NULL) + raise RuntimeError("required appsinks are missing") + raw_sink.connect("new-sample", self._on_raw_sample) + h264_sink.connect("new-sample", self._on_h264_sample) + bus = pipeline.get_bus() + bus.add_signal_watch() + bus.connect("message", self._on_bus_message) + self._pipeline = pipeline + self._bus = bus + self._main_loop = GLib.MainLoop() + self._main_loop_thread = Thread( + target=self._main_loop.run, + name="px4-gstreamer-bus", + daemon=True, + ) + self._main_loop_thread.start() + if pipeline.set_state(Gst.State.PLAYING) == Gst.StateChangeReturn.FAILURE: + self._release_pipeline() + raise RuntimeError("pipeline failed to enter PLAYING") + + @rpc + def stop(self) -> None: + """Release the owned Gst pipeline before closing Module resources.""" + self._release_pipeline() + super().stop() + + def _on_raw_sample(self, sink: Any) -> Any: + sample = sink.emit("pull-sample") + if sample is None: + return Gst.FlowReturn.ERROR + buffer = sample.get_buffer() + mapped, mapping = buffer.map(Gst.MapFlags.READ) + if not mapped: + return Gst.FlowReturn.ERROR + try: + caps = sample.get_caps().get_structure(0) + width = int(caps.get_value("width")) + height = int(caps.get_value("height")) + pts = 0 if buffer.pts == Gst.CLOCK_TIME_NONE else int(buffer.pts) + self.color_image.publish( + Image( + data=np.frombuffer(mapping.data, dtype=np.uint8) + .reshape((height, width, 3)) + .copy(), + format=ImageFormat.BGR, + frame_id=self.frame_id, + ts=pts / 1_000_000_000, + ) + ) + finally: + buffer.unmap(mapping) + return Gst.FlowReturn.OK + + def _on_h264_sample(self, sink: Any) -> Any: + sample = sink.emit("pull-sample") + if sample is None: + return Gst.FlowReturn.ERROR + buffer = sample.get_buffer() + mapped, mapping = buffer.map(Gst.MapFlags.READ) + if not mapped: + return Gst.FlowReturn.ERROR + try: + pts = 0 if buffer.pts == Gst.CLOCK_TIME_NONE else int(buffer.pts) + self.video_h264.publish( + CompressedVideo( + data=bytes(mapping.data), + format="h264", + frame_id=self.frame_id, + ts=pts / 1_000_000_000, + ) + ) + finally: + buffer.unmap(mapping) + return Gst.FlowReturn.OK + + def _on_bus_message(self, _bus: Any, message: Any) -> None: + if message.type == Gst.MessageType.ERROR: + error, debug = message.parse_error() + logger.error(f"GStreamer pipeline error: {error}: {debug}") + elif message.type == Gst.MessageType.EOS: + logger.info("GStreamer pipeline reached end of stream") + else: + return + self._release_pipeline() + + def _release_pipeline(self) -> None: + pipeline = self._pipeline + self._pipeline = None + bus = self._bus + self._bus = None + if bus is not None: + bus.remove_signal_watch() + if pipeline is not None: + pipeline.set_state(Gst.State.NULL) + main_loop = self._main_loop + self._main_loop = None + if main_loop is not None: + main_loop.quit() + thread = self._main_loop_thread + self._main_loop_thread = None + if thread is not None and thread is not current_thread(): + thread.join() diff --git a/dimos/robot/drone/px4/mid360_mount_tf.py b/dimos/robot/drone/px4/mid360_mount_tf.py new file mode 100644 index 0000000000..cbe2f07fd4 --- /dev/null +++ b/dimos/robot/drone/px4/mid360_mount_tf.py @@ -0,0 +1,57 @@ +# Copyright 2026 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. + +"""Static MID360-to-body mount transform for the MAVSDK drone. + +Point-LIO publishes the dynamic ``odom -> mid360_link`` edge. This module +attaches ``base_link`` below that sensor pose using the inverse of +:data:`BASE_TO_MID360`. + +Published frame relationship:: + + odom ─── mid360_link ─── base_link + (Point-LIO) (this publisher) + +The fixed transform is periodically re-published by :class:`StaticTfPublisher`. +""" + +from typing import Final + +import numpy as np +from typing_extensions import override + +from dimos.msgs.geometry_msgs.Transform import Transform +from dimos.protocol.tf.static_tf_publisher import StaticTfPublisher + +BASE_TO_MID360: Final[Transform] = Transform.from_matrix( + np.array( + ( + (0.9659258, 0.0, -0.2588190, 0.010625), + (0.0, 1.0, 0.0, 0.023290), + (0.2588190, 0.0, 0.9659258, 0.061153), + (0.0, 0.0, 0.0, 1.0), + ) + ), + frame_id="base_link", + child_frame_id="mid360_link", +) + + +class Mid360MountStaticTf(StaticTfPublisher): + """Publish the calibrated ``mid360_link -> base_link`` static transform.""" + + @override + def transforms(self) -> list[Transform]: + """Return the inverse mount with ``mid360_link`` as parent.""" + return [BASE_TO_MID360.inverse()] diff --git a/dimos/robot/drone/px4/test_flight_control.py b/dimos/robot/drone/px4/test_flight_control.py new file mode 100644 index 0000000000..54ee1f7915 --- /dev/null +++ b/dimos/robot/drone/px4/test_flight_control.py @@ -0,0 +1,446 @@ +# Copyright 2026 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 asyncio +from collections.abc import AsyncIterator, Iterator +import math +from threading import Event +from types import SimpleNamespace +from typing import Any +from unittest.mock import AsyncMock, MagicMock + +import numpy as np +import pytest +from pytest_mock import MockerFixture + +from dimos.msgs.geometry_msgs.Twist import Twist +from dimos.msgs.nav_msgs.Odometry import Odometry +from dimos.robot.drone.px4 import flight_control +from dimos.robot.drone.px4.flight_control import FlightController + + +def _mock_system() -> MagicMock: + return MagicMock( + action=MagicMock( + arm=AsyncMock(), + disarm=AsyncMock(), + set_takeoff_altitude=AsyncMock(), + takeoff=AsyncMock(), + land=AsyncMock(), + hold=AsyncMock(), + ), + offboard=MagicMock( + set_velocity_body=AsyncMock(), + set_position_ned=AsyncMock(), + start=AsyncMock(), + stop=AsyncMock(), + ), + mocap=MagicMock(set_vision_position_estimate=AsyncMock()), + telemetry=MagicMock(), + ) + + +@pytest.fixture(scope="module") +def flight_controller() -> Iterator[FlightController]: + module = FlightController() + yield module + module.__dict__.pop("_system", None) + module.stop() + + +@pytest.fixture +def controller(flight_controller: FlightController) -> Iterator[FlightController]: + _run(flight_controller, flight_controller._cancel_move_watchdog()) + flight_controller.config.move_watchdog_timeout_s = 0.5 + flight_controller._system = _mock_system() + flight_controller._connected = False + flight_controller._armed = None + flight_controller._in_air = None + flight_controller._flight_mode = None + flight_controller._offboard_active = False + flight_controller._position_ned = None + flight_controller._yaw_deg = None + flight_controller._telemetry_tasks.clear() + yield flight_controller + _run(flight_controller, flight_controller._cancel_move_watchdog()) + for task in flight_controller._telemetry_tasks: + if not task.done(): + task.cancel() + flight_controller._telemetry_tasks.clear() + flight_controller.__dict__.pop("_system", None) + + +def _mavsdk_error(error_type: type[Exception]) -> Exception: + result = SimpleNamespace(result="DENIED", result_str="denied") + return error_type(result, "test") + + +def _odometry() -> Odometry: + message = Odometry(ts=12.3456789, frame_id="odom", child_frame_id="mid360_link") + message.pose.position.x, message.pose.position.y, message.pose.position.z = (3.0, -2.0, 1.5) + message.pose.orientation.w = 1.0 + message.pose.covariance = np.eye(6).reshape(-1) + return message + + +def _run(controller: FlightController, coroutine: Any) -> Any: + assert controller._loop is not None + return asyncio.run_coroutine_threadsafe(coroutine, controller._loop).result() + + +def test_pointlio_odometry_builds_mavsdk_vision_estimate() -> None: + estimate = flight_control._build_vision_position_estimate(_odometry()) + + assert estimate.time_usec == 12_345_679 + assert ( + estimate.position_body.x_m, + estimate.position_body.y_m, + estimate.position_body.z_m, + ) == pytest.approx((2.973909, 2.02329, -1.443681), abs=1e-6) + assert len(estimate.pose_covariance.covariance_matrix) == 21 + + +def test_invalid_vision_covariance_uses_unknown_sentinel() -> None: + covariance = flight_control._transform_pose_covariance_to_frd((1.0,), (1.0, 0.0, 0.0, 0.0)) + + assert len(covariance) == 1 + assert math.isnan(covariance[0]) + + +def test_external_vision_rejects_invalid_samples() -> None: + message = _odometry() + message.frame_id = "map" + with pytest.raises( + flight_control._InvalidExternalVisionSampleError, match="odom -> mid360_link" + ): + flight_control._build_vision_position_estimate(message) + + message = _odometry() + message.ts = 0.0 + with pytest.raises(flight_control._InvalidExternalVisionSampleError, match="timestamp"): + flight_control._build_vision_position_estimate(message) + + message = _odometry() + message.pose.orientation.w = 2.0 + with pytest.raises( + flight_control._InvalidExternalVisionSampleError, match="orientation quaternion" + ): + flight_control._build_vision_position_estimate(message) + + message = _odometry() + message.pose.position.x = math.inf + with pytest.raises( + flight_control._InvalidExternalVisionSampleError, match="position must be finite" + ): + flight_control._build_vision_position_estimate(message) + + +@pytest.mark.parametrize( + ("method", "system_method", "success"), + ( + ("arm", "arm", "arm command sent"), + ("disarm", "disarm", "disarm command sent"), + ("land", "land", "land command sent"), + ("hold", "hold", "hold mode entered"), + ), +) +def test_action_skills_report_success( + controller: FlightController, method: str, system_method: str, success: str +) -> None: + action = getattr(controller._system.action, system_method) + action.return_value = None + + result = getattr(controller, method)() + + assert result == success + action.assert_awaited_once_with() + + +def test_action_skill_reports_mavsdk_error(controller: FlightController) -> None: + controller._system.action.arm.side_effect = _mavsdk_error(flight_control.ActionError) + + result = controller.arm() + + assert result.startswith("arm failed: DENIED") + + +@pytest.mark.parametrize("method", ("land", "hold")) +def test_action_mode_changes_leave_move_watchdog_running( + controller: FlightController, method: str +) -> None: + assert controller.move(forward=1.0) == "move command sent" + watchdog_task = controller._move_watchdog_task + assert watchdog_task is not None + + getattr(controller, method)() + + assert controller._move_watchdog_task is watchdog_task + assert not watchdog_task.done() + + +def test_takeoff_validates_altitude_and_sends_both_commands( + controller: FlightController, +) -> None: + assert controller.takeoff(math.nan) == ( + "takeoff failed: altitude must be a finite positive number" + ) + + result = controller.takeoff(4.5) + + assert result == "takeoff command sent for 4.5 m" + controller._system.action.set_takeoff_altitude.assert_awaited_once_with(4.5) + controller._system.action.takeoff.assert_awaited_once_with() + + +def test_move_converts_flu_velocity_to_mavsdk_frd(controller: FlightController) -> None: + result = controller.move(forward=1.0, left=2.0, up=3.0, yaw_rate=math.pi / 2) + + assert result == "move command sent" + velocity = controller._system.offboard.set_velocity_body.await_args.args[0] + assert ( + velocity.forward_m_s, + velocity.right_m_s, + velocity.down_m_s, + velocity.yawspeed_deg_s, + ) == pytest.approx((1.0, -2.0, -3.0, -90.0)) + assert controller._move_watchdog_task is not None + + +def test_move_replaces_existing_watchdog(controller: FlightController) -> None: + first_result = controller.move(forward=1.0) + first_task = controller._move_watchdog_task + assert first_task is not None + + second_result = controller.move(left=1.0) + + assert first_result == second_result == "move command sent" + assert first_task.cancelled() + assert controller._move_watchdog_task is not None + assert controller._move_watchdog_task is not first_task + + +def test_move_watchdog_stops_velocity(controller: FlightController, mocker: MockerFixture) -> None: + sleep_started = Event() + + async def make_release_event() -> asyncio.Event: + return asyncio.Event() + + release_watchdog = _run(controller, make_release_event()) + + async def expire_watchdog(timeout: float) -> None: + assert timeout == 0.5 + sleep_started.set() + await release_watchdog.wait() + + mocker.patch.object(flight_control.asyncio, "sleep", side_effect=expire_watchdog) + + result = controller.move(forward=1.0) + assert sleep_started.wait(timeout=1.0) + + async def wait_for_watchdog() -> None: + task = controller._move_watchdog_task + assert task is not None + release_watchdog.set() + await task + + _run(controller, wait_for_watchdog()) + + assert result == "move command sent" + velocities = controller._system.offboard.set_velocity_body.await_args_list + assert len(velocities) == 2 + stopped = velocities[-1].args[0] + assert ( + stopped.forward_m_s, + stopped.right_m_s, + stopped.down_m_s, + stopped.yawspeed_deg_s, + ) == (0.0, 0.0, 0.0, 0.0) + assert controller._move_watchdog_task is None + + +def test_move_rejects_non_finite_input(controller: FlightController) -> None: + result = controller.move(forward=math.inf) + + assert result == "move failed: frame conversion inputs must be finite" + controller._system.offboard.set_velocity_body.assert_not_awaited() + + +def test_move_rejects_invalid_watchdog_timeout(controller: FlightController) -> None: + controller.config.move_watchdog_timeout_s = 0.0 + + result = controller.move(forward=1.0) + + assert result == "move failed: watchdog timeout must be a finite positive number" + controller._system.offboard.set_velocity_body.assert_not_awaited() + + +def test_goto_validates_and_sends_ned_target(controller: FlightController) -> None: + assert controller.goto(math.nan, 0.0, 0.0) == ("goto failed: position and yaw must be finite") + + result = controller.goto(1.0, 2.0, -3.0, 45.0) + + assert result == "goto command sent" + target = controller._system.offboard.set_position_ned.await_args.args[0] + assert (target.north_m, target.east_m, target.down_m, target.yaw_deg) == ( + 1.0, + 2.0, + -3.0, + 45.0, + ) + + +def test_offboard_and_hover_follow_latest_telemetry(controller: FlightController) -> None: + assert controller.hover() == ("hover failed: local NED position and yaw are unavailable") + + assert controller.move(forward=1.0) == "move command sent" + enter_watchdog = controller._move_watchdog_task + assert enter_watchdog is not None + assert controller.enter_offboard() == "offboard mode entered" + assert enter_watchdog.cancelled() + assert controller._offboard_active is True + controller._position_ned = (4.0, 5.0, -6.0) + controller._yaw_deg = 30.0 + assert controller.hover() == "hover command sent" + assert controller.move(left=1.0) == "move command sent" + exit_watchdog = controller._move_watchdog_task + assert exit_watchdog is not None + assert controller.exit_offboard() == "offboard mode exited" + assert exit_watchdog.cancelled() + assert controller._offboard_active is False + + targets = controller._system.offboard.set_position_ned.await_args_list + target = targets[-1].args[0] + assert (target.north_m, target.east_m, target.down_m, target.yaw_deg) == ( + 4.0, + 5.0, + -6.0, + 30.0, + ) + + +def test_stream_handlers_forward_commands_and_reject_bad_odometry( + controller: FlightController, mocker: MockerFixture +) -> None: + assert controller.move(forward=0.5) == "move command sent" + watchdog_task = controller._move_watchdog_task + assert watchdog_task is not None + + twist = Twist() + twist.linear.x, twist.linear.y, twist.linear.z, twist.angular.z = (1.0, 2.0, 3.0, 0.5) + _run(controller, controller.handle_cmd_vel(twist)) + assert watchdog_task.cancelled() + assert controller._move_watchdog_task is None + assert controller._system.offboard.set_velocity_body.await_count == 2 + + invalid_odometry = mocker.Mock(frame_id="map", child_frame_id="mid360_link") + _run(controller, controller.handle_odometry(invalid_odometry)) + controller._system.mocap.set_vision_position_estimate.assert_not_awaited() + + +async def _values(*items: Any) -> AsyncIterator[Any]: + for item in items: + yield item + + +def test_telemetry_watchers_update_status(controller: FlightController) -> None: + controller._system.telemetry.armed.return_value = _values(True) + controller._system.telemetry.in_air.return_value = _values(False) + controller._system.telemetry.flight_mode.return_value = _values( + None, SimpleNamespace(name="OFFBOARD") + ) + controller._system.telemetry.position_velocity_ned.return_value = _values( + SimpleNamespace(position=SimpleNamespace(north_m=1.0, east_m=2.0, down_m=-3.0)) + ) + controller._system.telemetry.attitude_euler.return_value = _values( + SimpleNamespace(yaw_deg=25.0) + ) + + async def watch_all() -> None: + await asyncio.gather( + controller._watch_armed(), + controller._watch_in_air(), + controller._watch_flight_mode(), + controller._watch_position_velocity_ned(), + controller._watch_attitude_euler(), + ) + + _run( + controller, + watch_all(), + ) + + assert controller.get_status() == { + "connected": False, + "armed": True, + "in_air": False, + "flight_mode": "OFFBOARD", + } + assert controller._offboard_active is True + assert controller._position_ned == (1.0, 2.0, -3.0) + assert controller._yaw_deg == 25.0 + + +def test_start_mavsdk_reports_connection_timeout( + controller: FlightController, mocker: MockerFixture +) -> None: + system = mocker.Mock() + system.connect = mocker.AsyncMock() + mocker.patch.object(flight_control, "System", return_value=system) + mocker.patch.object( + controller, + "_wait_until_connected", + new_callable=mocker.AsyncMock, + side_effect=TimeoutError, + ) + + with pytest.raises(flight_control.MavsdkConnectionTimeoutError) as captured: + _run(controller, controller._start_mavsdk()) + + assert str(captured.value) == ( + "MAVSDK did not connect to serial:///dev/ttyTHS3:921600 within 10 seconds" + ) + system.connect.assert_awaited_once_with(system_address="serial:///dev/ttyTHS3:921600") + + +def test_stop_mavsdk_cancels_telemetry_and_resets_state( + controller: FlightController, +) -> None: + system = controller._system + + async def create_task() -> asyncio.Task[None]: + return asyncio.create_task(asyncio.sleep(60)) + + task = _run(controller, create_task()) + controller._telemetry_tasks = [task] + assert controller.move(forward=1.0) == "move command sent" + watchdog_task = controller._move_watchdog_task + assert watchdog_task is not None + controller._connected = True + controller._offboard_active = True + controller._armed = True + controller._in_air = True + controller._flight_mode = "OFFBOARD" + + _run(controller, controller._stop_mavsdk()) + + assert task.cancelled() + assert watchdog_task.cancelled() + system.offboard.stop.assert_awaited_once_with() + assert controller.get_status() == { + "connected": False, + "armed": None, + "in_air": None, + "flight_mode": None, + } + assert not hasattr(controller, "_system") diff --git a/dimos/robot/drone/px4/test_gstreamer_tee_camera.py b/dimos/robot/drone/px4/test_gstreamer_tee_camera.py new file mode 100644 index 0000000000..bab7618792 --- /dev/null +++ b/dimos/robot/drone/px4/test_gstreamer_tee_camera.py @@ -0,0 +1,215 @@ +# Copyright 2026 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 collections.abc import Iterator +import importlib.util +import sys +from types import SimpleNamespace +from unittest.mock import MagicMock, patch + +import numpy as np +import pytest +from pytest_mock import MockerFixture + +from dimos.msgs.sensor_msgs.Image import ImageFormat + +if importlib.util.find_spec("gi") is None: + gi = MagicMock() + repository = MagicMock() + with patch.dict( + sys.modules, + {"gi": gi, "gi.repository": repository}, + ): + from dimos.robot.drone.px4 import gstreamer_tee_camera as camera +else: + from dimos.robot.drone.px4 import gstreamer_tee_camera as camera + + +@pytest.fixture(scope="module") +def camera_module() -> Iterator[camera.GsTeeCamera]: + instance = camera.GsTeeCamera() + yield instance + instance.stop() + + +@pytest.fixture +def module( + camera_module: camera.GsTeeCamera, + mocker: MockerFixture, +) -> Iterator[camera.GsTeeCamera]: + mocker.patch.object(camera, "Thread") + camera_module.config.input_pipeline = camera.DEFAULT_INPUT_PIPELINE + camera_module.config.input_format = camera.GstInputFormat.RAW + camera_module.config.encoder = camera.GstEncoder.NVV4L2 + camera_module.config.bitrate = 4_000_000 + camera_module.config.gop = 30 + camera_module._release_pipeline() + yield camera_module + camera_module._release_pipeline() + + +@pytest.fixture +def gst_pipeline(mocker: MockerFixture) -> SimpleNamespace: + raw_sink = mocker.Mock() + h264_sink = mocker.Mock() + bus = mocker.Mock() + pipeline = mocker.Mock() + pipeline.get_by_name.side_effect = { + "raw_sink": raw_sink, + "h264_sink": h264_sink, + }.get + pipeline.get_bus.return_value = bus + pipeline.set_state.return_value = camera.Gst.StateChangeReturn.SUCCESS + parse_launch = mocker.patch.object(camera.Gst, "parse_launch", return_value=pipeline) + main_loop = mocker.patch.object(camera.GLib, "MainLoop") + return SimpleNamespace( + pipeline=pipeline, + raw_sink=raw_sink, + h264_sink=h264_sink, + bus=bus, + parse_launch=parse_launch, + main_loop=main_loop, + ) + + +def test_x264_pipeline_uses_configured_bitrate_and_gop( + module: camera.GsTeeCamera, + gst_pipeline: SimpleNamespace, +) -> None: + module.config.encoder = camera.GstEncoder.X264 + module.config.bitrate = 2_000_000 + module.config.gop = 15 + + module.start() + + pipeline_description = gst_pipeline.parse_launch.call_args.args[0] + assert "x264enc bitrate=2000 key-int-max=15" in pipeline_description + + +def test_h264_input_decodes_only_the_raw_branch( + module: camera.GsTeeCamera, gst_pipeline: SimpleNamespace +) -> None: + module.config.input_format = camera.GstInputFormat.H264 + + module.start() + + pipeline_description = gst_pipeline.parse_launch.call_args.args[0] + assert "avdec_h264" in pipeline_description + assert "x264enc" not in pipeline_description + assert "nvv4l2h264enc" not in pipeline_description + gst_pipeline.raw_sink.connect.assert_called_once_with("new-sample", module._on_raw_sample) + gst_pipeline.h264_sink.connect.assert_called_once_with("new-sample", module._on_h264_sample) + + +def test_raw_sample_publishes_bgr_image(module: camera.GsTeeCamera, mocker: MockerFixture) -> None: + pixels = bytes(range(12)) + mapping = SimpleNamespace(data=pixels) + buffer = mocker.Mock(pts=2_500_000_000) + buffer.map.return_value = (True, mapping) + caps = mocker.Mock() + caps.get_value.side_effect = {"width": 2, "height": 2}.get + sample = mocker.Mock() + sample.get_buffer.return_value = buffer + sample.get_caps.return_value.get_structure.return_value = caps + sink = mocker.Mock() + sink.emit.return_value = sample + publish = mocker.patch.object(module.color_image, "publish") + + result = module._on_raw_sample(sink) + + assert result == camera.Gst.FlowReturn.OK + image = publish.call_args.args[0] + assert image.data.tolist() == [ + [[0, 1, 2], [3, 4, 5]], + [[6, 7, 8], [9, 10, 11]], + ] + assert image.format is ImageFormat.BGR + assert image.frame_id == "camera_optical" + assert image.ts == 2.5 + buffer.unmap.assert_called_once_with(mapping) + + +def test_h264_sample_publishes_annex_b_packet( + module: camera.GsTeeCamera, mocker: MockerFixture +) -> None: + mapping = SimpleNamespace(data=b"\x00\x00\x00\x01\x65") + buffer = mocker.Mock(pts=camera.Gst.CLOCK_TIME_NONE) + buffer.map.return_value = (True, mapping) + sample = mocker.Mock() + sample.get_buffer.return_value = buffer + sink = mocker.Mock() + sink.emit.return_value = sample + publish = mocker.patch.object(module.video_h264, "publish") + + result = module._on_h264_sample(sink) + + assert result == camera.Gst.FlowReturn.OK + packet = publish.call_args.args[0] + assert np.array_equal(packet.data, np.array([0, 0, 0, 1, 0x65], dtype=np.uint8)) + assert packet.format == "h264" + assert packet.frame_id == "camera_optical" + assert packet.ts == 0.0 + buffer.unmap.assert_called_once_with(mapping) + + +@pytest.mark.parametrize("callback", ("_on_raw_sample", "_on_h264_sample")) +def test_sample_callback_reports_pull_failure( + module: camera.GsTeeCamera, mocker: MockerFixture, callback: str +) -> None: + sink = mocker.Mock() + sink.emit.return_value = None + + result = getattr(module, callback)(sink) + + assert result == camera.Gst.FlowReturn.ERROR + + +def test_start_failure_releases_pipeline( + module: camera.GsTeeCamera, gst_pipeline: SimpleNamespace +) -> None: + gst_pipeline.pipeline.set_state.return_value = camera.Gst.StateChangeReturn.FAILURE + + with pytest.raises(RuntimeError, match="failed to enter PLAYING"): + module.start() + + assert module._pipeline is None + gst_pipeline.bus.remove_signal_watch.assert_called_once_with() + gst_pipeline.pipeline.set_state.assert_called_with(camera.Gst.State.NULL) + gst_pipeline.main_loop.return_value.quit.assert_called_once_with() + + +@pytest.mark.parametrize("message_type", (camera.Gst.MessageType.ERROR, camera.Gst.MessageType.EOS)) +def test_terminal_bus_message_releases_pipeline( + module: camera.GsTeeCamera, + gst_pipeline: SimpleNamespace, + mocker: MockerFixture, + message_type: object, +) -> None: + module.start() + message = mocker.Mock(type=message_type) + message.parse_error.return_value = ("failure", "debug") + + module._on_bus_message(gst_pipeline.bus, message) + + assert module._pipeline is None + gst_pipeline.bus.remove_signal_watch.assert_called_once_with() + gst_pipeline.pipeline.set_state.assert_called_with(camera.Gst.State.NULL) + + +def test_h264_input_rejects_unused_encoder_options() -> None: + with pytest.raises(ValueError, match="bitrate cannot be set"): + camera.GsTeeCameraConfig( + input_format=camera.GstInputFormat.H264, + bitrate=2_000_000, + ) diff --git a/dimos/robot/test_all_blueprints.py b/dimos/robot/test_all_blueprints.py index 7d3e01f2c0..a498fad908 100644 --- a/dimos/robot/test_all_blueprints.py +++ b/dimos/robot/test_all_blueprints.py @@ -19,7 +19,15 @@ from dimos.robot.get_all_blueprints import get_blueprint_by_name # Optional dependencies that are allowed to be missing -OPTIONAL_DEPENDENCIES = {"pyrealsense2", "pyzed", "geometry_msgs", "turbojpeg", "unitree_sdk2py"} +OPTIONAL_DEPENDENCIES = { + "geometry_msgs", + "gi", + "mavsdk", + "pyrealsense2", + "pyzed", + "turbojpeg", + "unitree_sdk2py", +} OPTIONAL_ERROR_SUBSTRINGS = { "Unable to locate turbojpeg library automatically", "ZED SDK not installed", diff --git a/pyproject.toml b/pyproject.toml index 7c4faf5afa..ce612a7dce 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -338,7 +338,8 @@ mapping = [ # ] drone = [ - "pymavlink" + "mavsdk>=3.17.2", + "pymavlink", ] dds = [ diff --git a/uv.lock b/uv.lock index 399a4a909b..5d4d031246 100644 --- a/uv.lock +++ b/uv.lock @@ -29,7 +29,7 @@ resolution-markers = [ ] [options] -exclude-newer = "2026-08-03T19:08:40.466827Z" +exclude-newer = "0001-01-01T00:00:00Z" # This has no effect and is included for backwards compatibility when using relative exclude-newer values. exclude-newer-span = "P7D" [options.exclude-newer-package] @@ -1741,6 +1741,7 @@ all = [ { name = "lap" }, { name = "manifold3d" }, { name = "matplotlib" }, + { name = "mavsdk" }, { name = "moondream" }, { name = "mujoco" }, { name = "ollama" }, @@ -1829,6 +1830,7 @@ dds = [ { name = "cyclonedds" }, ] drone = [ + { name = "mavsdk" }, { name = "pymavlink" }, ] graspgenx = [ @@ -2234,6 +2236,7 @@ requires-dist = [ { name = "manifold3d", marker = "extra == 'apriltag'", specifier = ">=2.5.0" }, { name = "matplotlib", marker = "extra == 'graspgenx'", specifier = ">=3.7.1" }, { name = "matplotlib", marker = "extra == 'manipulation'", specifier = ">=3.7.1" }, + { name = "mavsdk", marker = "extra == 'drone'", specifier = ">=3.17.2" }, { name = "mcap", marker = "extra == 'unitree-dds'", specifier = ">=1.2.0" }, { name = "moondream", marker = "extra == 'perception'" }, { name = "mujoco", marker = "extra == 'sim'", specifier = ">=3.3.4" }, @@ -5047,6 +5050,26 @@ wheels = [ { url = "https://files.pythonhosted.org/packages/6c/93/e32e79333f0902ba292b996f504f5f06be59587f7d02ab8d5ed1e3066445/maturin-1.13.3-py3-none-win_arm64.whl", hash = "sha256:2389fe92d017cea9d94e521fa0175314a4c52f79a1057b901fbc9f8686ef7d0b", size = 9706562, upload-time = "2026-05-11T07:43:31.743Z" }, ] +[[package]] +name = "mavsdk" +version = "3.17.2" +source = { registry = "https://pypi.org/simple" } +dependencies = [ + { name = "grpcio" }, + { name = "protobuf" }, +] +wheels = [ + { url = "https://files.pythonhosted.org/packages/38/42/bf38eb788ed238dbfe0b54e2a3c8c6e73ca227bf48abf61171c6cf06162f/mavsdk-3.17.2-py3-none-linux_armv6l.whl", hash = "sha256:90ac3826e37dd70f5fdeea4c2d60b3681c22a54e78162d87955313f3deb77083", size = 16588500, upload-time = "2026-07-22T08:19:46.616Z" }, + { url = "https://files.pythonhosted.org/packages/45/9e/567b2957c1d12e15edf0837fbfcdbd43f139ca32490d848a095bbc55c3f3/mavsdk-3.17.2-py3-none-linux_armv7l.whl", hash = "sha256:1aa441c7c290c4e1df5cb43376aa776194ea80004ef156826dd60c5d8b13f2c0", size = 16588546, upload-time = "2026-07-22T08:19:43.234Z" }, + { url = "https://files.pythonhosted.org/packages/9c/91/2005f11804d277a4b599e97bef9c5021c949079ac2c992e5d4f44e304e3f/mavsdk-3.17.2-py3-none-macosx_13_0_x86_64.whl", hash = "sha256:24c40b9f9fa0ce5cea4c05d300cafbb21dfc1483da7518de77c15356112f2c97", size = 12221440, upload-time = "2026-07-22T08:19:53.168Z" }, + { url = "https://files.pythonhosted.org/packages/10/38/da962106d56d68e9290390ec7e1fbfa6d6914580c11673a89d0a8406241d/mavsdk-3.17.2-py3-none-macosx_14_0_arm64.whl", hash = "sha256:0247c9dcc0d4390b8734358ded178764efa4b83c304cb2f77162758b29cd6f57", size = 11684503, upload-time = "2026-07-22T08:19:48.707Z" }, + { url = "https://files.pythonhosted.org/packages/73/b6/72dc4499cb7172665c1e0c3af0b18f5d0ca0160b810c1e05ea6668dd5488/mavsdk-3.17.2-py3-none-manylinux1_x86_64.manylinux2014_x86_64.manylinux_2_17_x86_64.manylinux_2_5_x86_64.whl", hash = "sha256:78ac2402306022bb6000e1fa02dfe4af7a634c803d4ca90673b1c7918a23c937", size = 15246122, upload-time = "2026-07-22T08:20:01.196Z" }, + { url = "https://files.pythonhosted.org/packages/c0/b3/2b177c2350bbc0785bc3a74f770cfc7cdc134128ad566a4f759631b4e549/mavsdk-3.17.2-py3-none-manylinux2014_aarch64.whl", hash = "sha256:99e58f26705127986fa5e1358e3688aa4e4f8d006e719d2d39179d03368039f8", size = 18233585, upload-time = "2026-07-22T08:19:47.112Z" }, + { url = "https://files.pythonhosted.org/packages/fb/3e/843a6bba46b3d442b870c02186003c27c1f7cbe57d9abcf4df35f855f6ba/mavsdk-3.17.2-py3-none-win32.whl", hash = "sha256:a5bb3618b044820c679f62a4d2b5b1fd4029c2145f53fd3ed80a62bf6768e8e2", size = 14342433, upload-time = "2026-07-22T08:20:16.778Z" }, + { url = "https://files.pythonhosted.org/packages/47/4e/6bbd365a6119be77d93d7d61d7f3bb54faa8cc026ddc4471fe204b652e80/mavsdk-3.17.2-py3-none-win_amd64.whl", hash = "sha256:b613775dbf9324b542e29814f530a5e361cad0234304cb48fe56da4054642fc1", size = 14342433, upload-time = "2026-07-22T08:20:23.647Z" }, + { url = "https://files.pythonhosted.org/packages/6b/81/209201c6e2420cc27c377b3bc6e38b7880af19ba8e93f15e082cd7554705/mavsdk-3.17.2-py3-none-win_arm64.whl", hash = "sha256:727b2047e326d64e698bfccb8ca77f7eaada0d4b4d562b57d7e7ac55fa579ac1", size = 14342433, upload-time = "2026-07-22T08:20:10.977Z" }, +] + [[package]] name = "mcap" version = "1.3.1"