Ardupilot Update
This commit is contained in:
27
README.md
27
README.md
@@ -1,6 +1,9 @@
|
|||||||
# Drone Landing Simulation (GPS-Denied)
|
# 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
|
## Quick Start
|
||||||
|
|
||||||
@@ -37,6 +40,24 @@ ros2 launch gazebo/launch/drone_landing.launch.py
|
|||||||
python run_gazebo.py --pattern circular --speed 0.3
|
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
|
## Installation
|
||||||
|
|
||||||
| Platform | Command |
|
| Platform | Command |
|
||||||
@@ -53,6 +74,7 @@ python run_gazebo.py --pattern circular --speed 0.3
|
|||||||
| Standalone | ✅ | ✅ | ✅ | ✅ | ✅ |
|
| Standalone | ✅ | ✅ | ✅ | ✅ | ✅ |
|
||||||
| PyBullet + ROS 2 | ✅ | ⚠️ | ❌ | ❌ | ✅ |
|
| PyBullet + ROS 2 | ✅ | ⚠️ | ❌ | ❌ | ✅ |
|
||||||
| Gazebo + ROS 2 | ✅ | ⚠️ | ❌ | ❌ | ✅ |
|
| Gazebo + ROS 2 | ✅ | ⚠️ | ❌ | ❌ | ✅ |
|
||||||
|
| ArduPilot + Gazebo | ✅ | ⚠️ | ❌ | ❌ | ✅ |
|
||||||
|
|
||||||
## Files
|
## Files
|
||||||
|
|
||||||
@@ -62,6 +84,8 @@ python run_gazebo.py --pattern circular --speed 0.3
|
|||||||
| `simulation_host.py` | PyBullet simulator server |
|
| `simulation_host.py` | PyBullet simulator server |
|
||||||
| `run_bridge.py` | PyBullet bridge + Controllers |
|
| `run_bridge.py` | PyBullet bridge + Controllers |
|
||||||
| `run_gazebo.py` | Gazebo 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) |
|
| `config.py` | **Configuration file** (edit to customize) |
|
||||||
| `drone_controller.py` | **Your landing algorithm** (edit this!) |
|
| `drone_controller.py` | **Your landing algorithm** (edit this!) |
|
||||||
| `rover_controller.py` | Moving landing pad controller |
|
| `rover_controller.py` | Moving landing pad controller |
|
||||||
@@ -111,6 +135,7 @@ Options:
|
|||||||
| [Architecture](docs/architecture.md) | System components diagram |
|
| [Architecture](docs/architecture.md) | System components diagram |
|
||||||
| [Gazebo Guide](docs/gazebo.md) | Gazebo-specific instructions |
|
| [Gazebo Guide](docs/gazebo.md) | Gazebo-specific instructions |
|
||||||
| [PyBullet Guide](docs/pybullet.md) | PyBullet-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 |
|
| [Protocol](docs/protocol.md) | Sensor data formats |
|
||||||
| [Drone Guide](docs/drone_guide.md) | Landing algorithm guide |
|
| [Drone Guide](docs/drone_guide.md) | Landing algorithm guide |
|
||||||
|
|
||||||
|
|||||||
46
config.py
46
config.py
@@ -101,3 +101,49 @@ CONTROLLER = {
|
|||||||
# Control rate
|
# Control rate
|
||||||
"rate": 50, # Control loop frequency (Hz)
|
"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,
|
||||||
|
}
|
||||||
|
|||||||
@@ -62,6 +62,34 @@ Data flow:
|
|||||||
- DroneController receives telemetry, publishes to `/cmd_vel`
|
- DroneController receives telemetry, publishes to `/cmd_vel`
|
||||||
- GazeboBridge forwards to `/drone/cmd_vel` → Gazebo moves drone
|
- 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
|
## Components
|
||||||
|
|
||||||
| File | Description |
|
| File | Description |
|
||||||
@@ -71,12 +99,16 @@ Data flow:
|
|||||||
| `simulation_host.py` | PyBullet physics server (UDP) |
|
| `simulation_host.py` | PyBullet physics server (UDP) |
|
||||||
| `run_bridge.py` | PyBullet bridge + controllers |
|
| `run_bridge.py` | PyBullet bridge + controllers |
|
||||||
| `run_gazebo.py` | Gazebo 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** |
|
| `drone_controller.py` | **Your landing algorithm** |
|
||||||
| `rover_controller.py` | Moving landing pad |
|
| `rover_controller.py` | Moving landing pad |
|
||||||
| `ros_bridge.py` | ROS-UDP bridge (used by run_bridge.py) |
|
| `ros_bridge.py` | ROS-UDP bridge (used by run_bridge.py) |
|
||||||
| `gazebo_bridge.py` | Gazebo-ROS bridge (used by run_gazebo.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/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
|
## ROS 2 Topics
|
||||||
|
|
||||||
|
|||||||
285
docs/ardupilot.md
Normal file
285
docs/ardupilot.md
Normal file
@@ -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 |
|
||||||
@@ -7,6 +7,7 @@ Setup instructions for all supported platforms.
|
|||||||
| Platform | Command |
|
| Platform | Command |
|
||||||
|----------|---------|
|
|----------|---------|
|
||||||
| Ubuntu/Debian | `./setup/install_ubuntu.sh` |
|
| Ubuntu/Debian | `./setup/install_ubuntu.sh` |
|
||||||
|
| Ubuntu + ArduPilot | `./setup/install_ubuntu.sh --with-ardupilot` |
|
||||||
| Arch Linux | `./setup/install_arch.sh` |
|
| Arch Linux | `./setup/install_arch.sh` |
|
||||||
| macOS | `./setup/install_macos.sh` |
|
| macOS | `./setup/install_macos.sh` |
|
||||||
| Windows | `.\setup\install_windows.ps1` |
|
| Windows | `.\setup\install_windows.ps1` |
|
||||||
@@ -28,6 +29,7 @@ python standalone_simulation.py
|
|||||||
| **Standalone Simulation** | ✅ | ✅ | ✅ | ✅ | ✅ |
|
| **Standalone Simulation** | ✅ | ✅ | ✅ | ✅ | ✅ |
|
||||||
| **ROS 2** | ✅ | ⚠️ AUR | ❌ | ❌ | ✅ |
|
| **ROS 2** | ✅ | ⚠️ AUR | ❌ | ❌ | ✅ |
|
||||||
| **Gazebo** | ✅ | ⚠️ AUR | ❌ | ❌ | ✅ |
|
| **Gazebo** | ✅ | ⚠️ AUR | ❌ | ❌ | ✅ |
|
||||||
|
| **ArduPilot SITL** | ✅ | ⚠️ Manual | ❌ | ❌ | ✅ |
|
||||||
| **Full Mode** | ✅ | ⚠️ | ❌ | ❌ | ✅ |
|
| **Full Mode** | ✅ | ⚠️ | ❌ | ❌ | ✅ |
|
||||||
| **GUI Support** | ✅ | ✅ | ✅ | ✅ | ✅ WSLg |
|
| **GUI Support** | ✅ | ✅ | ✅ | ✅ | ✅ WSLg |
|
||||||
|
|
||||||
@@ -58,7 +60,18 @@ python standalone_simulation.py
|
|||||||
**Installs:**
|
**Installs:**
|
||||||
- ROS 2 (Humble or Jazzy based on Ubuntu version)
|
- ROS 2 (Humble or Jazzy based on Ubuntu version)
|
||||||
- Gazebo (ros-gz)
|
- 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 pybullet; print('PyBullet OK')"
|
||||||
python -c "import numpy; print('NumPy OK')"
|
python -c "import numpy; print('NumPy OK')"
|
||||||
python -c "from PIL import Image; print('Pillow OK')"
|
python -c "from PIL import Image; print('Pillow OK')"
|
||||||
|
python -c "from pymavlink import mavutil; print('pymavlink OK')"
|
||||||
```
|
```
|
||||||
|
|
||||||
All should print "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).
|
||||||
|
|||||||
@@ -174,6 +174,9 @@ class DroneController(Node):
|
|||||||
|
|
||||||
landing_pad = telemetry.get('landing_pad', None)
|
landing_pad = telemetry.get('landing_pad', None)
|
||||||
|
|
||||||
|
# camera = telemetry.get('camera', None)
|
||||||
|
|
||||||
|
|
||||||
# Descent control
|
# Descent control
|
||||||
if altitude > 1.0:
|
if altitude > 1.0:
|
||||||
target_descent_rate = -0.5
|
target_descent_rate = -0.5
|
||||||
|
|||||||
134
gazebo/launch/ardupilot_drone.launch.py
Normal file
134
gazebo/launch/ardupilot_drone.launch.py
Normal file
@@ -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")
|
||||||
504
gazebo/worlds/ardupilot_drone.sdf
Normal file
504
gazebo/worlds/ardupilot_drone.sdf
Normal file
@@ -0,0 +1,504 @@
|
|||||||
|
<?xml version="1.0" ?>
|
||||||
|
<!--
|
||||||
|
ArduPilot SITL + Gazebo Drone Landing World
|
||||||
|
|
||||||
|
This world integrates with ArduPilot SITL via the ardupilot_gazebo plugin.
|
||||||
|
The drone receives motor commands from ArduPilot and sends sensor data back.
|
||||||
|
|
||||||
|
Prerequisites:
|
||||||
|
1. ArduPilot Gazebo Plugin: https://github.com/ArduPilot/ardupilot_gazebo
|
||||||
|
2. ArduPilot SITL with JSON backend
|
||||||
|
|
||||||
|
Usage:
|
||||||
|
1. Start Gazebo: gz sim -r gazebo/worlds/ardupilot_drone.sdf
|
||||||
|
2. Start SITL: sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON
|
||||||
|
3. Run bridge: python run_ardupilot.py --no-sitl
|
||||||
|
-->
|
||||||
|
<sdf version="1.9">
|
||||||
|
<world name="ardupilot_landing_world">
|
||||||
|
|
||||||
|
<!-- Physics - match ArduPilot defaults -->
|
||||||
|
<physics name="1ms" type="ode">
|
||||||
|
<max_step_size>0.001</max_step_size>
|
||||||
|
<real_time_factor>1.0</real_time_factor>
|
||||||
|
<real_time_update_rate>1000</real_time_update_rate>
|
||||||
|
</physics>
|
||||||
|
|
||||||
|
<!-- Gazebo Plugins -->
|
||||||
|
<plugin filename="gz-sim-physics-system"
|
||||||
|
name="gz::sim::systems::Physics"/>
|
||||||
|
<plugin filename="gz-sim-user-commands-system"
|
||||||
|
name="gz::sim::systems::UserCommands"/>
|
||||||
|
<plugin filename="gz-sim-scene-broadcaster-system"
|
||||||
|
name="gz::sim::systems::SceneBroadcaster"/>
|
||||||
|
<plugin filename="gz-sim-sensors-system"
|
||||||
|
name="gz::sim::systems::Sensors">
|
||||||
|
<render_engine>ogre2</render_engine>
|
||||||
|
</plugin>
|
||||||
|
<plugin filename="gz-sim-imu-system"
|
||||||
|
name="gz::sim::systems::Imu"/>
|
||||||
|
<plugin filename="gz-sim-air-pressure-system"
|
||||||
|
name="gz::sim::systems::AirPressure"/>
|
||||||
|
<plugin filename="gz-sim-altimeter-system"
|
||||||
|
name="gz::sim::systems::Altimeter"/>
|
||||||
|
<plugin filename="gz-sim-navsat-system"
|
||||||
|
name="gz::sim::systems::NavSat"/>
|
||||||
|
|
||||||
|
<!-- Lighting -->
|
||||||
|
<light type="directional" name="sun">
|
||||||
|
<cast_shadows>true</cast_shadows>
|
||||||
|
<pose>0 0 10 0 0 0</pose>
|
||||||
|
<diffuse>0.8 0.8 0.8 1</diffuse>
|
||||||
|
<specular>0.2 0.2 0.2 1</specular>
|
||||||
|
<direction>-0.5 0.1 -0.9</direction>
|
||||||
|
</light>
|
||||||
|
|
||||||
|
<light type="directional" name="fill_light">
|
||||||
|
<cast_shadows>false</cast_shadows>
|
||||||
|
<pose>0 0 10 0 0 0</pose>
|
||||||
|
<diffuse>0.3 0.3 0.3 1</diffuse>
|
||||||
|
<specular>0.0 0.0 0.0 1</specular>
|
||||||
|
<direction>0.5 0.3 -0.5</direction>
|
||||||
|
</light>
|
||||||
|
|
||||||
|
<!-- Ground Plane -->
|
||||||
|
<model name="ground_plane">
|
||||||
|
<static>true</static>
|
||||||
|
<link name="link">
|
||||||
|
<collision name="collision">
|
||||||
|
<geometry>
|
||||||
|
<plane>
|
||||||
|
<normal>0 0 1</normal>
|
||||||
|
<size>100 100</size>
|
||||||
|
</plane>
|
||||||
|
</geometry>
|
||||||
|
<surface>
|
||||||
|
<friction>
|
||||||
|
<ode>
|
||||||
|
<mu>1</mu>
|
||||||
|
<mu2>1</mu2>
|
||||||
|
</ode>
|
||||||
|
</friction>
|
||||||
|
</surface>
|
||||||
|
</collision>
|
||||||
|
<visual name="visual">
|
||||||
|
<geometry>
|
||||||
|
<plane>
|
||||||
|
<normal>0 0 1</normal>
|
||||||
|
<size>100 100</size>
|
||||||
|
</plane>
|
||||||
|
</geometry>
|
||||||
|
<material>
|
||||||
|
<ambient>0.5 0.5 0.5 1</ambient>
|
||||||
|
<diffuse>0.7 0.7 0.7 1</diffuse>
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
</link>
|
||||||
|
</model>
|
||||||
|
|
||||||
|
<!-- Landing Pad (Rover) - Velocity controlled for moving target -->
|
||||||
|
<model name="landing_pad">
|
||||||
|
<pose>0 0 0.15 0 0 0</pose>
|
||||||
|
|
||||||
|
<link name="base_link">
|
||||||
|
<inertial>
|
||||||
|
<mass>10.0</mass>
|
||||||
|
<inertia>
|
||||||
|
<ixx>1.0</ixx><ixy>0</ixy><ixz>0</ixz>
|
||||||
|
<iyy>1.0</iyy><iyz>0</iyz>
|
||||||
|
<izz>1.0</izz>
|
||||||
|
</inertia>
|
||||||
|
</inertial>
|
||||||
|
|
||||||
|
<visual name="visual">
|
||||||
|
<geometry>
|
||||||
|
<box><size>1.0 1.0 0.3</size></box>
|
||||||
|
</geometry>
|
||||||
|
<material>
|
||||||
|
<ambient>0.1 0.5 0.1 1</ambient>
|
||||||
|
<diffuse>0.2 0.7 0.2 1</diffuse>
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision name="collision">
|
||||||
|
<geometry>
|
||||||
|
<box><size>1.0 1.0 0.3</size></box>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
|
||||||
|
<!-- H marker -->
|
||||||
|
<visual name="h_center">
|
||||||
|
<pose>0 0 0.151 0 0 0</pose>
|
||||||
|
<geometry><box><size>0.6 0.1 0.001</size></box></geometry>
|
||||||
|
<material>
|
||||||
|
<ambient>1 1 1 1</ambient>
|
||||||
|
<diffuse>1 1 1 1</diffuse>
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<visual name="h_left">
|
||||||
|
<pose>-0.25 0 0.151 0 0 0</pose>
|
||||||
|
<geometry><box><size>0.1 0.5 0.001</size></box></geometry>
|
||||||
|
<material>
|
||||||
|
<ambient>1 1 1 1</ambient>
|
||||||
|
<diffuse>1 1 1 1</diffuse>
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<visual name="h_right">
|
||||||
|
<pose>0.25 0 0.151 0 0 0</pose>
|
||||||
|
<geometry><box><size>0.1 0.5 0.001</size></box></geometry>
|
||||||
|
<material>
|
||||||
|
<ambient>1 1 1 1</ambient>
|
||||||
|
<diffuse>1 1 1 1</diffuse>
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<!-- Velocity control for rover -->
|
||||||
|
<plugin filename="gz-sim-velocity-control-system"
|
||||||
|
name="gz::sim::systems::VelocityControl">
|
||||||
|
<topic>/rover/cmd_vel</topic>
|
||||||
|
<initial_linear>0 0 0</initial_linear>
|
||||||
|
</plugin>
|
||||||
|
</model>
|
||||||
|
|
||||||
|
<!--
|
||||||
|
ArduPilot Iris Quadcopter
|
||||||
|
|
||||||
|
This model uses the ArduPilot Gazebo plugin for flight control.
|
||||||
|
Motor commands come from ArduPilot SITL, and sensor data is sent back.
|
||||||
|
-->
|
||||||
|
<model name="iris_with_ardupilot">
|
||||||
|
<pose>0 0 0.194923 0 0 0</pose>
|
||||||
|
|
||||||
|
<link name="base_link">
|
||||||
|
<inertial>
|
||||||
|
<pose>0 0 0 0 0 0</pose>
|
||||||
|
<mass>1.5</mass>
|
||||||
|
<inertia>
|
||||||
|
<ixx>0.029125</ixx>
|
||||||
|
<ixy>0</ixy>
|
||||||
|
<ixz>0</ixz>
|
||||||
|
<iyy>0.029125</iyy>
|
||||||
|
<iyz>0</iyz>
|
||||||
|
<izz>0.055225</izz>
|
||||||
|
</inertia>
|
||||||
|
</inertial>
|
||||||
|
|
||||||
|
<collision name="base_link_collision">
|
||||||
|
<geometry>
|
||||||
|
<box><size>0.47 0.47 0.11</size></box>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
|
||||||
|
<!-- Main body -->
|
||||||
|
<visual name="base_link_visual">
|
||||||
|
<geometry>
|
||||||
|
<box><size>0.3 0.3 0.1</size></box>
|
||||||
|
</geometry>
|
||||||
|
<material>
|
||||||
|
<ambient>0.8 0.1 0.1 1</ambient>
|
||||||
|
<diffuse>0.9 0.2 0.2 1</diffuse>
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
|
||||||
|
<!-- Arms -->
|
||||||
|
<visual name="arm_fl">
|
||||||
|
<pose>0.12 0.12 0 0 0 0.785</pose>
|
||||||
|
<geometry><box><size>0.2 0.02 0.02</size></box></geometry>
|
||||||
|
<material><diffuse>0.3 0.3 0.3 1</diffuse></material>
|
||||||
|
</visual>
|
||||||
|
<visual name="arm_fr">
|
||||||
|
<pose>0.12 -0.12 0 0 0 -0.785</pose>
|
||||||
|
<geometry><box><size>0.2 0.02 0.02</size></box></geometry>
|
||||||
|
<material><diffuse>0.3 0.3 0.3 1</diffuse></material>
|
||||||
|
</visual>
|
||||||
|
<visual name="arm_bl">
|
||||||
|
<pose>-0.12 0.12 0 0 0 -0.785</pose>
|
||||||
|
<geometry><box><size>0.2 0.02 0.02</size></box></geometry>
|
||||||
|
<material><diffuse>0.3 0.3 0.3 1</diffuse></material>
|
||||||
|
</visual>
|
||||||
|
<visual name="arm_br">
|
||||||
|
<pose>-0.12 -0.12 0 0 0 0.785</pose>
|
||||||
|
<geometry><box><size>0.2 0.02 0.02</size></box></geometry>
|
||||||
|
<material><diffuse>0.3 0.3 0.3 1</diffuse></material>
|
||||||
|
</visual>
|
||||||
|
|
||||||
|
<!-- GPS antenna indicator -->
|
||||||
|
<visual name="gps_antenna">
|
||||||
|
<pose>0 0 0.08 0 0 0</pose>
|
||||||
|
<geometry><cylinder><radius>0.02</radius><length>0.04</length></cylinder></geometry>
|
||||||
|
<material><diffuse>0.2 0.2 0.8 1</diffuse></material>
|
||||||
|
</visual>
|
||||||
|
|
||||||
|
<!-- IMU Sensor -->
|
||||||
|
<sensor name="imu_sensor" type="imu">
|
||||||
|
<always_on>true</always_on>
|
||||||
|
<update_rate>400</update_rate>
|
||||||
|
<topic>imu</topic>
|
||||||
|
<imu>
|
||||||
|
<angular_velocity>
|
||||||
|
<x><noise type="gaussian"><mean>0</mean><stddev>0.0002</stddev></noise></x>
|
||||||
|
<y><noise type="gaussian"><mean>0</mean><stddev>0.0002</stddev></noise></y>
|
||||||
|
<z><noise type="gaussian"><mean>0</mean><stddev>0.0002</stddev></noise></z>
|
||||||
|
</angular_velocity>
|
||||||
|
<linear_acceleration>
|
||||||
|
<x><noise type="gaussian"><mean>0</mean><stddev>0.017</stddev></noise></x>
|
||||||
|
<y><noise type="gaussian"><mean>0</mean><stddev>0.017</stddev></noise></y>
|
||||||
|
<z><noise type="gaussian"><mean>0</mean><stddev>0.017</stddev></noise></z>
|
||||||
|
</linear_acceleration>
|
||||||
|
</imu>
|
||||||
|
</sensor>
|
||||||
|
|
||||||
|
<!-- Air Pressure Sensor (Barometer) -->
|
||||||
|
<sensor name="air_pressure" type="air_pressure">
|
||||||
|
<always_on>true</always_on>
|
||||||
|
<update_rate>50</update_rate>
|
||||||
|
<topic>air_pressure</topic>
|
||||||
|
<air_pressure>
|
||||||
|
<reference_altitude>0</reference_altitude>
|
||||||
|
<noise type="gaussian">
|
||||||
|
<mean>0</mean>
|
||||||
|
<stddev>0.01</stddev>
|
||||||
|
</noise>
|
||||||
|
</air_pressure>
|
||||||
|
</sensor>
|
||||||
|
|
||||||
|
<!-- Downward Camera for Landing Pad Detection -->
|
||||||
|
<sensor name="camera" type="camera">
|
||||||
|
<pose>0 0 -0.05 0 1.5708 0</pose>
|
||||||
|
<always_on>true</always_on>
|
||||||
|
<update_rate>30</update_rate>
|
||||||
|
<topic>/drone/camera</topic>
|
||||||
|
<camera>
|
||||||
|
<horizontal_fov>1.047</horizontal_fov>
|
||||||
|
<image>
|
||||||
|
<width>320</width>
|
||||||
|
<height>240</height>
|
||||||
|
<format>R8G8B8</format>
|
||||||
|
</image>
|
||||||
|
<clip>
|
||||||
|
<near>0.1</near>
|
||||||
|
<far>100</far>
|
||||||
|
</clip>
|
||||||
|
</camera>
|
||||||
|
</sensor>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<!-- Rotor 0: Front Right (CCW) -->
|
||||||
|
<link name="rotor_0">
|
||||||
|
<pose>0.13 -0.22 0.023 0 0 0</pose>
|
||||||
|
<inertial>
|
||||||
|
<mass>0.025</mass>
|
||||||
|
<inertia>
|
||||||
|
<ixx>9.75e-06</ixx><ixy>0</ixy><ixz>0</ixz>
|
||||||
|
<iyy>1.66704e-04</iyy><iyz>0</iyz>
|
||||||
|
<izz>1.66704e-04</izz>
|
||||||
|
</inertia>
|
||||||
|
</inertial>
|
||||||
|
<visual name="visual">
|
||||||
|
<geometry><cylinder><radius>0.1</radius><length>0.005</length></cylinder></geometry>
|
||||||
|
<material><diffuse>0.1 0.1 0.1 1</diffuse></material>
|
||||||
|
</visual>
|
||||||
|
<collision name="collision">
|
||||||
|
<geometry><cylinder><radius>0.1</radius><length>0.005</length></cylinder></geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<joint name="rotor_0_joint" type="revolute">
|
||||||
|
<parent>base_link</parent>
|
||||||
|
<child>rotor_0</child>
|
||||||
|
<axis>
|
||||||
|
<xyz>0 0 1</xyz>
|
||||||
|
<limit><lower>-1e16</lower><upper>1e16</upper></limit>
|
||||||
|
</axis>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<!-- Rotor 1: Back Left (CCW) -->
|
||||||
|
<link name="rotor_1">
|
||||||
|
<pose>-0.13 0.2 0.023 0 0 0</pose>
|
||||||
|
<inertial>
|
||||||
|
<mass>0.025</mass>
|
||||||
|
<inertia>
|
||||||
|
<ixx>9.75e-06</ixx><ixy>0</ixy><ixz>0</ixz>
|
||||||
|
<iyy>1.66704e-04</iyy><iyz>0</iyz>
|
||||||
|
<izz>1.66704e-04</izz>
|
||||||
|
</inertia>
|
||||||
|
</inertial>
|
||||||
|
<visual name="visual">
|
||||||
|
<geometry><cylinder><radius>0.1</radius><length>0.005</length></cylinder></geometry>
|
||||||
|
<material><diffuse>0.1 0.1 0.1 1</diffuse></material>
|
||||||
|
</visual>
|
||||||
|
<collision name="collision">
|
||||||
|
<geometry><cylinder><radius>0.1</radius><length>0.005</length></cylinder></geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<joint name="rotor_1_joint" type="revolute">
|
||||||
|
<parent>base_link</parent>
|
||||||
|
<child>rotor_1</child>
|
||||||
|
<axis>
|
||||||
|
<xyz>0 0 1</xyz>
|
||||||
|
<limit><lower>-1e16</lower><upper>1e16</upper></limit>
|
||||||
|
</axis>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<!-- Rotor 2: Front Left (CW) -->
|
||||||
|
<link name="rotor_2">
|
||||||
|
<pose>0.13 0.22 0.023 0 0 0</pose>
|
||||||
|
<inertial>
|
||||||
|
<mass>0.025</mass>
|
||||||
|
<inertia>
|
||||||
|
<ixx>9.75e-06</ixx><ixy>0</ixy><ixz>0</ixz>
|
||||||
|
<iyy>1.66704e-04</iyy><iyz>0</iyz>
|
||||||
|
<izz>1.66704e-04</izz>
|
||||||
|
</inertia>
|
||||||
|
</inertial>
|
||||||
|
<visual name="visual">
|
||||||
|
<geometry><cylinder><radius>0.1</radius><length>0.005</length></cylinder></geometry>
|
||||||
|
<material><diffuse>0.1 0.1 0.1 1</diffuse></material>
|
||||||
|
</visual>
|
||||||
|
<collision name="collision">
|
||||||
|
<geometry><cylinder><radius>0.1</radius><length>0.005</length></cylinder></geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<joint name="rotor_2_joint" type="revolute">
|
||||||
|
<parent>base_link</parent>
|
||||||
|
<child>rotor_2</child>
|
||||||
|
<axis>
|
||||||
|
<xyz>0 0 1</xyz>
|
||||||
|
<limit><lower>-1e16</lower><upper>1e16</upper></limit>
|
||||||
|
</axis>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<!-- Rotor 3: Back Right (CW) -->
|
||||||
|
<link name="rotor_3">
|
||||||
|
<pose>-0.13 -0.2 0.023 0 0 0</pose>
|
||||||
|
<inertial>
|
||||||
|
<mass>0.025</mass>
|
||||||
|
<inertia>
|
||||||
|
<ixx>9.75e-06</ixx><ixy>0</ixy><ixz>0</ixz>
|
||||||
|
<iyy>1.66704e-04</iyy><iyz>0</iyz>
|
||||||
|
<izz>1.66704e-04</izz>
|
||||||
|
</inertia>
|
||||||
|
</inertial>
|
||||||
|
<visual name="visual">
|
||||||
|
<geometry><cylinder><radius>0.1</radius><length>0.005</length></cylinder></geometry>
|
||||||
|
<material><diffuse>0.1 0.1 0.1 1</diffuse></material>
|
||||||
|
</visual>
|
||||||
|
<collision name="collision">
|
||||||
|
<geometry><cylinder><radius>0.1</radius><length>0.005</length></cylinder></geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<joint name="rotor_3_joint" type="revolute">
|
||||||
|
<parent>base_link</parent>
|
||||||
|
<child>rotor_3</child>
|
||||||
|
<axis>
|
||||||
|
<xyz>0 0 1</xyz>
|
||||||
|
<limit><lower>-1e16</lower><upper>1e16</upper></limit>
|
||||||
|
</axis>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<!--
|
||||||
|
ArduPilot Plugin Configuration
|
||||||
|
|
||||||
|
This plugin connects to ArduPilot SITL via the JSON interface.
|
||||||
|
It receives motor commands and sends back sensor data.
|
||||||
|
|
||||||
|
fdm_addr: Address of ArduPilot SITL (default 127.0.0.1)
|
||||||
|
fdm_port_in: Port to receive motor commands (default 9002)
|
||||||
|
listen_addr: Our address for sending sensor data (127.0.0.1)
|
||||||
|
listen_port_out: SITL port for receiving sensor data (9003)
|
||||||
|
|
||||||
|
If ardupilot_gazebo plugin is not available, this will be ignored.
|
||||||
|
-->
|
||||||
|
<plugin filename="ArduPilotPlugin"
|
||||||
|
name="gz::sim::systems::ArduPilotPlugin">
|
||||||
|
<!-- Connection settings -->
|
||||||
|
<fdm_addr>127.0.0.1</fdm_addr>
|
||||||
|
<fdm_port_in>9002</fdm_port_in>
|
||||||
|
<listen_addr>127.0.0.1</listen_addr>
|
||||||
|
<listen_port_out>9003</listen_port_out>
|
||||||
|
|
||||||
|
<!-- Model reference link -->
|
||||||
|
<modelXYZToAirplaneXForwardZDown>0 0 0 3.141593 0 0</modelXYZToAirplaneXForwardZDown>
|
||||||
|
<gazeboXYZToNED>0 0 0 3.141593 0 0</gazeboXYZToNED>
|
||||||
|
|
||||||
|
<!-- IMU configuration -->
|
||||||
|
<imuName>imu_sensor</imuName>
|
||||||
|
|
||||||
|
<!-- Motor configuration (CCW positive, CW negative) -->
|
||||||
|
<!-- Rotor 0: FR CCW -->
|
||||||
|
<control channel="0">
|
||||||
|
<jointName>rotor_0_joint</jointName>
|
||||||
|
<type>VELOCITY</type>
|
||||||
|
<offset>0</offset>
|
||||||
|
<multiplier>838</multiplier>
|
||||||
|
<p_gain>0.2</p_gain>
|
||||||
|
<i_gain>0</i_gain>
|
||||||
|
<d_gain>0</d_gain>
|
||||||
|
<i_max>0</i_max>
|
||||||
|
<i_min>0</i_min>
|
||||||
|
<cmd_max>2.5</cmd_max>
|
||||||
|
<cmd_min>-2.5</cmd_min>
|
||||||
|
</control>
|
||||||
|
|
||||||
|
<!-- Rotor 1: BL CCW -->
|
||||||
|
<control channel="1">
|
||||||
|
<jointName>rotor_1_joint</jointName>
|
||||||
|
<type>VELOCITY</type>
|
||||||
|
<offset>0</offset>
|
||||||
|
<multiplier>838</multiplier>
|
||||||
|
<p_gain>0.2</p_gain>
|
||||||
|
<i_gain>0</i_gain>
|
||||||
|
<d_gain>0</d_gain>
|
||||||
|
<i_max>0</i_max>
|
||||||
|
<i_min>0</i_min>
|
||||||
|
<cmd_max>2.5</cmd_max>
|
||||||
|
<cmd_min>-2.5</cmd_min>
|
||||||
|
</control>
|
||||||
|
|
||||||
|
<!-- Rotor 2: FL CW -->
|
||||||
|
<control channel="2">
|
||||||
|
<jointName>rotor_2_joint</jointName>
|
||||||
|
<type>VELOCITY</type>
|
||||||
|
<offset>0</offset>
|
||||||
|
<multiplier>-838</multiplier>
|
||||||
|
<p_gain>0.2</p_gain>
|
||||||
|
<i_gain>0</i_gain>
|
||||||
|
<d_gain>0</d_gain>
|
||||||
|
<i_max>0</i_max>
|
||||||
|
<i_min>0</i_min>
|
||||||
|
<cmd_max>2.5</cmd_max>
|
||||||
|
<cmd_min>-2.5</cmd_min>
|
||||||
|
</control>
|
||||||
|
|
||||||
|
<!-- Rotor 3: BR CW -->
|
||||||
|
<control channel="3">
|
||||||
|
<jointName>rotor_3_joint</jointName>
|
||||||
|
<type>VELOCITY</type>
|
||||||
|
<offset>0</offset>
|
||||||
|
<multiplier>-838</multiplier>
|
||||||
|
<p_gain>0.2</p_gain>
|
||||||
|
<i_gain>0</i_gain>
|
||||||
|
<d_gain>0</d_gain>
|
||||||
|
<i_max>0</i_max>
|
||||||
|
<i_min>0</i_min>
|
||||||
|
<cmd_max>2.5</cmd_max>
|
||||||
|
<cmd_min>-2.5</cmd_min>
|
||||||
|
</control>
|
||||||
|
</plugin>
|
||||||
|
|
||||||
|
<!-- Odometry publisher for compatibility -->
|
||||||
|
<plugin filename="gz-sim-odometry-publisher-system"
|
||||||
|
name="gz::sim::systems::OdometryPublisher">
|
||||||
|
<odom_frame>odom</odom_frame>
|
||||||
|
<robot_base_frame>base_link</robot_base_frame>
|
||||||
|
<odom_topic>/model/drone/odometry</odom_topic>
|
||||||
|
<odom_publish_frequency>50</odom_publish_frequency>
|
||||||
|
</plugin>
|
||||||
|
|
||||||
|
</model>
|
||||||
|
|
||||||
|
</world>
|
||||||
|
</sdf>
|
||||||
580
mavlink_bridge.py
Normal file
580
mavlink_bridge.py
Normal file
@@ -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()
|
||||||
@@ -2,3 +2,4 @@ pybullet>=3.2.5
|
|||||||
numpy>=1.20.0
|
numpy>=1.20.0
|
||||||
pillow>=9.0.0
|
pillow>=9.0.0
|
||||||
pyinstaller>=6.0.0
|
pyinstaller>=6.0.0
|
||||||
|
pymavlink>=2.4.0
|
||||||
|
|||||||
359
run_ardupilot.py
Normal file
359
run_ardupilot.py
Normal file
@@ -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())
|
||||||
@@ -4,6 +4,7 @@
|
|||||||
# =============================================================================
|
# =============================================================================
|
||||||
# Installs PyBullet and Python dependencies
|
# Installs PyBullet and Python dependencies
|
||||||
# ROS 2 requires AUR (optional - not needed for standalone mode)
|
# ROS 2 requires AUR (optional - not needed for standalone mode)
|
||||||
|
# ArduPilot SITL can be built from source (optional)
|
||||||
#
|
#
|
||||||
# Usage: ./install_arch.sh
|
# 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 pybullet; print(' PyBullet: OK')" || echo " PyBullet: FAILED"
|
||||||
python -c "import numpy; print(' NumPy: OK')" || echo " NumPy: 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 PIL import Image; print(' Pillow: OK')" || echo " Pillow: FAILED"
|
||||||
|
python -c "from pymavlink import mavutil; print(' pymavlink: OK')" || echo " pymavlink: FAILED"
|
||||||
|
|
||||||
echo ""
|
echo ""
|
||||||
echo "=============================================="
|
echo "=============================================="
|
||||||
@@ -185,4 +187,28 @@ echo "After installing, use:"
|
|||||||
echo " ign gazebo gazebo/worlds/drone_landing.sdf # Terminal 1"
|
echo " ign gazebo gazebo/worlds/drone_landing.sdf # Terminal 1"
|
||||||
echo " python run_gazebo.py --pattern circular # Terminal 2"
|
echo " python run_gazebo.py --pattern circular # Terminal 2"
|
||||||
echo ""
|
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 ""
|
||||||
|
|
||||||
|
|||||||
@@ -3,8 +3,9 @@
|
|||||||
# Drone Simulation - Ubuntu/Debian Installation Script
|
# Drone Simulation - Ubuntu/Debian Installation Script
|
||||||
# =============================================================================
|
# =============================================================================
|
||||||
# Installs ROS 2, Gazebo, PyBullet, and all required dependencies
|
# 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
|
set -e
|
||||||
@@ -47,10 +48,20 @@ sudo apt-get install -y \
|
|||||||
python3 \
|
python3 \
|
||||||
python3-pip \
|
python3-pip \
|
||||||
python3-venv \
|
python3-venv \
|
||||||
git
|
git \
|
||||||
|
cmake \
|
||||||
|
build-essential
|
||||||
|
|
||||||
echo "[INFO] System dependencies installed"
|
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
|
# Step 2: ROS 2 Repository Setup
|
||||||
# -----------------------------------------------------------------------------
|
# -----------------------------------------------------------------------------
|
||||||
@@ -190,10 +201,25 @@ fi
|
|||||||
# Set Gazebo model path
|
# Set Gazebo model path
|
||||||
export GZ_SIM_RESOURCE_PATH="$SCRIPT_DIR/gazebo/models:$GZ_SIM_RESOURCE_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 ""
|
||||||
echo "Environment ready! Run one of:"
|
echo "Environment ready! Run one of:"
|
||||||
echo " python standalone_simulation.py (No ROS 2 required)"
|
echo " python standalone_simulation.py (No ROS 2 required)"
|
||||||
echo " python simulation_host.py (With ROS 2)"
|
echo " python simulation_host.py (With ROS 2)"
|
||||||
|
echo " python run_ardupilot.py (With ArduPilot SITL)"
|
||||||
echo ""
|
echo ""
|
||||||
EOF
|
EOF
|
||||||
|
|
||||||
@@ -213,6 +239,66 @@ echo "Checking Python packages:"
|
|||||||
python3 -c "import pybullet; print(' PyBullet: OK')" || echo " PyBullet: FAILED"
|
python3 -c "import pybullet; print(' PyBullet: OK')" || echo " PyBullet: FAILED"
|
||||||
python3 -c "import numpy; print(' NumPy: OK')" || echo " NumPy: 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 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 ""
|
||||||
echo "=============================================="
|
echo "=============================================="
|
||||||
@@ -223,8 +309,17 @@ echo "Quick start:"
|
|||||||
echo " source activate.sh"
|
echo " source activate.sh"
|
||||||
echo " python standalone_simulation.py"
|
echo " python standalone_simulation.py"
|
||||||
echo ""
|
echo ""
|
||||||
echo "Or with ROS 2:"
|
echo "With ROS 2 + Gazebo:"
|
||||||
echo " python simulation_host.py # Terminal 1"
|
echo " python simulation_host.py # Terminal 1"
|
||||||
echo " python ros_bridge.py # Terminal 2"
|
echo " python run_bridge.py # Terminal 2"
|
||||||
echo " python controllers.py # Terminal 3"
|
|
||||||
echo ""
|
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
|
||||||
|
|||||||
Reference in New Issue
Block a user