diff --git a/README.md b/README.md index 10ce82d..eecaded 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,9 @@ # Drone Landing Simulation (GPS-Denied) -A GPS-denied drone landing simulation using relative sensors (IMU, altimeter, camera, landing pad detection) with **PyBullet** and **Gazebo** simulators. +A GPS-denied drone landing simulation using relative sensors (IMU, altimeter, camera, landing pad detection) with multiple backends: +- **PyBullet** - Lightweight physics simulation +- **Gazebo** - Full robotics simulator +- **ArduPilot SITL** - Realistic flight controller with MAVProxy ## Quick Start @@ -37,6 +40,24 @@ ros2 launch gazebo/launch/drone_landing.launch.py python run_gazebo.py --pattern circular --speed 0.3 ``` +### ArduPilot SITL + Gazebo (Three Terminals - Realistic Flight Controller) + +**Terminal 1 - Launch Gazebo with ArduPilot world:** +```bash +ros2 launch gazebo/launch/ardupilot_drone.launch.py +``` + +**Terminal 2 - Start ArduPilot SITL:** +```bash +cd ~/ardupilot +sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --console +``` + +**Terminal 3 - Run MAVLink Bridge + Controllers:** +```bash +python run_ardupilot.py --no-sitl --pattern circular +``` + ## Installation | Platform | Command | @@ -53,6 +74,7 @@ python run_gazebo.py --pattern circular --speed 0.3 | Standalone | ✅ | ✅ | ✅ | ✅ | ✅ | | PyBullet + ROS 2 | ✅ | ⚠️ | ❌ | ❌ | ✅ | | Gazebo + ROS 2 | ✅ | ⚠️ | ❌ | ❌ | ✅ | +| ArduPilot + Gazebo | ✅ | ⚠️ | ❌ | ❌ | ✅ | ## Files @@ -62,6 +84,8 @@ python run_gazebo.py --pattern circular --speed 0.3 | `simulation_host.py` | PyBullet simulator server | | `run_bridge.py` | PyBullet bridge + Controllers | | `run_gazebo.py` | Gazebo bridge + Controllers | +| `run_ardupilot.py` | **ArduPilot SITL** + MAVLink bridge | +| `mavlink_bridge.py` | MAVLink ↔ ROS 2 bridge | | `config.py` | **Configuration file** (edit to customize) | | `drone_controller.py` | **Your landing algorithm** (edit this!) | | `rover_controller.py` | Moving landing pad controller | @@ -111,6 +135,7 @@ Options: | [Architecture](docs/architecture.md) | System components diagram | | [Gazebo Guide](docs/gazebo.md) | Gazebo-specific instructions | | [PyBullet Guide](docs/pybullet.md) | PyBullet-specific instructions | +| [ArduPilot Guide](docs/ardupilot.md) | **ArduPilot SITL + MAVProxy** | | [Protocol](docs/protocol.md) | Sensor data formats | | [Drone Guide](docs/drone_guide.md) | Landing algorithm guide | diff --git a/config.py b/config.py index 603f8fe..7055a6d 100644 --- a/config.py +++ b/config.py @@ -101,3 +101,49 @@ CONTROLLER = { # Control rate "rate": 50, # Control loop frequency (Hz) } + +# ============================================================================= +# ARDUPILOT / MAVLINK CONFIGURATION +# ============================================================================= + +ARDUPILOT = { + # Vehicle type + "vehicle": "ArduCopter", # ArduCopter, ArduPlane, APMrover2 + "frame": "gazebo-iris", # Gazebo model frame + + # SITL connection + "sitl_host": "127.0.0.1", # SITL host address + "sitl_port": 5760, # SITL TCP port + + # MAVProxy output (for MAVLink bridge) + "mavproxy_host": "127.0.0.1", + "mavproxy_port": 14550, # MAVProxy UDP output + + # Gazebo plugin connection (JSON interface) + "gazebo_fdm_port_in": 9002, # Port for motor commands + "gazebo_fdm_port_out": 9003, # Port for sensor data + + # Arming requirements (for simulation, can be relaxed) + "require_gps": False, # GPS required for arming + "require_ekf": True, # EKF required for arming +} + +MAVLINK = { + # MAVLink system IDs + "system_id": 1, # Our system ID + "component_id": 191, # MAV_COMP_ID_MISSIONPLANNER + + # Target system (usually the autopilot) + "target_system": 1, + "target_component": 1, + + # Connection timeout (seconds) + "heartbeat_timeout": 5.0, + "connection_timeout": 30.0, + + # Data stream rates (Hz) + "stream_rate_position": 50, + "stream_rate_attitude": 50, + "stream_rate_raw_sensors": 50, + "stream_rate_extended_status": 5, +} diff --git a/docs/architecture.md b/docs/architecture.md index 57aa553..03060dc 100644 --- a/docs/architecture.md +++ b/docs/architecture.md @@ -62,6 +62,34 @@ Data flow: - DroneController receives telemetry, publishes to `/cmd_vel` - GazeboBridge forwards to `/drone/cmd_vel` → Gazebo moves drone +### 4. ArduPilot SITL + Gazebo Mode (Three Terminals, Linux/WSL2) + +``` +Terminal 1 Terminal 2 Terminal 3 +┌──────────────┐ ┌─────────────────┐ ┌────────────────────────┐ +│ Gazebo + │ │ ArduPilot SITL │ │ run_ardupilot.py │ +│ ArduPilot │◄──►│ sim_vehicle.py │ │ ┌──────────────────┐ │ +│ Plugin │JSON│ + MAVProxy │◄───►│ │ MAVLinkBridge │ │ +│ │ │ │ UDP │ │ DroneController │ │ +│ ardupilot_ │ │ Flight Control │ │ │ RoverController │ │ +│ drone.sdf │ │ + GCS │ │ └──────────────────┘ │ +└──────────────┘ └─────────────────┘ └────────────────────────┘ +``` + +Data flow: +- ArduPilot SITL sends motor commands → Gazebo plugin controls drone +- Gazebo plugin sends sensor data → ArduPilot SITL for state estimation +- MAVProxy outputs telemetry → MAVLinkBridge converts to ROS telemetry +- DroneController receives telemetry, publishes velocity commands +- MAVLinkBridge sends MAVLink commands → ArduPilot SITL executes + +Key differences from simple Gazebo mode: +- Full ArduPilot flight controller (EKF, stabilization, failsafes) +- Real MAVLink protocol for commands and telemetry +- Support for all ArduPilot flight modes (GUIDED, LAND, etc.) +- Arming checks and safety features +- Compatible with ground control stations (QGroundControl, Mission Planner) + ## Components | File | Description | @@ -71,12 +99,16 @@ Data flow: | `simulation_host.py` | PyBullet physics server (UDP) | | `run_bridge.py` | PyBullet bridge + controllers | | `run_gazebo.py` | Gazebo bridge + controllers | +| `run_ardupilot.py` | **ArduPilot SITL** + MAVLink bridge | +| `mavlink_bridge.py` | MAVLink ↔ ROS 2 bridge | | `drone_controller.py` | **Your landing algorithm** | | `rover_controller.py` | Moving landing pad | | `ros_bridge.py` | ROS-UDP bridge (used by run_bridge.py) | | `gazebo_bridge.py` | Gazebo-ROS bridge (used by run_gazebo.py) | | `gazebo/launch/drone_landing.launch.py` | ROS 2 launch file for Gazebo | -| `gazebo/worlds/drone_landing.sdf` | Gazebo world with drone + rover | +| `gazebo/launch/ardupilot_drone.launch.py` | ROS 2 launch file for ArduPilot | +| `gazebo/worlds/drone_landing.sdf` | Gazebo world with simple velocity control | +| `gazebo/worlds/ardupilot_drone.sdf` | Gazebo world with ArduPilot plugin | ## ROS 2 Topics diff --git a/docs/ardupilot.md b/docs/ardupilot.md new file mode 100644 index 0000000..922c369 --- /dev/null +++ b/docs/ardupilot.md @@ -0,0 +1,285 @@ +# ArduPilot SITL + Gazebo Integration + +This guide explains how to run the drone simulation with ArduPilot Software-In-The-Loop (SITL) and MAVProxy, providing a realistic flight controller stack. + +## Overview + +The ArduPilot integration replaces the simple velocity control with a full ArduPilot flight stack: + +``` +┌──────────────────┐ ┌─────────────────┐ ┌──────────────────┐ +│ ArduPilot SITL │◄───►│ Gazebo + Plugin │◄───►│ MAVLink Bridge │ +│ (Flight Control)│ JSON│ (Physics Sim) │ ROS │ + Controllers │ +└──────────────────┘ └─────────────────┘ └──────────────────┘ + ▲ │ + │ UDP │ + │ ▼ +┌──────────────────┐ ┌──────────────────┐ +│ MAVProxy │◄────────────────────────────►│ DroneController │ +│ (GCS) │ MAVLink Commands │ (Your Algorithm) │ +└──────────────────┘ └──────────────────┘ +``` + +## Components + +| Component | Description | +|-----------|-------------| +| **ArduPilot SITL** | Full autopilot firmware running in simulation | +| **ardupilot_gazebo** | Plugin connecting Gazebo physics to ArduPilot | +| **MAVProxy** | Ground Control Station for monitoring/commands | +| **MAVLink Bridge** | ROS 2 node bridging MAVLink ↔ ROS topics | +| **Drone Controller** | Your landing algorithm | + +## Prerequisites + +### 1. ArduPilot SITL + +Install ArduPilot development environment: + +```bash +# Ubuntu/Debian +git clone https://github.com/ArduPilot/ardupilot.git ~/ardupilot +cd ~/ardupilot +git submodule update --init --recursive +Tools/environment_install/install-prereqs-ubuntu.sh -y +. ~/.profile + +# Set environment +echo 'export PATH=$PATH:$HOME/ardupilot/Tools/autotest' >> ~/.bashrc +echo 'export ARDUPILOT_HOME=$HOME/ardupilot' >> ~/.bashrc +source ~/.bashrc +``` + +### 2. ArduPilot Gazebo Plugin + +Install the ardupilot_gazebo plugin: + +```bash +# For Gazebo Garden/Harmonic +git clone https://github.com/ArduPilot/ardupilot_gazebo.git ~/ardupilot_gazebo +cd ~/ardupilot_gazebo +mkdir build && cd build +cmake .. -DCMAKE_BUILD_TYPE=Release +make -j4 + +# Add to Gazebo plugin path +echo 'export GZ_SIM_SYSTEM_PLUGIN_PATH=$HOME/ardupilot_gazebo/build:$GZ_SIM_SYSTEM_PLUGIN_PATH' >> ~/.bashrc +echo 'export GZ_SIM_RESOURCE_PATH=$HOME/ardupilot_gazebo/models:$HOME/ardupilot_gazebo/worlds:$GZ_SIM_RESOURCE_PATH' >> ~/.bashrc +source ~/.bashrc +``` + +### 3. pymavlink + +```bash +pip install pymavlink +``` + +## Quick Start + +### Option 1: Integrated Launch (Recommended) + +This starts everything together: + +```bash +# Terminal 1: Start Gazebo +ros2 launch gazebo/launch/ardupilot_drone.launch.py + +# Terminal 2: Start SITL +cd ~/ardupilot +sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --console --map + +# Terminal 3: Run bridge + controllers +python run_ardupilot.py --no-sitl --pattern circular +``` + +### Option 2: Manual Setup + +```bash +# Terminal 1: Start Gazebo world +gz sim -r gazebo/worlds/ardupilot_drone.sdf + +# Terminal 2: Start ArduPilot SITL +cd ~/ardupilot +sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --console + +# Terminal 3: Run MAVLink bridge + controllers +python run_ardupilot.py --no-sitl +``` + +### Option 3: Full Automatic + +```bash +# Starts everything (requires SITL installed) +python run_ardupilot.py --pattern circular --console --map +``` + +## Flight Operations + +### Using MAVProxy Commands + +Once connected, use MAVProxy to control the drone: + +```bash +# Set GUIDED mode for algorithm control +mode guided + +# Arm motors +arm throttle + +# Take off to 5 meters +takeoff 5 + +# Land +mode land + +# Disarm +disarm +``` + +### Using the MAVLink Bridge API + +From Python, you can control the drone directly: + +```python +from mavlink_bridge import MAVLinkBridge + +# Create bridge +bridge = MAVLinkBridge(sitl_port=14550) + +# Arm and takeoff +bridge.set_mode('GUIDED') +bridge.arm() +bridge.takeoff(altitude=5.0) + +# Land +bridge.land() +``` + +## Files + +| File | Description | +|------|-------------| +| `mavlink_bridge.py` | ROS 2 ↔ MAVLink bridge | +| `run_ardupilot.py` | Integrated launcher | +| `gazebo/worlds/ardupilot_drone.sdf` | Gazebo world with ArduPilot plugin | +| `gazebo/launch/ardupilot_drone.launch.py` | ROS 2 launch file | + +## Configuration + +Edit `config.py` to adjust ArduPilot settings: + +```python +ARDUPILOT = { + "vehicle": "ArduCopter", # ArduCopter, ArduPlane, APMrover2 + "frame": "gazebo-iris", # Gazebo frame + "sitl_host": "127.0.0.1", + "sitl_port": 5760, + "mavproxy_port": 14550, +} + +MAVLINK = { + "system_id": 1, + "component_id": 191, + "heartbeat_timeout": 5.0, +} +``` + +## Telemetry Format + +The MAVLink bridge publishes telemetry in the same format as other modes: + +```json +{ + "imu": { + "orientation": {"roll": 0.0, "pitch": 0.0, "yaw": 0.0}, + "angular_velocity": {"x": 0.0, "y": 0.0, "z": 0.0} + }, + "altimeter": { + "altitude": 5.0, + "vertical_velocity": 0.0 + }, + "velocity": {"x": 0.0, "y": 0.0, "z": 0.0}, + "position": {"x": 0.0, "y": 0.0, "z": 5.0}, + "landing_pad": { + "relative_x": 0.5, + "relative_y": 0.2, + "distance": 4.8, + "confidence": 0.95 + }, + "battery": {"voltage": 12.6, "remaining": 100}, + "armed": true, + "flight_mode": "GUIDED", + "connected": true +} +``` + +## Troubleshooting + +### SITL Not Starting + +```bash +# Check if SITL is installed +which sim_vehicle.py + +# Set ArduPilot path +export ARDUPILOT_HOME=~/ardupilot +export PATH=$PATH:$ARDUPILOT_HOME/Tools/autotest +``` + +### Gazebo Plugin Not Found + +```bash +# Check plugin path +echo $GZ_SIM_SYSTEM_PLUGIN_PATH + +# Verify plugin exists +ls ~/ardupilot_gazebo/build/libArduPilotPlugin.so +``` + +### No MAVLink Connection + +```bash +# Check if SITL is listening +netstat -tuln | grep 14550 + +# Test with mavlink console +python -c "from pymavlink import mavutil; c = mavutil.mavlink_connection('udpin:127.0.0.1:14550'); print(c.wait_heartbeat())" +``` + +### Drone Won't Arm + +Common issues: +1. **Pre-arm checks failing** - Check MAVProxy console for errors +2. **GPS required** - In simulation, you may need to wait for GPS lock +3. **EKF not ready** - Wait for EKF to initialize + +Disable pre-arm checks for testing (not recommended for real flights): +``` +# In MAVProxy +param set ARMING_CHECK 0 +``` + +## Flight Modes + +| Mode | Description | +|------|-------------| +| **GUIDED** | Accept velocity/position commands from controller | +| **LOITER** | Hold position (GPS required) | +| **ALT_HOLD** | Maintain altitude, manual horizontal | +| **LAND** | Automatic landing | +| **STABILIZE** | Attitude stabilization only | + +For autonomous landing, use **GUIDED** mode. + +## Architecture Comparison + +| Feature | Simple Gazebo | ArduPilot + Gazebo | +|---------|--------------|-------------------| +| Flight Controller | Velocity control | Full ArduPilot | +| Stabilization | Manual PD | Inbuilt EKF + PID | +| Flight Modes | None | All ArduPilot modes | +| Arming | Not required | Safety checks | +| Failsafes | None | Battery, GPS, etc. | +| MAVLink | No | Full protocol | +| GCS Support | No | QGC, Mission Planner | +| Realism | Low | High | diff --git a/docs/installation.md b/docs/installation.md index 3d819d8..ae96b6b 100644 --- a/docs/installation.md +++ b/docs/installation.md @@ -7,6 +7,7 @@ Setup instructions for all supported platforms. | Platform | Command | |----------|---------| | Ubuntu/Debian | `./setup/install_ubuntu.sh` | +| Ubuntu + ArduPilot | `./setup/install_ubuntu.sh --with-ardupilot` | | Arch Linux | `./setup/install_arch.sh` | | macOS | `./setup/install_macos.sh` | | Windows | `.\setup\install_windows.ps1` | @@ -28,6 +29,7 @@ python standalone_simulation.py | **Standalone Simulation** | ✅ | ✅ | ✅ | ✅ | ✅ | | **ROS 2** | ✅ | ⚠️ AUR | ❌ | ❌ | ✅ | | **Gazebo** | ✅ | ⚠️ AUR | ❌ | ❌ | ✅ | +| **ArduPilot SITL** | ✅ | ⚠️ Manual | ❌ | ❌ | ✅ | | **Full Mode** | ✅ | ⚠️ | ❌ | ❌ | ✅ | | **GUI Support** | ✅ | ✅ | ✅ | ✅ | ✅ WSLg | @@ -58,7 +60,18 @@ python standalone_simulation.py **Installs:** - ROS 2 (Humble or Jazzy based on Ubuntu version) - Gazebo (ros-gz) -- Python packages: pybullet, numpy, pillow, pyinstaller +- Python packages: pybullet, numpy, pillow, pyinstaller, pymavlink + +**With ArduPilot SITL (full flight controller):** +```bash +# Run installer with ArduPilot +./setup/install_ubuntu.sh --with-ardupilot + +# This will also install: +# - ArduPilot SITL (~15-20 min build) +# - ArduPilot Gazebo plugin +# - MAVProxy +``` --- @@ -407,6 +420,84 @@ After installation, verify packages: python -c "import pybullet; print('PyBullet OK')" python -c "import numpy; print('NumPy OK')" python -c "from PIL import Image; print('Pillow OK')" +python -c "from pymavlink import mavutil; print('pymavlink OK')" ``` All should print "OK". + +--- + +## ArduPilot SITL Manual Setup + +If you want to install ArduPilot SITL manually (without the install script): + +### 1. Install ArduPilot + +```bash +# Clone ArduPilot +git clone --recurse-submodules https://github.com/ArduPilot/ardupilot.git ~/ardupilot +cd ~/ardupilot + +# Install prerequisites (Ubuntu) +Tools/environment_install/install-prereqs-ubuntu.sh -y + +# Reload profile +. ~/.profile + +# Build ArduCopter SITL +./waf configure --board sitl +./waf copter +``` + +### 2. Install ArduPilot Gazebo Plugin + +```bash +# Clone plugin +git clone https://github.com/ArduPilot/ardupilot_gazebo.git ~/ardupilot_gazebo +cd ~/ardupilot_gazebo + +# Build +mkdir build && cd build +cmake .. -DCMAKE_BUILD_TYPE=Release +make -j$(nproc) +``` + +### 3. Set Environment Variables + +Add to `~/.bashrc`: + +```bash +# ArduPilot +export ARDUPILOT_HOME=$HOME/ardupilot +export PATH=$PATH:$ARDUPILOT_HOME/Tools/autotest + +# ArduPilot Gazebo Plugin +export GZ_SIM_SYSTEM_PLUGIN_PATH=$HOME/ardupilot_gazebo/build:$GZ_SIM_SYSTEM_PLUGIN_PATH +export GZ_SIM_RESOURCE_PATH=$HOME/ardupilot_gazebo/models:$HOME/ardupilot_gazebo/worlds:$GZ_SIM_RESOURCE_PATH +``` + +### 4. Test SITL + +```bash +# Test ArduCopter SITL +cd ~/ardupilot +sim_vehicle.py -v ArduCopter --console --map +``` + +### 5. Run with Gazebo + +```bash +# Terminal 1: Launch Gazebo +ros2 launch gazebo/launch/ardupilot_drone.launch.py + +# Terminal 2: Start SITL +cd ~/ardupilot +sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --console + +# Terminal 3: Run controllers +cd ~/simulation +source activate.sh +python run_ardupilot.py --no-sitl --pattern circular +``` + +For more details, see [ArduPilot Guide](ardupilot.md). diff --git a/drone_controller.py b/drone_controller.py index d5553d1..5be2f4e 100644 --- a/drone_controller.py +++ b/drone_controller.py @@ -173,6 +173,9 @@ class DroneController(Node): vel_y = velocity.get('y', 0.0) landing_pad = telemetry.get('landing_pad', None) + + # camera = telemetry.get('camera', None) + # Descent control if altitude > 1.0: diff --git a/gazebo/launch/ardupilot_drone.launch.py b/gazebo/launch/ardupilot_drone.launch.py new file mode 100644 index 0000000..f9bb7c7 --- /dev/null +++ b/gazebo/launch/ardupilot_drone.launch.py @@ -0,0 +1,134 @@ +#!/usr/bin/env python3 +""" +ArduPilot SITL + Gazebo Launch File +Works with Gazebo Harmonic/Garden and ArduPilot SITL. + +This launch file: +1. Starts Gazebo with ArduPilot-compatible drone model +2. Sets up ROS-Gazebo bridge for telemetry +3. Optionally starts ArduPilot SITL + +NOTE: ArduPilot SITL integration requires the ardupilot_gazebo plugin. +Install from: https://github.com/ArduPilot/ardupilot_gazebo +""" + +import os +import shutil +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, ExecuteProcess, TimerAction +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + """Generate the launch description.""" + + # Get paths + script_dir = os.path.dirname(os.path.abspath(__file__)) + gazebo_dir = os.path.dirname(script_dir) + world_file = os.path.join(gazebo_dir, 'worlds', 'ardupilot_drone.sdf') + + # Check for ArduPilot Gazebo plugin + plugin_paths = [ + os.path.expanduser("~/ardupilot_gazebo/build"), + "/opt/ardupilot_gazebo/lib", + os.environ.get("GZ_SIM_SYSTEM_PLUGIN_PATH", ""), + ] + ardupilot_plugin_found = any(os.path.exists(p) for p in plugin_paths if p) + + # Determine Gazebo command + if shutil.which('gz'): + gz_cmd = ['gz', 'sim', '-r', world_file] + elif shutil.which('ign'): + gz_cmd = ['ign', 'gazebo', '-r', world_file] + else: + gz_cmd = ['gz', 'sim', '-r', world_file] + + use_sim_time = LaunchConfiguration('use_sim_time', default='true') + start_sitl = LaunchConfiguration('start_sitl', default='false') + + # Set plugin path for ArduPilot + env = os.environ.copy() + plugin_search_paths = ':'.join([p for p in plugin_paths if p and os.path.exists(p)]) + if plugin_search_paths: + env['GZ_SIM_SYSTEM_PLUGIN_PATH'] = plugin_search_paths + ':' + env.get('GZ_SIM_SYSTEM_PLUGIN_PATH', '') + + actions = [ + DeclareLaunchArgument( + 'use_sim_time', + default_value='true', + description='Use simulation clock' + ), + + DeclareLaunchArgument( + 'start_sitl', + default_value='false', + description='Start ArduPilot SITL automatically' + ), + + # Start Gazebo with ArduPilot world + ExecuteProcess( + cmd=gz_cmd, + output='screen', + additional_env=env + ), + + # ROS-Gazebo Bridge for telemetry and commands + # Delayed start to wait for Gazebo + TimerAction( + period=2.0, + actions=[ + Node( + package='ros_gz_bridge', + executable='parameter_bridge', + name='gz_bridge', + arguments=[ + # Rover velocity commands (ROS to Gazebo) + '/rover/cmd_vel@geometry_msgs/msg/Twist]gz.msgs.Twist', + # Odometry (from Gazebo to ROS) - fallback if MAVLink not used + '/model/drone/odometry@nav_msgs/msg/Odometry[gz.msgs.Odometry', + # Camera (from Gazebo to ROS) + '/drone/camera@sensor_msgs/msg/Image[gz.msgs.Image', + # IMU (from Gazebo to ROS) + '/imu@sensor_msgs/msg/Imu[gz.msgs.IMU', + # Clock + '/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock', + ], + parameters=[{'use_sim_time': use_sim_time}], + output='screen' + ), + ] + ), + ] + + return LaunchDescription(actions) + + +if __name__ == '__main__': + print("=" * 60) + print(" ArduPilot SITL + Gazebo Launch File") + print("=" * 60) + print() + print("This is a ROS 2 launch file for ArduPilot integration.") + print() + print("Prerequisites:") + print(" 1. Gazebo (gz sim or ign gazebo)") + print(" 2. ros_gz_bridge package") + print(" 3. ArduPilot Gazebo plugin (optional, for SITL)") + print() + print("Usage:") + print(" ros2 launch gazebo/launch/ardupilot_drone.launch.py") + print() + print("Then in another terminal:") + print(" # Start SITL") + print(" sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON") + print() + print(" # Or use the integrated runner") + print(" python run_ardupilot.py --no-sitl") + print() + print("Manual Gazebo Start:") + + if shutil.which('gz'): + print(" gz sim -r gazebo/worlds/ardupilot_drone.sdf") + else: + print(" ign gazebo gazebo/worlds/ardupilot_drone.sdf") diff --git a/gazebo/worlds/ardupilot_drone.sdf b/gazebo/worlds/ardupilot_drone.sdf new file mode 100644 index 0000000..b1ff4ba --- /dev/null +++ b/gazebo/worlds/ardupilot_drone.sdf @@ -0,0 +1,504 @@ + + + + + + + + 0.001 + 1.0 + 1000 + + + + + + + + ogre2 + + + + + + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + -0.5 0.1 -0.9 + + + + false + 0 0 10 0 0 0 + 0.3 0.3 0.3 1 + 0.0 0.0 0.0 1 + 0.5 0.3 -0.5 + + + + + true + + + + + 0 0 1 + 100 100 + + + + + + 1 + 1 + + + + + + + + 0 0 1 + 100 100 + + + + 0.5 0.5 0.5 1 + 0.7 0.7 0.7 1 + + + + + + + + 0 0 0.15 0 0 0 + + + + 10.0 + + 1.000 + 1.00 + 1.0 + + + + + + 1.0 1.0 0.3 + + + 0.1 0.5 0.1 1 + 0.2 0.7 0.2 1 + + + + + 1.0 1.0 0.3 + + + + + + 0 0 0.151 0 0 0 + 0.6 0.1 0.001 + + 1 1 1 1 + 1 1 1 1 + + + + -0.25 0 0.151 0 0 0 + 0.1 0.5 0.001 + + 1 1 1 1 + 1 1 1 1 + + + + 0.25 0 0.151 0 0 0 + 0.1 0.5 0.001 + + 1 1 1 1 + 1 1 1 1 + + + + + + + /rover/cmd_vel + 0 0 0 + + + + + + 0 0 0.194923 0 0 0 + + + + 0 0 0 0 0 0 + 1.5 + + 0.029125 + 0 + 0 + 0.029125 + 0 + 0.055225 + + + + + + 0.47 0.47 0.11 + + + + + + + 0.3 0.3 0.1 + + + 0.8 0.1 0.1 1 + 0.9 0.2 0.2 1 + + + + + + 0.12 0.12 0 0 0 0.785 + 0.2 0.02 0.02 + 0.3 0.3 0.3 1 + + + 0.12 -0.12 0 0 0 -0.785 + 0.2 0.02 0.02 + 0.3 0.3 0.3 1 + + + -0.12 0.12 0 0 0 -0.785 + 0.2 0.02 0.02 + 0.3 0.3 0.3 1 + + + -0.12 -0.12 0 0 0 0.785 + 0.2 0.02 0.02 + 0.3 0.3 0.3 1 + + + + + 0 0 0.08 0 0 0 + 0.020.04 + 0.2 0.2 0.8 1 + + + + + true + 400 + imu + + + 00.0002 + 00.0002 + 00.0002 + + + 00.017 + 00.017 + 00.017 + + + + + + + true + 50 + air_pressure + + 0 + + 0 + 0.01 + + + + + + + 0 0 -0.05 0 1.5708 0 + true + 30 + /drone/camera + + 1.047 + + 320 + 240 + R8G8B8 + + + 0.1 + 100 + + + + + + + + 0.13 -0.22 0.023 0 0 0 + + 0.025 + + 9.75e-0600 + 1.66704e-040 + 1.66704e-04 + + + + 0.10.005 + 0.1 0.1 0.1 1 + + + 0.10.005 + + + + + base_link + rotor_0 + + 0 0 1 + -1e161e16 + + + + + + -0.13 0.2 0.023 0 0 0 + + 0.025 + + 9.75e-0600 + 1.66704e-040 + 1.66704e-04 + + + + 0.10.005 + 0.1 0.1 0.1 1 + + + 0.10.005 + + + + + base_link + rotor_1 + + 0 0 1 + -1e161e16 + + + + + + 0.13 0.22 0.023 0 0 0 + + 0.025 + + 9.75e-0600 + 1.66704e-040 + 1.66704e-04 + + + + 0.10.005 + 0.1 0.1 0.1 1 + + + 0.10.005 + + + + + base_link + rotor_2 + + 0 0 1 + -1e161e16 + + + + + + -0.13 -0.2 0.023 0 0 0 + + 0.025 + + 9.75e-0600 + 1.66704e-040 + 1.66704e-04 + + + + 0.10.005 + 0.1 0.1 0.1 1 + + + 0.10.005 + + + + + base_link + rotor_3 + + 0 0 1 + -1e161e16 + + + + + + + 127.0.0.1 + 9002 + 127.0.0.1 + 9003 + + + 0 0 0 3.141593 0 0 + 0 0 0 3.141593 0 0 + + + imu_sensor + + + + + rotor_0_joint + VELOCITY + 0 + 838 + 0.2 + 0 + 0 + 0 + 0 + 2.5 + -2.5 + + + + + rotor_1_joint + VELOCITY + 0 + 838 + 0.2 + 0 + 0 + 0 + 0 + 2.5 + -2.5 + + + + + rotor_2_joint + VELOCITY + 0 + -838 + 0.2 + 0 + 0 + 0 + 0 + 2.5 + -2.5 + + + + + rotor_3_joint + VELOCITY + 0 + -838 + 0.2 + 0 + 0 + 0 + 0 + 2.5 + -2.5 + + + + + + odom + base_link + /model/drone/odometry + 50 + + + + + + diff --git a/mavlink_bridge.py b/mavlink_bridge.py new file mode 100644 index 0000000..fdeb042 --- /dev/null +++ b/mavlink_bridge.py @@ -0,0 +1,580 @@ +#!/usr/bin/env python3 +""" +MAVLink Bridge for ArduPilot SITL Integration. +Connects Gazebo simulation to ArduPilot SITL via MAVProxy. + +This bridge: +- Connects to ArduPilot SITL via MAVLink (UDP) +- Receives telemetry from ArduPilot (position, attitude, battery, etc.) +- Sends commands to ArduPilot (velocity, position, etc.) +- Publishes ROS 2 telemetry for the drone controller +""" + +import json +import math +import time +import threading +from typing import Optional, Dict, Any, Tuple + +import rclpy +from rclpy.node import Node +from geometry_msgs.msg import Twist +from std_msgs.msg import String + +try: + from pymavlink import mavutil + from pymavlink.dialects.v20 import ardupilotmega as mavlink + MAVLINK_AVAILABLE = True +except ImportError: + MAVLINK_AVAILABLE = False + print("WARNING: pymavlink not installed. Run: pip install pymavlink") + + +class MAVLinkBridge(Node): + """ + ROS 2 Node that bridges between ROS topics and ArduPilot SITL via MAVLink. + """ + + # MAVLink connection defaults + DEFAULT_SITL_HOST = "127.0.0.1" + DEFAULT_SITL_PORT = 14550 # MAVProxy output port + DEFAULT_SYSTEM_ID = 1 + DEFAULT_COMPONENT_ID = 191 # MAV_COMP_ID_MISSIONPLANNER + + # Telemetry rate (Hz) + TELEMETRY_RATE = 50.0 + + def __init__( + self, + sitl_host: str = None, + sitl_port: int = None, + system_id: int = None, + component_id: int = None + ): + super().__init__('mavlink_bridge') + + # Connection parameters (with parameter overrides) + self.declare_parameter('sitl_host', sitl_host or self.DEFAULT_SITL_HOST) + self.declare_parameter('sitl_port', sitl_port or self.DEFAULT_SITL_PORT) + self.declare_parameter('system_id', system_id or self.DEFAULT_SYSTEM_ID) + self.declare_parameter('component_id', component_id or self.DEFAULT_COMPONENT_ID) + + self._sitl_host = self.get_parameter('sitl_host').value + self._sitl_port = self.get_parameter('sitl_port').value + self._system_id = self.get_parameter('system_id').value + self._component_id = self.get_parameter('component_id').value + + self.get_logger().info('=' * 60) + self.get_logger().info('MAVLink Bridge Starting...') + self.get_logger().info(f' Connecting to SITL at {self._sitl_host}:{self._sitl_port}') + self.get_logger().info('=' * 60) + + # MAVLink connection + self._mav_conn: Optional[mavutil.mavlink_connection] = None + self._connected = False + self._armed = False + self._flight_mode = "UNKNOWN" + + # Telemetry state + self._position = [0.0, 0.0, 0.0] # x, y, z (NED frame, converted to ENU) + self._velocity = [0.0, 0.0, 0.0] # vx, vy, vz + self._attitude = [0.0, 0.0, 0.0] # roll, pitch, yaw (radians) + self._angular_velocity = [0.0, 0.0, 0.0] # wx, wy, wz + self._battery_voltage = 0.0 + self._battery_remaining = 100 + self._gps_fix = 0 + self._landed = False + self._home_position = None + self._last_heartbeat = 0.0 + + # Rover position (for landing pad detection) + self._rover_position = [0.0, 0.0, 0.15] + + # Thread safety + self._lock = threading.Lock() + + # ROS 2 Subscriptions + self._cmd_vel_sub = self.create_subscription( + Twist, '/cmd_vel', self._cmd_vel_callback, 10 + ) + self.get_logger().info(' Subscribed to: /cmd_vel') + + self._rover_sub = self.create_subscription( + String, '/rover/telemetry', self._rover_callback, 10 + ) + self.get_logger().info(' Subscribed to: /rover/telemetry') + + # ROS 2 Publishers + self._telemetry_pub = self.create_publisher( + String, '/drone/telemetry', 10 + ) + self.get_logger().info(' Publishing to: /drone/telemetry') + + self._status_pub = self.create_publisher( + String, '/mavlink/status', 10 + ) + self.get_logger().info(' Publishing to: /mavlink/status') + + # Initialize MAVLink connection + self._connect_mavlink() + + # Start telemetry thread + self._running = True + self._mavlink_thread = threading.Thread(target=self._mavlink_loop, daemon=True) + self._mavlink_thread.start() + + # Telemetry timer + telemetry_period = 1.0 / self.TELEMETRY_RATE + self._telemetry_timer = self.create_timer(telemetry_period, self._publish_telemetry) + + self.get_logger().info('MAVLink Bridge Ready!') + + def _connect_mavlink(self) -> bool: + """Establish MAVLink connection to ArduPilot SITL.""" + if not MAVLINK_AVAILABLE: + self.get_logger().error('pymavlink not available!') + return False + + try: + connection_string = f'udpin:{self._sitl_host}:{self._sitl_port}' + self.get_logger().info(f'Connecting to: {connection_string}') + + self._mav_conn = mavutil.mavlink_connection( + connection_string, + source_system=self._system_id, + source_component=self._component_id + ) + + # Wait for heartbeat + self.get_logger().info('Waiting for heartbeat...') + self._mav_conn.wait_heartbeat(timeout=30) + + self._connected = True + self._last_heartbeat = time.time() + self.get_logger().info( + f'Connected! System: {self._mav_conn.target_system}, ' + f'Component: {self._mav_conn.target_component}' + ) + + # Request data streams + self._request_data_streams() + + return True + + except Exception as e: + self.get_logger().error(f'MAVLink connection failed: {e}') + self._connected = False + return False + + def _request_data_streams(self): + """Request required data streams from ArduPilot.""" + if not self._mav_conn: + return + + # Request all data streams at high rate + streams = [ + (mavutil.mavlink.MAV_DATA_STREAM_ALL, 10), + (mavutil.mavlink.MAV_DATA_STREAM_RAW_SENSORS, 50), + (mavutil.mavlink.MAV_DATA_STREAM_EXTENDED_STATUS, 5), + (mavutil.mavlink.MAV_DATA_STREAM_POSITION, 50), + (mavutil.mavlink.MAV_DATA_STREAM_EXTRA1, 50), # Attitude + (mavutil.mavlink.MAV_DATA_STREAM_EXTRA2, 10), # VFR_HUD + ] + + for stream_id, rate in streams: + self._mav_conn.mav.request_data_stream_send( + self._mav_conn.target_system, + self._mav_conn.target_component, + stream_id, + rate, + 1 # Start + ) + + def _mavlink_loop(self): + """Background thread for receiving MAVLink messages.""" + reconnect_delay = 2.0 + + while self._running: + if not self._connected: + time.sleep(reconnect_delay) + self._connect_mavlink() + continue + + try: + # Non-blocking message receive + msg = self._mav_conn.recv_match(blocking=True, timeout=0.1) + + if msg is None: + # Check for connection timeout + if time.time() - self._last_heartbeat > 5.0: + self.get_logger().warning('Heartbeat timeout, reconnecting...') + self._connected = False + continue + + self._process_mavlink_message(msg) + + except Exception as e: + self.get_logger().error(f'MAVLink receive error: {e}') + self._connected = False + + def _process_mavlink_message(self, msg): + """Process incoming MAVLink messages.""" + msg_type = msg.get_type() + + with self._lock: + if msg_type == 'HEARTBEAT': + self._last_heartbeat = time.time() + self._armed = msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED + self._flight_mode = mavutil.mode_string_v10(msg) + + elif msg_type == 'LOCAL_POSITION_NED': + # Convert NED to ENU (ROS convention) + self._position = [msg.y, msg.x, -msg.z] # NED to ENU + self._velocity = [msg.vy, msg.vx, -msg.vz] + + elif msg_type == 'GLOBAL_POSITION_INT': + # Backup position from GPS + if self._home_position is None: + self._home_position = (msg.lat, msg.lon, msg.alt) + + elif msg_type == 'ATTITUDE': + self._attitude = [msg.roll, msg.pitch, msg.yaw] + self._angular_velocity = [msg.rollspeed, msg.pitchspeed, msg.yawspeed] + + elif msg_type == 'SYS_STATUS': + self._battery_voltage = msg.voltage_battery / 1000.0 # mV to V + self._battery_remaining = msg.battery_remaining + + elif msg_type == 'GPS_RAW_INT': + self._gps_fix = msg.fix_type + + elif msg_type == 'EXTENDED_SYS_STATE': + # Check landed state + self._landed = (msg.landed_state == mavutil.mavlink.MAV_LANDED_STATE_ON_GROUND) + + def _cmd_vel_callback(self, msg: Twist): + """Handle velocity commands from drone controller.""" + if not self._connected or not self._mav_conn: + return + + try: + # Convert ROS Twist to MAVLink velocity command + # linear.x = forward (pitch), linear.y = left (roll), linear.z = up (thrust) + # angular.z = yaw rate + + # Use SET_POSITION_TARGET_LOCAL_NED for velocity control + # Type mask: velocity only (ignore position and acceleration) + type_mask = ( + mavutil.mavlink.POSITION_TARGET_TYPEMASK_X_IGNORE | + mavutil.mavlink.POSITION_TARGET_TYPEMASK_Y_IGNORE | + mavutil.mavlink.POSITION_TARGET_TYPEMASK_Z_IGNORE | + mavutil.mavlink.POSITION_TARGET_TYPEMASK_AX_IGNORE | + mavutil.mavlink.POSITION_TARGET_TYPEMASK_AY_IGNORE | + mavutil.mavlink.POSITION_TARGET_TYPEMASK_AZ_IGNORE | + mavutil.mavlink.POSITION_TARGET_TYPEMASK_YAW_IGNORE + ) + + # Convert velocities (ENU to NED) + vx = msg.linear.x # Forward (body frame pitch command) + vy = msg.linear.y # Left (body frame roll command) + vz = -msg.linear.z # Down (negative because NED) + yaw_rate = msg.angular.z + + self._mav_conn.mav.set_position_target_local_ned_send( + 0, # time_boot_ms (not used) + self._mav_conn.target_system, + self._mav_conn.target_component, + mavutil.mavlink.MAV_FRAME_LOCAL_NED, + type_mask, + 0, 0, 0, # position (ignored) + vx, vy, vz, # velocity + 0, 0, 0, # acceleration (ignored) + 0, yaw_rate # yaw, yaw_rate + ) + + except Exception as e: + self.get_logger().warning(f'Failed to send velocity command: {e}') + + def _rover_callback(self, msg: String): + """Receive rover position for landing pad detection.""" + try: + data = json.loads(msg.data) + pos = data.get('position', {}) + with self._lock: + self._rover_position = [ + pos.get('x', 0.0), + pos.get('y', 0.0), + pos.get('z', 0.15) + ] + except json.JSONDecodeError: + pass + + def _get_landing_pad_detection(self) -> Optional[Dict[str, float]]: + """Simulate landing pad detection based on relative position.""" + # Camera parameters + CAMERA_FOV = 60.0 + CAMERA_RANGE = 10.0 + + dx = self._rover_position[0] - self._position[0] + dy = self._rover_position[1] - self._position[1] + dz = self._rover_position[2] - self._position[2] + + horizontal_dist = math.sqrt(dx**2 + dy**2) + vertical_dist = -dz # Height above rover + + if vertical_dist <= 0 or vertical_dist > CAMERA_RANGE: + return None + + fov_rad = math.radians(CAMERA_FOV / 2) + max_horizontal = vertical_dist * math.tan(fov_rad) + + if horizontal_dist > max_horizontal: + return None + + # Transform to body frame using yaw + yaw = self._attitude[2] + cos_yaw = math.cos(-yaw) + sin_yaw = math.sin(-yaw) + relative_x = dx * cos_yaw - dy * sin_yaw + relative_y = dx * sin_yaw + dy * cos_yaw + + # Calculate confidence + angle = math.atan2(horizontal_dist, vertical_dist) + confidence = max(0.0, 1.0 - (angle / fov_rad)) + confidence *= max(0.0, 1.0 - (vertical_dist / CAMERA_RANGE)) + + return { + "relative_x": round(relative_x, 4), + "relative_y": round(relative_y, 4), + "distance": round(vertical_dist, 4), + "confidence": round(confidence, 4) + } + + def _publish_telemetry(self): + """Publish drone telemetry to ROS 2 topic.""" + with self._lock: + # Landing pad detection + pad_detection = self._get_landing_pad_detection() + + telemetry = { + "imu": { + "orientation": { + "roll": round(self._attitude[0], 4), + "pitch": round(self._attitude[1], 4), + "yaw": round(self._attitude[2], 4) + }, + "angular_velocity": { + "x": round(self._angular_velocity[0], 4), + "y": round(self._angular_velocity[1], 4), + "z": round(self._angular_velocity[2], 4) + }, + "linear_acceleration": { + "x": 0.0, + "y": 0.0, + "z": 9.81 + } + }, + "altimeter": { + "altitude": round(self._position[2], 4), + "vertical_velocity": round(self._velocity[2], 4) + }, + "velocity": { + "x": round(self._velocity[0], 4), + "y": round(self._velocity[1], 4), + "z": round(self._velocity[2], 4) + }, + "position": { + "x": round(self._position[0], 4), + "y": round(self._position[1], 4), + "z": round(self._position[2], 4) + }, + "landing_pad": pad_detection, + "camera": { + "width": 320, + "height": 240, + "fov": 60.0, + "image": None # Would need camera integration + }, + "battery": { + "voltage": round(self._battery_voltage, 2), + "remaining": self._battery_remaining + }, + "landed": self._landed, + "armed": self._armed, + "flight_mode": self._flight_mode, + "connected": self._connected, + "timestamp": round(time.time(), 4) + } + + telemetry_msg = String() + telemetry_msg.data = json.dumps(telemetry) + self._telemetry_pub.publish(telemetry_msg) + + # Publish status + status_msg = String() + status_msg.data = json.dumps({ + "connected": self._connected, + "armed": self._armed, + "mode": self._flight_mode, + "gps_fix": self._gps_fix + }) + self._status_pub.publish(status_msg) + + # ========================================================================= + # ArduPilot Control Commands + # ========================================================================= + + def arm(self, force: bool = False) -> bool: + """Arm the drone.""" + if not self._connected or not self._mav_conn: + return False + + self.get_logger().info('Arming drone...') + self._mav_conn.arducopter_arm() + return self._wait_for_arm(True, timeout=5.0) + + def disarm(self, force: bool = False) -> bool: + """Disarm the drone.""" + if not self._connected or not self._mav_conn: + return False + + self.get_logger().info('Disarming drone...') + self._mav_conn.arducopter_disarm() + return self._wait_for_arm(False, timeout=5.0) + + def _wait_for_arm(self, expected: bool, timeout: float = 5.0) -> bool: + """Wait for arm/disarm to complete.""" + start = time.time() + while time.time() - start < timeout: + if self._armed == expected: + return True + time.sleep(0.1) + return False + + def set_mode(self, mode: str) -> bool: + """Set flight mode (e.g., 'GUIDED', 'LAND', 'LOITER').""" + if not self._connected or not self._mav_conn: + return False + + self.get_logger().info(f'Setting mode to: {mode}') + + mode_mapping = self._mav_conn.mode_mapping() + if mode.upper() not in mode_mapping: + self.get_logger().error(f'Unknown mode: {mode}') + return False + + mode_id = mode_mapping[mode.upper()] + self._mav_conn.set_mode(mode_id) + + # Wait for mode change + start = time.time() + while time.time() - start < 5.0: + if self._flight_mode.upper() == mode.upper(): + return True + time.sleep(0.1) + + return False + + def takeoff(self, altitude: float = 5.0) -> bool: + """Take off to specified altitude.""" + if not self._connected or not self._mav_conn: + return False + + self.get_logger().info(f'Taking off to {altitude}m...') + + # Set GUIDED mode + if not self.set_mode('GUIDED'): + self.get_logger().error('Failed to set GUIDED mode') + return False + + # Arm if not armed + if not self._armed: + if not self.arm(): + self.get_logger().error('Failed to arm') + return False + + # Command takeoff + self._mav_conn.mav.command_long_send( + self._mav_conn.target_system, + self._mav_conn.target_component, + mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, + 0, # confirmation + 0, 0, 0, 0, # params 1-4 (unused) + 0, 0, # lat, lon (use current) + altitude # altitude + ) + + return True + + def land(self) -> bool: + """Command the drone to land.""" + if not self._connected or not self._mav_conn: + return False + + self.get_logger().info('Landing...') + return self.set_mode('LAND') + + def goto_position(self, x: float, y: float, z: float, yaw: float = 0.0): + """Go to a local position (ENU frame).""" + if not self._connected or not self._mav_conn: + return + + # Convert ENU to NED + type_mask = ( + mavutil.mavlink.POSITION_TARGET_TYPEMASK_VX_IGNORE | + mavutil.mavlink.POSITION_TARGET_TYPEMASK_VY_IGNORE | + mavutil.mavlink.POSITION_TARGET_TYPEMASK_VZ_IGNORE | + mavutil.mavlink.POSITION_TARGET_TYPEMASK_AX_IGNORE | + mavutil.mavlink.POSITION_TARGET_TYPEMASK_AY_IGNORE | + mavutil.mavlink.POSITION_TARGET_TYPEMASK_AZ_IGNORE | + mavutil.mavlink.POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE + ) + + self._mav_conn.mav.set_position_target_local_ned_send( + 0, # time_boot_ms + self._mav_conn.target_system, + self._mav_conn.target_component, + mavutil.mavlink.MAV_FRAME_LOCAL_NED, + type_mask, + y, x, -z, # ENU to NED position + 0, 0, 0, # velocity (ignored) + 0, 0, 0, # acceleration (ignored) + yaw, 0 # yaw, yaw_rate + ) + + def destroy_node(self): + """Clean shutdown.""" + self._running = False + if self._mavlink_thread.is_alive(): + self._mavlink_thread.join(timeout=2.0) + if self._mav_conn: + self._mav_conn.close() + super().destroy_node() + + +def main(args=None): + if not MAVLINK_AVAILABLE: + print("ERROR: pymavlink is required. Install with: pip install pymavlink") + return + + print("\n" + "=" * 60) + print(" MAVLink Bridge for ArduPilot SITL") + print("=" * 60 + "\n") + + rclpy.init(args=args) + bridge = None + + try: + bridge = MAVLinkBridge() + rclpy.spin(bridge) + except KeyboardInterrupt: + print('\nShutting down...') + finally: + if bridge: + bridge.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/requirements.txt b/requirements.txt index 1de73d1..a4d1dd4 100644 --- a/requirements.txt +++ b/requirements.txt @@ -2,3 +2,4 @@ pybullet>=3.2.5 numpy>=1.20.0 pillow>=9.0.0 pyinstaller>=6.0.0 +pymavlink>=2.4.0 diff --git a/run_ardupilot.py b/run_ardupilot.py new file mode 100644 index 0000000..621a68c --- /dev/null +++ b/run_ardupilot.py @@ -0,0 +1,359 @@ +#!/usr/bin/env python3 +""" +Run ArduPilot SITL + Gazebo Simulation. + +This script orchestrates the full ArduPilot SITL + Gazebo simulation stack: +1. Launches ArduPilot SITL (sim_vehicle.py with Gazebo model) +2. Starts MAVProxy for GCS connectivity +3. Runs MAVLink bridge + controllers + +Usage: + python run_ardupilot.py # Start everything + python run_ardupilot.py --pattern circular # With rover movement + python run_ardupilot.py --no-sitl # Just controllers (SITL already running) +""" + +import argparse +import os +import signal +import subprocess +import sys +import time +from pathlib import Path + +import rclpy +from rclpy.executors import MultiThreadedExecutor + +# Check for pymavlink +try: + from pymavlink import mavutil + MAVLINK_AVAILABLE = True +except ImportError: + MAVLINK_AVAILABLE = False + +from mavlink_bridge import MAVLinkBridge +from drone_controller import DroneController +from rover_controller import RoverController, MovementPattern + + +def find_sim_vehicle() -> str: + """Find sim_vehicle.py in common ArduPilot locations.""" + locations = [ + os.path.expanduser("~/ardupilot/Tools/autotest/sim_vehicle.py"), + "/opt/ardupilot/Tools/autotest/sim_vehicle.py", + os.environ.get("ARDUPILOT_HOME", "") + "/Tools/autotest/sim_vehicle.py", + ] + + for loc in locations: + if os.path.exists(loc): + return loc + + # Try finding it in PATH + import shutil + if shutil.which("sim_vehicle.py"): + return "sim_vehicle.py" + + return None + + +def parse_args(): + parser = argparse.ArgumentParser( + description='Run ArduPilot SITL + Gazebo + MAVLink Bridge', + formatter_class=argparse.RawDescriptionHelpFormatter, + epilog=""" +Examples: + python run_ardupilot.py # Full stack with Gazebo + python run_ardupilot.py --pattern circular # Moving rover + python run_ardupilot.py --no-sitl # SITL already running + python run_ardupilot.py --mavproxy-port 14551 # Custom port + """ + ) + + # SITL options + parser.add_argument( + '--no-sitl', action='store_true', + help='Skip launching SITL (assume already running)' + ) + parser.add_argument( + '--sitl-path', type=str, + help='Path to sim_vehicle.py' + ) + parser.add_argument( + '--vehicle', type=str, default='ArduCopter', + choices=['ArduCopter', 'ArduPlane', 'APMrover2'], + help='Vehicle type (default: ArduCopter)' + ) + parser.add_argument( + '--frame', type=str, default='gazebo-iris', + help='Vehicle frame (default: gazebo-iris)' + ) + + # MAVProxy settings + parser.add_argument( + '--mavproxy-port', type=int, default=14550, + help='MAVProxy output port (default: 14550)' + ) + parser.add_argument( + '--sitl-port', type=int, default=5760, + help='SITL connection port (default: 5760)' + ) + + # Rover options + parser.add_argument( + '--pattern', type=str, default='stationary', + choices=['stationary', 'linear', 'circular', 'random', 'square'], + help='Rover movement pattern (default: stationary)' + ) + parser.add_argument( + '--speed', '-s', type=float, default=0.5, + help='Rover speed in m/s (default: 0.5)' + ) + parser.add_argument( + '--amplitude', '-a', type=float, default=2.0, + help='Rover movement amplitude in meters (default: 2.0)' + ) + parser.add_argument( + '--no-rover', action='store_true', + help='Disable rover controller' + ) + + # Other + parser.add_argument( + '--console', action='store_true', + help='Show MAVProxy console' + ) + parser.add_argument( + '--map', action='store_true', + help='Show MAVProxy map' + ) + + args, _ = parser.parse_known_args() + return args + + +class SITLProcess: + """Manages the ArduPilot SITL process.""" + + def __init__( + self, + sim_vehicle_path: str, + vehicle: str = 'ArduCopter', + frame: str = 'gazebo-iris', + sitl_port: int = 5760, + mavproxy_port: int = 14550, + console: bool = False, + show_map: bool = False + ): + self.sim_vehicle_path = sim_vehicle_path + self.vehicle = vehicle + self.frame = frame + self.sitl_port = sitl_port + self.mavproxy_port = mavproxy_port + self.console = console + self.show_map = show_map + self.process = None + + def start(self) -> bool: + """Start SITL process.""" + if not self.sim_vehicle_path: + print("ERROR: sim_vehicle.py not found!") + print("Please set --sitl-path or ARDUPILOT_HOME environment variable") + return False + + cmd = [ + sys.executable, self.sim_vehicle_path, + '-v', self.vehicle, + '-f', self.frame, + '--model', 'JSON', + '--sitl-instance-args', f'--sim-address=127.0.0.1', + '--out', f'udp:127.0.0.1:{self.mavproxy_port}', + ] + + if self.console: + cmd.append('--console') + if self.show_map: + cmd.append('--map') + + print(f"Starting SITL: {' '.join(cmd)}") + + try: + self.process = subprocess.Popen( + cmd, + stdout=subprocess.PIPE, + stderr=subprocess.STDOUT + ) + return True + except Exception as e: + print(f"Failed to start SITL: {e}") + return False + + def stop(self): + """Stop SITL process.""" + if self.process: + self.process.terminate() + self.process.wait(timeout=5.0) + self.process = None + + +def wait_for_sitl(host: str = '127.0.0.1', port: int = 14550, timeout: float = 60.0) -> bool: + """Wait for SITL to be ready.""" + print(f"Waiting for SITL on {host}:{port}...") + start = time.time() + + while time.time() - start < timeout: + try: + conn = mavutil.mavlink_connection(f'udpin:{host}:{port}', timeout=1) + if conn.wait_heartbeat(timeout=1): + print("SITL is ready!") + conn.close() + return True + except Exception: + pass + time.sleep(1) + + print("Timeout waiting for SITL") + return False + + +def main(): + args = parse_args() + + if not MAVLINK_AVAILABLE: + print("ERROR: pymavlink is required!") + print("Install with: pip install pymavlink") + return 1 + + print("=" * 60) + print(" ArduPilot SITL + Gazebo Simulation") + print("=" * 60) + print(f" Vehicle: {args.vehicle}") + print(f" Frame: {args.frame}") + print(f" Rover Pattern: {args.pattern}") + print(f" MAVProxy Port: {args.mavproxy_port}") + print("=" * 60) + print() + + sitl = None + + # Start SITL if not skipped + if not args.no_sitl: + sim_vehicle = args.sitl_path or find_sim_vehicle() + + if not sim_vehicle: + print("=" * 60) + print("WARNING: sim_vehicle.py not found!") + print() + print("Please either:") + print("1. Install ArduPilot: https://ardupilot.org/dev/docs/building-setup-linux-ubuntu-apt.html") + print("2. Set ARDUPILOT_HOME environment variable") + print("3. Use --sitl-path to specify location") + print("4. Use --no-sitl if SITL is already running") + print("=" * 60) + print() + print("Continuing without starting SITL...") + else: + sitl = SITLProcess( + sim_vehicle, + vehicle=args.vehicle, + frame=args.frame, + sitl_port=args.sitl_port, + mavproxy_port=args.mavproxy_port, + console=args.console, + show_map=args.map + ) + + if not sitl.start(): + return 1 + + print("\nWaiting for SITL to initialize...") + time.sleep(5) # Give SITL time to start + + # Wait for SITL connection + if not wait_for_sitl(port=args.mavproxy_port, timeout=30): + print("Could not connect to SITL!") + if sitl: + sitl.stop() + return 1 + + # Initialize ROS + rclpy.init() + + nodes = [] + executor = MultiThreadedExecutor(num_threads=4) + + try: + # Create MAVLink bridge + bridge = MAVLinkBridge( + sitl_port=args.mavproxy_port + ) + nodes.append(bridge) + executor.add_node(bridge) + print("[OK] MAVLink Bridge started") + + # Create drone controller + drone = DroneController() + nodes.append(drone) + executor.add_node(drone) + print("[OK] Drone Controller started") + + # Create rover controller + if not args.no_rover: + pattern = MovementPattern[args.pattern.upper()] + rover = RoverController( + pattern=pattern, + speed=args.speed, + amplitude=args.amplitude + ) + nodes.append(rover) + executor.add_node(rover) + print("[OK] Rover Controller started") + + print() + print("=" * 60) + print("System ready!") + print() + print("To arm and takeoff, use MAVProxy commands:") + print(" mode guided") + print(" arm throttle") + print(" takeoff 5") + print() + print("Or use the bridge.arm() and bridge.takeoff() methods") + print("=" * 60) + print() + print("Press Ctrl+C to stop.") + print() + + # Handle Ctrl+C + def signal_handler(sig, frame): + print("\nShutting down...") + executor.shutdown() + + signal.signal(signal.SIGINT, signal_handler) + + executor.spin() + + except Exception as e: + print(f"Error: {e}") + raise + finally: + print("Cleaning up...") + + # Stop nodes + for node in nodes: + node.destroy_node() + + # Stop ROS + if rclpy.ok(): + rclpy.shutdown() + + # Stop SITL + if sitl: + sitl.stop() + + print("Shutdown complete.") + + return 0 + + +if __name__ == '__main__': + sys.exit(main()) diff --git a/setup/install_arch.sh b/setup/install_arch.sh index d89b718..e4c5336 100755 --- a/setup/install_arch.sh +++ b/setup/install_arch.sh @@ -4,6 +4,7 @@ # ============================================================================= # Installs PyBullet and Python dependencies # ROS 2 requires AUR (optional - not needed for standalone mode) +# ArduPilot SITL can be built from source (optional) # # Usage: ./install_arch.sh # ============================================================================= @@ -153,6 +154,7 @@ echo "Checking Python packages:" python -c "import pybullet; print(' PyBullet: OK')" || echo " PyBullet: FAILED" python -c "import numpy; print(' NumPy: OK')" || echo " NumPy: FAILED" python -c "from PIL import Image; print(' Pillow: OK')" || echo " Pillow: FAILED" +python -c "from pymavlink import mavutil; print(' pymavlink: OK')" || echo " pymavlink: FAILED" echo "" echo "==============================================" @@ -185,4 +187,28 @@ echo "After installing, use:" echo " ign gazebo gazebo/worlds/drone_landing.sdf # Terminal 1" echo " python run_gazebo.py --pattern circular # Terminal 2" echo "" +echo "==============================================" +echo " Optional: Install ArduPilot SITL" +echo "==============================================" +echo "" +echo "For realistic flight controller simulation:" +echo "" +echo " # Clone and build ArduPilot" +echo " git clone --recurse-submodules https://github.com/ArduPilot/ardupilot.git ~/ardupilot" +echo " cd ~/ardupilot" +echo " pip install -r Tools/environment_install/requirements.txt" +echo "" +echo " # Clone and build ArduPilot Gazebo plugin" +echo " git clone https://github.com/ArduPilot/ardupilot_gazebo.git ~/ardupilot_gazebo" +echo " cd ~/ardupilot_gazebo && mkdir build && cd build" +echo " cmake .. && make -j$(nproc)" +echo "" +echo " # Add to environment" +echo " export ARDUPILOT_HOME=~/ardupilot" +echo " export PATH=\$PATH:\$ARDUPILOT_HOME/Tools/autotest" +echo " export GZ_SIM_SYSTEM_PLUGIN_PATH=~/ardupilot_gazebo/build:\$GZ_SIM_SYSTEM_PLUGIN_PATH" +echo "" +echo "Then run:" +echo " python run_ardupilot.py --pattern circular" +echo "" diff --git a/setup/install_ubuntu.sh b/setup/install_ubuntu.sh index 63c15b9..bcaeb0e 100755 --- a/setup/install_ubuntu.sh +++ b/setup/install_ubuntu.sh @@ -3,8 +3,9 @@ # Drone Simulation - Ubuntu/Debian Installation Script # ============================================================================= # Installs ROS 2, Gazebo, PyBullet, and all required dependencies +# Includes optional ArduPilot SITL setup for realistic flight controller # -# Usage: ./install_ubuntu.sh +# Usage: ./install_ubuntu.sh [--with-ardupilot] # ============================================================================= set -e @@ -47,10 +48,20 @@ sudo apt-get install -y \ python3 \ python3-pip \ python3-venv \ - git + git \ + cmake \ + build-essential echo "[INFO] System dependencies installed" +# Check for ArduPilot option +INSTALL_ARDUPILOT=false +for arg in "$@"; do + if [ "$arg" = "--with-ardupilot" ]; then + INSTALL_ARDUPILOT=true + fi +done + # ----------------------------------------------------------------------------- # Step 2: ROS 2 Repository Setup # ----------------------------------------------------------------------------- @@ -190,10 +201,25 @@ fi # Set Gazebo model path export GZ_SIM_RESOURCE_PATH="$SCRIPT_DIR/gazebo/models:$GZ_SIM_RESOURCE_PATH" +# Set ArduPilot paths if installed +if [ -d "$HOME/ardupilot" ]; then + export ARDUPILOT_HOME="$HOME/ardupilot" + export PATH="$PATH:$ARDUPILOT_HOME/Tools/autotest" + echo "[OK] ArduPilot SITL available" +fi + +# Set ArduPilot Gazebo plugin path if installed +if [ -d "$HOME/ardupilot_gazebo/build" ]; then + export GZ_SIM_SYSTEM_PLUGIN_PATH="$HOME/ardupilot_gazebo/build:$GZ_SIM_SYSTEM_PLUGIN_PATH" + export GZ_SIM_RESOURCE_PATH="$HOME/ardupilot_gazebo/models:$HOME/ardupilot_gazebo/worlds:$GZ_SIM_RESOURCE_PATH" + echo "[OK] ArduPilot Gazebo plugin available" +fi + echo "" echo "Environment ready! Run one of:" echo " python standalone_simulation.py (No ROS 2 required)" echo " python simulation_host.py (With ROS 2)" +echo " python run_ardupilot.py (With ArduPilot SITL)" echo "" EOF @@ -213,6 +239,66 @@ echo "Checking Python packages:" python3 -c "import pybullet; print(' PyBullet: OK')" || echo " PyBullet: FAILED" python3 -c "import numpy; print(' NumPy: OK')" || echo " NumPy: FAILED" python3 -c "from PIL import Image; print(' Pillow: OK')" || echo " Pillow: FAILED" +python3 -c "from pymavlink import mavutil; print(' pymavlink: OK')" || echo " pymavlink: FAILED" + +# ----------------------------------------------------------------------------- +# Step 9: Optional ArduPilot SITL Installation +# ----------------------------------------------------------------------------- +if [ "$INSTALL_ARDUPILOT" = true ]; then + echo "" + echo "[STEP 9] Installing ArduPilot SITL..." + + # Install ArduPilot dependencies + sudo apt-get install -y \ + python3-dev \ + python3-opencv \ + python3-wxgtk4.0 \ + python3-matplotlib \ + python3-lxml \ + libxml2-dev \ + libxslt1-dev || true + + # Clone ArduPilot if not exists + if [ ! -d "$HOME/ardupilot" ]; then + echo "[INFO] Cloning ArduPilot..." + git clone --recurse-submodules https://github.com/ArduPilot/ardupilot.git "$HOME/ardupilot" + cd "$HOME/ardupilot" + Tools/environment_install/install-prereqs-ubuntu.sh -y + cd "$PROJECT_ROOT" + else + echo "[INFO] ArduPilot already exists at $HOME/ardupilot" + fi + + # Clone ArduPilot Gazebo plugin if not exists + if [ ! -d "$HOME/ardupilot_gazebo" ]; then + echo "[INFO] Cloning ArduPilot Gazebo plugin..." + git clone https://github.com/ArduPilot/ardupilot_gazebo.git "$HOME/ardupilot_gazebo" + cd "$HOME/ardupilot_gazebo" + mkdir -p build && cd build + cmake .. -DCMAKE_BUILD_TYPE=Release + make -j$(nproc) + cd "$PROJECT_ROOT" + else + echo "[INFO] ArduPilot Gazebo plugin already exists at $HOME/ardupilot_gazebo" + fi + + # Add to bashrc + if ! grep -q "ARDUPILOT_HOME" ~/.bashrc; then + echo '' >> ~/.bashrc + echo '# ArduPilot SITL' >> ~/.bashrc + echo 'export ARDUPILOT_HOME=$HOME/ardupilot' >> ~/.bashrc + echo 'export PATH=$PATH:$ARDUPILOT_HOME/Tools/autotest' >> ~/.bashrc + fi + + if ! grep -q "ardupilot_gazebo" ~/.bashrc; then + echo '' >> ~/.bashrc + echo '# ArduPilot Gazebo Plugin' >> ~/.bashrc + echo 'export GZ_SIM_SYSTEM_PLUGIN_PATH=$HOME/ardupilot_gazebo/build:$GZ_SIM_SYSTEM_PLUGIN_PATH' >> ~/.bashrc + echo 'export GZ_SIM_RESOURCE_PATH=$HOME/ardupilot_gazebo/models:$HOME/ardupilot_gazebo/worlds:$GZ_SIM_RESOURCE_PATH' >> ~/.bashrc + fi + + echo "[INFO] ArduPilot SITL installed" +fi echo "" echo "==============================================" @@ -223,8 +309,17 @@ echo "Quick start:" echo " source activate.sh" echo " python standalone_simulation.py" echo "" -echo "Or with ROS 2:" +echo "With ROS 2 + Gazebo:" echo " python simulation_host.py # Terminal 1" -echo " python ros_bridge.py # Terminal 2" -echo " python controllers.py # Terminal 3" +echo " python run_bridge.py # Terminal 2" echo "" +echo "With ArduPilot SITL (realistic flight controller):" +echo " ros2 launch gazebo/launch/ardupilot_drone.launch.py # Terminal 1" +echo " sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON # Terminal 2" +echo " python run_ardupilot.py --no-sitl # Terminal 3" +echo "" +if [ "$INSTALL_ARDUPILOT" != true ]; then + echo "To install ArduPilot SITL, run:" + echo " ./setup/install_ubuntu.sh --with-ardupilot" + echo "" +fi