SirBlob/RDC_Simulation
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: Simplify drone controller to basic flight pattern, overhaul installation guide, and update related scripts.

Browse Source
This commit is contained in:
default
2026-01-07 21:02:31 +00:00
parent 760293d896
commit e266f75fca
9 changed files with 919 additions and 1407 deletions
Download Patch File Download Diff File Expand all files Collapse all files

178
README.md
View File

@@ -1,121 +1,121 @@
# GPS-Denied Drone Landing Simulation # RDC Simulation
ArduPilot + ROS 2 + Gazebo (ARG) simulation for landing on a moving platform. GPS-denied drone landing simulation using ArduPilot and Gazebo.
## Quick Start (2 Terminals) ## Quick Start
**Terminal 1 - Gazebo:** ### Prerequisites
```bash - Ubuntu 22.04/24.04 (or WSL2 on Windows)
source activate.sh - Python 3.10+
./scripts/run_ardupilot_sim.sh runway - ~10GB disk space
```
**Terminal 2 - Controller:** ### Installation
```bash
source activate.sh
./scripts/run_ardupilot_controller.sh
```
## World Options
```bash ```bash
./scripts/run_ardupilot_sim.sh runway # Default (outdoor) # Clone the repository
./scripts/run_ardupilot_sim.sh warehouse # Indoor git clone <repo-url> RDC_Simulation
./scripts/run_ardupilot_sim.sh custom # Custom landing pad cd RDC_Simulation
./scripts/run_ardupilot_sim.sh my_world # gazebo/worlds/my_world.sdf
```
## Installation # Install everything (20-30 minutes)
./setup/install_ubuntu.sh --with-ardupilot
### Ubuntu (Native or WSL2) # Reload environment
```bash
./setup/install_ubuntu.sh
./setup/install_ardupilot.sh
source ~/.bashrc source ~/.bashrc
``` ```
### Windows (via WSL2) ### Run the Simulation
```powershell You need **3 terminals**:
# PowerShell as Administrator
wsl --install -d Ubuntu-24.04 **Terminal 1 - Gazebo:**
# Restart, then in Ubuntu terminal: ```bash
./setup/install_ubuntu.sh --with-ardupilot cd ~/RDC_Simulation
./scripts/run_ardupilot_sim.sh runway
# Wait until the drone appears in Gazebo
``` ```
### macOS **Terminal 2 - SITL:**
```bash
source ~/venv-ardupilot/bin/activate
sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --console
# Wait for "Waiting for connection" then telemetry
```
**Terminal 3 - Controller:**
```bash
source ~/venv-ardupilot/bin/activate
cd ~/RDC_Simulation
python scripts/run_ardupilot.py --pattern square
```
### Flight Patterns
```bash ```bash
./setup/install_macos.sh # Fly a square pattern (default)
python standalone_simulation.py # Gazebo not supported python scripts/run_ardupilot.py --pattern square --size 5
```
See [Installation Guide](docs/installation.md) for detailed platform instructions. # Fly a circle
python scripts/run_ardupilot.py --pattern circle --size 5
# Just hover in place
python scripts/run_ardupilot.py --pattern hover
# Custom altitude
python scripts/run_ardupilot.py --pattern square --altitude 10
```
## Project Structure ## Project Structure
``` ```
simulation/ RDC_Simulation/
├── config.py
├── src/
│ └── drone_controller.py # Your algorithm
├── scripts/ ├── scripts/
│ ├── run_ardupilot_sim.sh │ ├── run_ardupilot_sim.sh # Launch Gazebo
│ └── run_ardupilot_controller.sh │ ├── run_ardupilot_controller.sh # Launch SITL + Controller
├── gazebo/ │ └── run_ardupilot.py # Flight controller
│ ├── worlds/ # Your worlds ├── src/
│ └── models/ # Your models │ ├── drone_controller.py # Simple drone controller
│ ├── rover_controller.py # Moving landing pad (ROS 2)
│ └── camera_viewer.py # Camera viewer (ROS 2)
├── setup/ ├── setup/
└── docs/ │ ├── install_ubuntu.sh # Ubuntu installer
│ ├── install_ardupilot.sh # ArduPilot installer
│ └── install_arch.sh # Arch Linux installer
├── gazebo/
│ └── models/ # Custom Gazebo models
├── docs/ # Documentation
└── config.py # Configuration
``` ```
## 3-Phase Mission
| Phase | Action |
|-------|--------|
| SEARCH | Find QR code on rover |
| COMMAND | Send commands to rover |
| LAND | Land on rover |
## Environment Setup
Always activate the environment before running:
```bash
source activate.sh
```
This sets up:
- ROS 2 environment
- ArduPilot venv (with empy, pymavlink, etc.)
- Gazebo resource paths
- ArduPilot tools path
## Troubleshooting ## Troubleshooting
**sim_vehicle.py not found:** ### "No JSON sensor message received"
```bash Gazebo isn't sending data to SITL.
source ~/.ardupilot_env - **Fix**: Start Gazebo FIRST, wait for it to fully load, THEN start SITL.
```
**empy not found:** ### "empy not found"
```bash Wrong Python environment.
source ~/venv-ardupilot/bin/activate - **Fix**: `source ~/venv-ardupilot/bin/activate`
pip install empy==3.3.4
```
**Gazebo slow (software rendering):** ### "sim_vehicle.py not found"
```bash ArduPilot tools not in PATH.
glxinfo | grep "OpenGL renderer" - **Fix**: `source ~/.ardupilot_env`
# Should show GPU, not "llvmpipe"
``` ### Can't arm
The drone won't arm or takeoff.
- **Fix**: Make sure you're in GUIDED mode and Gazebo is running.
- Check console for pre-arm errors.
### Gazebo is slow
- Try a lighter world: `./scripts/run_ardupilot_sim.sh runway`
- Check GPU: `glxinfo | grep "OpenGL renderer"`
## Documentation ## Documentation
- [Installation](docs/installation.md) - Full setup for all platforms + WSL - [Installation Guide](docs/installation.md)
- [ArduPilot Guide](docs/ardupilot.md) - SITL setup and troubleshooting - [ArduPilot Guide](docs/ardupilot.md)
- [Drone Controller](docs/drone_guide.md) - Landing algorithm - [Architecture](docs/architecture.md)
- [Custom Worlds](docs/gazebo_worlds.md) - Create Gazebo environments - [Gazebo Worlds](docs/gazebo_worlds.md)
- [Blender Models](docs/blender_models.md) - 3D model workflow
- [Architecture](docs/architecture.md) - System design ## License
MIT License

282
docs/ardupilot.md
View File

@@ -1,202 +1,184 @@
# ArduPilot + Gazebo Simulation # ArduPilot + Gazebo Guide
## Prerequisites ## Overview
Before running, ensure your environment is set up: This project uses:
- **ArduPilot SITL**: Software-in-the-loop flight controller
```bash - **Gazebo**: 3D physics simulation
source activate.sh - **MAVLink**: Communication protocol
# or
source ~/.ardupilot_env && source ~/venv-ardupilot/bin/activate
```
## Quick Start (2 Terminals)
**Terminal 1 - Start Gazebo:**
```bash
./scripts/run_ardupilot_sim.sh runway
```
**Terminal 2 - Start Controller:**
```bash
./scripts/run_ardupilot_controller.sh
```
## Architecture ## Architecture
``` ```
┌─────────────────┐ ┌─────────────────┐ ┌─────────────────┐ ┌─────────────┐ JSON (UDP 9002) ┌─────────────┐ MAVLink (TCP 5760) ┌─────────────┐
│ Gazebo │◄───►│ ArduPilot SITL │◄───►│ run_ardupilot.py│ │ Gazebo │◄──────────────────────►│ ArduPilot │◄────────────────────────►│ Your │
│ (Physics) │JSON │ (Flight Ctrl) │MAV │ (Your Logic) │ │ (Physics) │ sensor data │ SITL │ commands/telemetry │ Controller │
└─────────────────┘ └─────────────────┘ └─────────────────┘ └─────────────┘ └─────────────┘ └─────────────┘
``` ```
**Communication:** ## Quick Start
- Gazebo ↔ SITL: JSON sensor data over UDP (port 9002)
- SITL ↔ Your Code: MAVLink over TCP (port 5760)
## World Options ### Step 1: Start Gazebo
```bash ```bash
./scripts/run_ardupilot_sim.sh runway # Default (outdoor runway) cd ~/RDC_Simulation
./scripts/run_ardupilot_sim.sh warehouse # Indoor warehouse ./scripts/run_ardupilot_sim.sh runway
./scripts/run_ardupilot_sim.sh gimbal # With camera gimbal
./scripts/run_ardupilot_sim.sh zephyr # Fixed-wing aircraft
./scripts/run_ardupilot_sim.sh custom # Your custom world
./scripts/run_ardupilot_sim.sh my_world # gazebo/worlds/my_world.sdf
``` ```
## GPS-Denied Mode Wait until the drone appears in the 3D view.
The simulation runs in GPS-denied mode by default. The controller disables GPS checks and uses visual navigation. ### Step 2: Start SITL
For manual debugging with MAVProxy:
```bash
sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --console
# In MAVProxy console:
param set ARMING_CHECK 0
mode stabilize
arm throttle force
rc 3 1500 # Throttle up
```
## Controller Options
```bash ```bash
./scripts/run_ardupilot_controller.sh # Auto takeoff
./scripts/run_ardupilot_controller.sh --no-takeoff # Manual control
./scripts/run_ardupilot_controller.sh -a 10 # 10m altitude
```
## Files
| File | Purpose |
|------|---------|
| `scripts/run_ardupilot_sim.sh` | Gazebo launcher with GPU detection |
| `scripts/run_ardupilot_controller.sh` | SITL + Controller launcher |
| `scripts/run_ardupilot.py` | MAVLink interface script |
| `src/drone_controller.py` | Your landing algorithm |
## Environment Variables
The scripts automatically set these, but for manual runs:
```bash
# ArduPilot tools
export PATH=$PATH:~/ardupilot/Tools/autotest
# ArduPilot venv (has empy, pymavlink, etc.)
source ~/venv-ardupilot/bin/activate source ~/venv-ardupilot/bin/activate
sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --console
```
# Gazebo plugin paths You should see telemetry scrolling (NOT "No JSON sensor message").
export GZ_SIM_SYSTEM_PLUGIN_PATH=~/ardupilot_gazebo/build:$GZ_SIM_SYSTEM_PLUGIN_PATH
export GZ_SIM_RESOURCE_PATH=~/ardupilot_gazebo/models:~/ardupilot_gazebo/worlds:$GZ_SIM_RESOURCE_PATH ### Step 3: Run Controller
```bash
source ~/venv-ardupilot/bin/activate
cd ~/RDC_Simulation
python scripts/run_ardupilot.py --pattern square
```
## Flight Patterns
```bash
# Square pattern (5m sides, 5m altitude)
python scripts/run_ardupilot.py --pattern square --size 5 --altitude 5
# Circle pattern (5m radius)
python scripts/run_ardupilot.py --pattern circle --size 5
# Hover in place for testing
python scripts/run_ardupilot.py --pattern hover
# Larger pattern at higher altitude
python scripts/run_ardupilot.py --pattern square --size 10 --altitude 15
```
## Controller Code
The flight controller is in `src/drone_controller.py`:
```python
from src.drone_controller import SimpleDroneController
drone = SimpleDroneController()
drone.connect()
drone.set_mode("GUIDED")
drone.arm()
drone.takeoff(5.0)
drone.fly_square(size=5, altitude=5)
drone.land()
```
### Key Methods
| Method | Description |
|--------|-------------|
| `connect()` | Connect to SITL via MAVLink |
| `set_mode(mode)` | Set flight mode (GUIDED, LAND, etc.) |
| `arm()` | Arm motors (force arm) |
| `takeoff(alt)` | Take off to altitude |
| `goto(x, y, z)` | Go to position (NED frame) |
| `fly_to_and_wait(x, y, alt)` | Go to position and wait |
| `fly_square(size, alt)` | Fly square pattern |
| `fly_circle(radius, alt)` | Fly circle pattern |
| `land()` | Land the drone |
## Available Worlds
```bash
# Outdoor runway (default)
./scripts/run_ardupilot_sim.sh runway
# Indoor warehouse
./scripts/run_ardupilot_sim.sh warehouse
```
## MAVLink Connection
The controller connects via TCP port 5760:
```python
# config.py
MAVLINK = {
"connection_string": "tcp:127.0.0.1:5760",
}
``` ```
## Troubleshooting ## Troubleshooting
### "No JSON sensor message" or "No heartbeat" ### "No JSON sensor message received"
**Cause:** Gazebo isn't running or ArduPilot plugin isn't loaded. SITL isn't receiving data from Gazebo.
**Fix:** **Fix:**
1. Start Gazebo first (Terminal 1) 1. Make sure Gazebo is running FIRST
2. Wait for world to fully load 2. Wait for the world to fully load
3. Then start controller (Terminal 2) 3. Then start SITL
### "sim_vehicle.py not found" ### Drone won't arm
**Cause:** ArduPilot tools not in PATH.
**Fix:** **Fix:**
```bash ```bash
source ~/.ardupilot_env # Check pre-arm status in MAVProxy
# or arm status
export PATH=$PATH:~/ardupilot/Tools/autotest
# Force arm
arm throttle force
``` ```
### "empy not found" during waf build ### Wrong Python environment
**Cause:** Wrong Python environment.
**Fix:** **Fix:**
```bash ```bash
source ~/venv-ardupilot/bin/activate source ~/venv-ardupilot/bin/activate
pip install empy==3.3.4 which python3 # Should be ~/venv-ardupilot/bin/python3
cd ~/ardupilot
./waf configure --board sitl
./waf copter
``` ```
### Drone doesn't respond to commands ### Gazebo plugin not loading
**Possible causes:**
1. Not in GUIDED mode - check MAVProxy: `mode`
2. Not armed - run: `arm throttle force`
3. Pre-arm checks failing - run: `param set ARMING_CHECK 0`
### Drone immediately crashes in Gazebo
**Cause:** Usually a physics/plugin issue.
**Fix:** **Fix:**
```bash ```bash
# Rebuild the ArduPilot Gazebo plugin # Check plugin exists
cd ~/ardupilot_gazebo/build ls ~/ardupilot_gazebo/build/libArduPilotPlugin.so
cmake .. -DCMAKE_BUILD_TYPE=Release
make -j4 # Check paths
echo $GZ_SIM_SYSTEM_PLUGIN_PATH
# Should include: ~/ardupilot_gazebo/build
``` ```
### Simulation is laggy ## Manual SITL Control
**Cause:** Software rendering or GPU not detected. Use MAVProxy commands:
**Check:**
```bash ```bash
glxinfo | grep "OpenGL renderer" # In SITL console
mode GUIDED
arm throttle force
takeoff 5
# Move to position
guided 10 0 -5
# Land
mode LAND
``` ```
Should show your GPU name, not "llvmpipe" or "software". ## Useful Commands
**Fix for WSL:**
- Install NVIDIA drivers for WSL: https://developer.nvidia.com/cuda/wsl
- Ensure WSLg is enabled (Windows 11)
### MAVProxy console not opening
**Cause:** Missing `--console` flag or display issues.
**Fix:**
```bash ```bash
# Run directly # List Gazebo topics
gz topic -l
# Check SITL status
sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --console --map sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --console --map
# If display errors: # View camera (if available)
export DISPLAY=:0 # Linux/WSL with X server gz topic -e -t /camera
```
## Advanced: Custom Takeoff
To customize the takeoff behavior, edit `scripts/run_ardupilot.py`:
```python
# Change altitude
TAKEOFF_ALTITUDE = 15 # meters
# Change timeout
CONNECTION_TIMEOUT = 60 # seconds
```
## Logs
ArduPilot logs are saved to:
```
~/ardupilot/logs/
~/ardupilot/build/sitl/logs/
```
View with:
```bash
mavlogdump.py --types GPS,ATT ~/ardupilot/logs/LASTLOG.BIN
``` ```

239
docs/drone_guide.md
View File

@@ -1,79 +1,176 @@
# Drone Controller Guide # Drone Controller Guide
## 3-Phase Mission ## Overview
``` The drone controller (`src/drone_controller.py`) provides a simple interface for flying the drone in simulation.
SEARCH ──► COMMAND ──► LAND ──► COMPLETE
```
| Phase | Action | ## Usage
|-------|--------|
| SEARCH | Find QR code on rover |
| COMMAND | Send commands to rover |
| LAND | Land on rover |
## Your Code ### Command Line
Edit `src/drone_controller.py`:
### Search Phase
```python
def calculate_search_maneuver(self, telemetry):
return (thrust, pitch, roll, yaw)
def detect_qr_code(self):
return {'data': 'qr_content', 'position': {...}} or None
```
### Command Phase
```python
def generate_rover_command(self, qr_data):
return {'type': 'move', 'x': 0, 'y': 0}
```
### Land Phase
```python
def calculate_landing_maneuver(self, telemetry, rover_telemetry):
return (thrust, pitch, roll, yaw)
```
## Telemetry
```python
telemetry = {
"altimeter": {"altitude": 5.0, "vertical_velocity": -0.1},
"velocity": {"x": 0, "y": 0, "z": 0},
"imu": {"orientation": {"roll": 0, "pitch": 0, "yaw": 0}},
"landing_pad": {"relative_x": 0.5, "relative_y": -0.2, "distance": 5.0}
}
```
## Control Output
| Value | Range | Effect |
|-------|-------|--------|
| thrust | ±1.0 | Vertical velocity |
| pitch | ±0.5 | Forward/back |
| roll | ±0.5 | Left/right |
| yaw | ±0.5 | Rotation |
## Configuration
Edit `config.py`:
```python
CONTROLLER = {
"Kp_z": 0.5,
"Kd_z": 0.3,
"Kp_xy": 0.3,
"Kd_xy": 0.2,
"rate": 50,
}
```
## Testing
```bash ```bash
./scripts/run_ardupilot_sim.sh runway source ~/venv-ardupilot/bin/activate
./scripts/run_ardupilot_controller.sh cd ~/RDC_Simulation
# Fly a square pattern
python scripts/run_ardupilot.py --pattern square
# Options
python scripts/run_ardupilot.py --help
```
### Options
| Option | Default | Description |
|--------|---------|-------------|
| `--pattern` | square | Flight pattern: square, circle, hover |
| `--altitude` | 5 | Flight altitude in meters |
| `--size` | 5 | Pattern size (side length or radius) |
| `--connection` | tcp:127.0.0.1:5760 | MAVLink connection string |
### Examples
```bash
# Fly a 10m square at 8m altitude
python scripts/run_ardupilot.py --pattern square --size 10 --altitude 8
# Fly a circle with 5m radius
python scripts/run_ardupilot.py --pattern circle --size 5
# Just hover (useful for testing)
python scripts/run_ardupilot.py --pattern hover
```
## Python API
### Basic Usage
```python
from src.drone_controller import SimpleDroneController
# Create and connect
drone = SimpleDroneController()
if not drone.connect():
print("Connection failed")
exit(1)
# Takeoff
drone.set_mode("GUIDED")
drone.arm()
drone.takeoff(5.0)
# Fly
drone.fly_square(size=5, altitude=5)
# Land
drone.land()
```
### Class: SimpleDroneController
#### Connection
```python
drone = SimpleDroneController(connection_string="tcp:127.0.0.1:5760")
drone.connect(timeout=30) # Returns True/False
```
#### State
```python
drone.armed # bool: Is the drone armed?
drone.mode # str: Current flight mode
drone.altitude # float: Current altitude (m)
drone.position # dict: {"x": float, "y": float, "z": float}
```
#### Commands
```python
drone.set_mode("GUIDED") # Set flight mode
drone.arm() # Arm motors (force arm)
drone.takeoff(5.0) # Takeoff to altitude
drone.goto(x, y, z) # Go to position (NED frame)
drone.fly_to_and_wait(x, y, alt) # Go and wait until reached
drone.land() # Land
```
#### Patterns
```python
drone.fly_square(size=5, altitude=5) # Square pattern
drone.fly_circle(radius=5, altitude=5) # Circle pattern
```
## Coordinate System
ArduPilot uses NED (North-East-Down):
- **X**: North (positive forward)
- **Y**: East (positive right)
- **Z**: Down (negative is up!)
```python
# Go 5m north, 3m east, at 10m altitude
drone.goto(5, 3, -10) # Z is negative for altitude
```
## Flight Modes
| Mode | Description |
|------|-------------|
| GUIDED | Accept external commands |
| LAND | Automatic landing |
| LOITER | Hold position |
| RTL | Return to launch |
| STABILIZE | Manual control |
## Sequence Diagram
```
┌─────────┐ ┌──────────┐ ┌─────────┐
│ Gazebo │ │ SITL │ │ Script │
└────┬────┘ └────┬─────┘ └────┬────┘
│ │ │
│ Physics data │ │
│ ◄──────────────│ │
│ │ │
│ Sensor JSON │ │
│ ───────────────► │
│ │ Connect │
│ │ ◄────────────────│
│ │ │
│ │ Set GUIDED │
│ │ ◄────────────────│
│ │ │
│ │ Arm │
│ │ ◄────────────────│
│ │ │
│ │ Takeoff │
│ │ ◄────────────────│
│ │ │
│ Motor commands │ │
│ ◄──────────────│ │
│ │ │
│ Drone moves │ │
│ │ │
```
## Troubleshooting
### Drone doesn't move
1. Check Gazebo is running
2. Check SITL shows telemetry (not "No JSON sensor message")
3. Check drone is armed
### Timeout waiting for altitude
- Increase takeoff timeout
- Check for pre-arm failures in SITL console
### Connection refused
```bash
# Check SITL is running
nc -z 127.0.0.1 5760 && echo "SITL is ready" || echo "SITL not running"
``` ```

262
docs/installation.md
View File

@@ -1,230 +1,174 @@
# Installation # Installation Guide
## Quick Install (Ubuntu/WSL) ## System Requirements
- **OS**: Ubuntu 22.04 or 24.04 (Windows users: use WSL2)
- **RAM**: 8GB minimum, 16GB recommended
- **Disk**: 10GB free space
- **GPU**: Any GPU with OpenGL 3.3+ support
## Quick Install (Ubuntu)
```bash ```bash
./setup/install_ubuntu.sh # Clone repository
./setup/install_ardupilot.sh git clone <repo-url> RDC_Simulation
cd RDC_Simulation
# Run installer (includes ArduPilot)
./setup/install_ubuntu.sh --with-ardupilot
# Reload shell
source ~/.bashrc source ~/.bashrc
``` ```
## What Gets Installed This installs:
- Python 3 + virtual environment
- Gazebo Harmonic
- ArduPilot SITL
- ArduPilot Gazebo plugin
- MAVProxy
| Component | Location | ## Windows (WSL2)
|-----------|----------|
| ArduPilot SITL | `~/ardupilot` |
| ArduPilot venv | `~/venv-ardupilot` |
| ardupilot_gazebo | `~/ardupilot_gazebo` |
| Gazebo Harmonic | System |
| ROS 2 | System |
| MAVProxy | `~/.local/bin` |
## Platform-Specific Installation ### Step 1: Install WSL2
### Ubuntu (Native)
```bash
./setup/install_ubuntu.sh --with-ardupilot
```
### Windows (via WSL2)
WSL2 (Windows Subsystem for Linux) is the recommended way to run this simulation on Windows.
#### Step 1: Install WSL2
Open PowerShell as Administrator: Open PowerShell as Administrator:
```powershell ```powershell
wsl --install -d Ubuntu-24.04 wsl --install -d Ubuntu-24.04
``` ```
Restart computer, then open Ubuntu from Start menu.
Restart your computer when prompted. ### Step 2: Install in WSL
#### Step 2: Configure WSL2 for GUI Apps
WSL2 on Windows 11 has built-in GUI support (WSLg). For Windows 10, you may need an X server:
```powershell
# Windows 11 - WSLg is automatic, no extra setup needed
# Windows 10 - Install VcXsrv or X410 from Microsoft Store
```
#### Step 3: Install in WSL
Open Ubuntu from Start menu:
```bash ```bash
# Clone the project (or copy from Windows) # Update system
git clone https://github.com/your-repo/RDC_Simulation.git sudo apt update && sudo apt upgrade -y
cd RDC_Simulation
# Run installation # Clone and install
./setup/install_ubuntu.sh git clone <repo-url> ~/RDC_Simulation
./setup/install_ardupilot.sh cd ~/RDC_Simulation
./setup/install_ubuntu.sh --with-ardupilot
# Reload
source ~/.bashrc source ~/.bashrc
``` ```
#### Step 4: GPU Acceleration in WSL ### WSL2 Tips
For best performance with Gazebo: - **GUI Apps**: Windows 11 has WSLg built-in. Gazebo should work automatically.
- **GPU**: Install NVIDIA WSL driver from [nvidia.com/wsl](https://developer.nvidia.com/cuda/wsl)
```bash - **Access Windows files**: `/mnt/c/Users/YourName/`
# Check GPU availability - **Performance**: Clone repo to Linux filesystem (not `/mnt/c/`)
glxinfo | grep "OpenGL renderer"
## macOS
# Should show your GPU, not "llvmpipe"
# If using NVIDIA, install NVIDIA GPU driver for WSL: macOS doesn't support Gazebo Harmonic natively. Options:
# https://developer.nvidia.com/cuda/wsl
``` 1. **Docker** (recommended): Use Linux container
2. **VM**: Use UTM or Parallels with Ubuntu
#### WSL Tips 3. **Standalone**: Run PyBullet-only simulation
- **Access Windows files:** `/mnt/c/Users/YourName/`
- **Open file explorer:** `explorer.exe .`
- **Copy to clipboard:** `cat file | clip.exe`
- **Memory limit:** Create `%UserProfile%\.wslconfig`:
```ini
[wsl2]
memory=8GB
processors=4
```
### macOS
Gazebo is not officially supported on macOS. Use standalone mode:
```bash ```bash
# Install basic dependencies
./setup/install_macos.sh ./setup/install_macos.sh
# Use standalone simulation
python standalone_simulation.py python standalone_simulation.py
``` ```
For full simulation, use a Linux VM (UTM for Apple Silicon) or Docker. ## Arch Linux
### Arch Linux
```bash ```bash
./setup/install_arch.sh ./setup/install_arch.sh --with-ardupilot
source ~/.bashrc
``` ```
ROS 2 and Gazebo are available via AUR - see script output for details.
## Dependencies
```bash
pip install -r requirements.txt
```
- pybullet
- numpy
- pillow
- opencv-python
- pymavlink
- pexpect
## Verify Installation ## Verify Installation
```bash ```bash
# Source the environment # Check ArduPilot
source activate.sh source ~/venv-ardupilot/bin/activate
python -c "import em; print('empy OK')"
# Check ArduPilot tools
sim_vehicle.py --help sim_vehicle.py --help
which mavproxy.py
# Check Gazebo # Check Gazebo
gz sim --help gz sim --version
ls ~/ardupilot_gazebo/build/libArduPilotPlugin.so
``` ```
## Environment Setup ## Post-Installation
After installation, always source the environment before running: ### Environment Setup
The installer creates `activate.sh` in the project root:
```bash ```bash
cd ~/RDC_Simulation
source activate.sh source activate.sh
# or
source ~/.bashrc # if ArduPilot was installed
``` ```
The activation script sets up: This sources:
- ROS 2 environment - ROS 2 (if installed)
- ArduPilot Python venv (with empy, pymavlink, etc.) - ArduPilot virtual environment
- Gazebo resource paths - Gazebo paths
- ArduPilot tools path
### First Run
```bash
# Terminal 1: Start Gazebo
./scripts/run_ardupilot_sim.sh runway
# Terminal 2: Start SITL (after Gazebo loads)
source ~/venv-ardupilot/bin/activate
sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --console
# Terminal 3: Run controller
source ~/venv-ardupilot/bin/activate
cd ~/RDC_Simulation
python scripts/run_ardupilot.py --pattern square
```
## Troubleshooting ## Troubleshooting
### "empy not found" or waf configure fails ### "empy not found"
The ArduPilot venv isn't activated. Run:
```bash ```bash
source ~/venv-ardupilot/bin/activate source ~/venv-ardupilot/bin/activate
pip install empy==3.3.4 pip install empy==3.3.4
``` ```
### sim_vehicle.py not found ### "No JSON sensor message"
SITL isn't receiving data from Gazebo.
- Start Gazebo FIRST
- Wait for it to fully load
- Then start SITL
### "sim_vehicle.py not found"
```bash ```bash
source ~/.ardupilot_env source ~/.ardupilot_env
# or # OR
export PATH=$PATH:~/ardupilot/Tools/autotest export PATH=$PATH:~/ardupilot/Tools/autotest
``` ```
### mavproxy.py not found ### Gazebo crashes / slow
```bash
export PATH=$PATH:~/.local/bin
```
### pexpect error
```bash
pip install pexpect
```
### Gazebo slow / Software rendering
```bash ```bash
# Check GPU
glxinfo | grep "OpenGL renderer" glxinfo | grep "OpenGL renderer"
```
Should show your GPU, not "llvmpipe". Install proper GPU drivers: # Try software rendering (slow but works)
export LIBGL_ALWAYS_SOFTWARE=1
- **NVIDIA:** `sudo apt install nvidia-driver-XXX` gz sim -v4 -r ~/ardupilot_gazebo/worlds/iris_runway.sdf
- **AMD:** `sudo apt install mesa-vulkan-drivers`
- **Intel:** Usually works out of box
- **WSL:** Install NVIDIA WSL driver from nvidia.com
### Wrong Python environment
If you see errors about missing packages that should be installed:
```bash
# Check which Python is being used
which python3
# Should be: ~/venv-ardupilot/bin/python3
# If not, activate the correct venv:
source ~/venv-ardupilot/bin/activate
``` ```
### WSL: Display not working ### WSL: Display not working
```bash ```bash
# Check DISPLAY variable # Check WSLg
echo $DISPLAY ls /mnt/wslg
# For WSLg (Windows 11), should be auto-set # If missing, update WSL
# For Windows 10 with X server: wsl --update
export DISPLAY=:0
``` ```
### WSL: Out of memory
Create `%UserProfile%\.wslconfig`:
```ini
[wsl2]
memory=8GB
swap=4GB
```
Then restart WSL: `wsl --shutdown`

344
scripts/run_ardupilot.py
View File

@@ -1,351 +1,23 @@
#!/usr/bin/env python3 #!/usr/bin/env python3
""" """
ArduPilot Drone Controller ArduPilot Flight Demo
========================== =====================
Connects your drone_controller.py logic to ArduPilot SITL via MAVLink. Simple script to make the drone takeoff, fly a pattern, and land.
Usage: Usage:
Terminal 1: ./scripts/run_ardupilot_sim.sh runway Terminal 1: ./scripts/run_ardupilot_sim.sh runway
Terminal 2: sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --console Terminal 2: sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --console
Terminal 3: python run_ardupilot.py Terminal 3: python scripts/run_ardupilot.py --pattern square
This script:
1. Connects to ArduPilot SITL via MAVLink
2. Gets telemetry (position, attitude, velocity)
3. Runs your drone_controller.calculate_landing_maneuver()
4. Sends velocity commands to ArduPilot
""" """
import sys import sys
import os import os
# Add project root to path
sys.path.insert(0, os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) sys.path.insert(0, os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
import time # Import the simplified drone controller
import math from src.drone_controller import main
import argparse
try:
from pymavlink import mavutil
except ImportError:
print("ERROR: pymavlink not installed")
print("Run: pip install pymavlink")
sys.exit(1)
from config import ARDUPILOT, CONTROLLER, MAVLINK
class ArduPilotInterface:
"""Interface to ArduPilot SITL via MAVLink."""
def __init__(self, connection_string=None):
self.connection_string = connection_string or MAVLINK["connection_string"]
self.mav = None
self.armed = False
self.mode = "UNKNOWN"
# Telemetry state
self.position = {"x": 0, "y": 0, "z": 0}
self.velocity = {"x": 0, "y": 0, "z": 0}
self.attitude = {"roll": 0, "pitch": 0, "yaw": 0}
self.altitude = 0
self.vertical_velocity = 0
def connect(self, timeout=30):
"""Connect to ArduPilot SITL."""
print(f"[INFO] Connecting to {self.connection_string}...")
try:
self.mav = mavutil.mavlink_connection(self.connection_string)
self.mav.wait_heartbeat(timeout=timeout)
print(f"[OK] Connected to system {self.mav.target_system}")
return True
except Exception as e:
print(f"[ERROR] Connection failed: {e}")
return False
def update_telemetry(self):
"""Read and update telemetry from ArduPilot."""
# Read all available messages
while True:
msg = self.mav.recv_match(blocking=False)
if msg is None:
break
msg_type = msg.get_type()
if msg_type == "HEARTBEAT":
self.armed = (msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED) != 0
# Get mode name using mavutil helper
try:
self.mode = mavutil.mode_string_v10(msg)
except Exception:
self.mode = str(msg.custom_mode)
elif msg_type == "LOCAL_POSITION_NED":
self.position = {"x": msg.x, "y": msg.y, "z": msg.z}
self.velocity = {"x": msg.vx, "y": msg.vy, "z": msg.vz}
self.altitude = -msg.z # NED: down is positive
self.vertical_velocity = -msg.vz
elif msg_type == "ATTITUDE":
self.attitude = {
"roll": msg.roll,
"pitch": msg.pitch,
"yaw": msg.yaw
}
def get_telemetry(self):
"""Get telemetry in the same format as standalone simulation."""
self.update_telemetry()
return {
"imu": {
"orientation": self.attitude,
"angular_velocity": {"x": 0, "y": 0, "z": 0} # Not available via MAVLink easily
},
"altimeter": {
"altitude": self.altitude,
"vertical_velocity": self.vertical_velocity
},
"velocity": self.velocity,
"landing_pad": None, # Not available in ArduPilot mode - use vision
"position": self.position, # Extra: actual position (GPS-like)
"armed": self.armed,
"mode": self.mode
}
def set_mode(self, mode_name):
"""Set flight mode."""
mode_map = self.mav.mode_mapping()
if mode_name.upper() not in mode_map:
print(f"[WARN] Unknown mode: {mode_name}")
return False
mode_id = mode_map[mode_name.upper()]
self.mav.set_mode(mode_id)
print(f"[OK] Mode set to {mode_name}")
return True
def arm(self, force=True):
"""Arm motors."""
print("[INFO] Arming...")
for attempt in range(3):
if force:
# Force arm (bypass checks)
self.mav.mav.command_long_send(
self.mav.target_system,
self.mav.target_component,
mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
0, 1, 21196, 0, 0, 0, 0, 0 # 21196 = force arm
)
else:
self.mav.arducopter_arm()
# Wait and check if armed
time.sleep(1)
self.update_telemetry()
if self.armed:
print("[OK] Armed")
return True
else:
print(f"[WARN] Arm attempt {attempt + 1}/3 failed, retrying...")
print("[ERROR] Failed to arm after 3 attempts")
return False
def disarm(self):
"""Disarm motors."""
self.mav.arducopter_disarm()
print("[OK] Disarmed")
def takeoff(self, altitude=5.0):
"""Takeoff to specified altitude."""
self.mav.mav.command_long_send(
self.mav.target_system,
self.mav.target_component,
mavutil.mavlink.MAV_CMD_NAV_TAKEOFF,
0, 0, 0, 0, 0, 0, 0, altitude
)
print(f"[OK] Takeoff to {altitude}m")
def send_velocity(self, vx, vy, vz, yaw_rate=0):
"""Send velocity command in body frame.
Args:
vx: Forward velocity (m/s)
vy: Right velocity (m/s)
vz: Down velocity (m/s, positive = descend)
yaw_rate: Yaw rate (rad/s)
"""
# Type mask: use velocity only
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
)
self.mav.mav.set_position_target_local_ned_send(
0, # time_boot_ms
self.mav.target_system,
self.mav.target_component,
mavutil.mavlink.MAV_FRAME_BODY_NED,
type_mask,
0, 0, 0, # position (ignored)
vx, vy, vz, # velocity
0, 0, 0, # acceleration (ignored)
0, yaw_rate # yaw, yaw_rate
)
class SimpleController:
"""Simple landing controller that doesn't require ROS 2."""
def __init__(self):
self.Kp_z = CONTROLLER.get("Kp_z", 0.5)
self.Kd_z = CONTROLLER.get("Kd_z", 0.3)
self.Kp_xy = CONTROLLER.get("Kp_xy", 0.3)
self.Kd_xy = CONTROLLER.get("Kd_xy", 0.2)
def calculate_landing_maneuver(self, telemetry, rover_telemetry=None):
"""Calculate control outputs for landing."""
altimeter = telemetry.get('altimeter', {})
altitude = altimeter.get('altitude', 5.0)
vertical_vel = altimeter.get('vertical_velocity', 0.0)
velocity = telemetry.get('velocity', {'x': 0, 'y': 0, 'z': 0})
vel_x = velocity.get('x', 0.0)
vel_y = velocity.get('y', 0.0)
landing_pad = telemetry.get('landing_pad', None)
# Descent rate control
if altitude > 1.0:
target_descent_rate = -0.5
else:
target_descent_rate = -0.2
descent_error = target_descent_rate - vertical_vel
thrust = self.Kp_z * descent_error
# Horizontal control
if landing_pad is not None:
target_x = landing_pad.get('relative_x', 0.0)
target_y = landing_pad.get('relative_y', 0.0)
pitch = self.Kp_xy * target_x - self.Kd_xy * vel_x
roll = self.Kp_xy * target_y - self.Kd_xy * vel_y
else:
# Just dampen velocity
pitch = -self.Kd_xy * vel_x
roll = -self.Kd_xy * vel_y
yaw = 0.0
return (thrust, pitch, roll, yaw)
def main():
parser = argparse.ArgumentParser(description="ArduPilot Drone Controller")
parser.add_argument("--connection", "-c", default=None,
help="MAVLink connection string (default: from config.py)")
parser.add_argument("--takeoff-alt", "-a", type=float, default=5.0,
help="Takeoff altitude (meters)")
parser.add_argument("--no-takeoff", action="store_true",
help="Don't auto takeoff, just control")
args = parser.parse_args()
# Create ArduPilot interface
ardupilot = ArduPilotInterface(args.connection)
# Connect
if not ardupilot.connect():
print("[ERROR] Could not connect to ArduPilot")
print("Make sure sim_vehicle.py is running")
sys.exit(1)
# Create simple controller (no ROS 2 required)
controller = SimpleController()
print("\n" + "=" * 50)
print(" ArduPilot GPS-Denied Controller")
print("=" * 50)
print(f"Mode: {ardupilot.mode}")
print(f"Armed: {ardupilot.armed}")
print("")
if not args.no_takeoff:
# Set GUIDED mode and arm
print("[INFO] Setting up for takeoff...")
ardupilot.set_mode("GUIDED")
time.sleep(2) # Give mode time to change
# Update telemetry to get current mode
ardupilot.update_telemetry()
print(f"[INFO] Current mode: {ardupilot.mode}")
if not ardupilot.arm(force=True):
print("[ERROR] Could not arm - continuing in manual mode")
print("[INFO] Use MAVProxy to arm: arm throttle force")
else:
ardupilot.takeoff(args.takeoff_alt)
print(f"[INFO] Taking off to {args.takeoff_alt}m...")
# Wait for takeoff (up to 15 seconds)
for i in range(150):
telemetry = ardupilot.get_telemetry()
alt = telemetry["altimeter"]["altitude"]
if alt > args.takeoff_alt * 0.9:
print(f"\n[OK] Reached target altitude ({alt:.1f}m)")
break
if i % 10 == 0:
print(f"\r[INFO] Climbing... {alt:.1f}m / {args.takeoff_alt}m", end="")
time.sleep(0.1)
print("")
print("\n[INFO] Starting controller loop...")
print("[INFO] Press Ctrl+C to stop\n")
rate = 20 # Hz
try:
while True:
# Get telemetry
telemetry = ardupilot.get_telemetry()
# Run your controller
thrust, pitch, roll, yaw = controller.calculate_landing_maneuver(
telemetry,
rover_telemetry=None # No rover in ArduPilot mode yet
)
# Convert control outputs to velocities
# thrust -> vertical velocity (negative = up)
# pitch -> forward velocity
# roll -> right velocity
vz = -thrust * 2.0 # Scale factor
vx = pitch * 2.0
vy = roll * 2.0
yaw_rate = yaw * 1.0
# Send velocity command
ardupilot.send_velocity(vx, vy, vz, yaw_rate)
# Print status
alt = telemetry["altimeter"]["altitude"]
mode = telemetry.get("mode", "?")
print(f"\rAlt: {alt:5.1f}m | Mode: {mode:10} | Cmd: vx={vx:+.2f} vy={vy:+.2f} vz={vz:+.2f}", end="")
time.sleep(1.0 / rate)
except KeyboardInterrupt:
print("\n\n[INFO] Stopping...")
ardupilot.set_mode("LAND")
print("[OK] Landing mode set")
if __name__ == "__main__": if __name__ == "__main__":
main() main()

68
scripts/run_ardupilot_controller.sh
View File

@@ -1,12 +1,13 @@
#!/bin/bash #!/bin/bash
# ============================================================================= # =============================================================================
# ArduPilot Controller Launcher # ArduPilot SITL + Controller Launcher
# ============================================================================= # =============================================================================
# Starts ArduPilot SITL and your drone controller together. # Starts SITL and the drone controller together.
# Run Gazebo FIRST in another terminal!
# #
# Usage: # Usage:
# Terminal 1: ./scripts/run_ardupilot_sim.sh runway # Terminal 1: ./scripts/run_ardupilot_sim.sh
# Terminal 2: ./scripts/run_ardupilot_controller.sh # Terminal 2: ./scripts/run_ardupilot_controller.sh --pattern square
# ============================================================================= # =============================================================================
set -e set -e
@@ -14,7 +15,7 @@ set -e
SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
PROJECT_DIR="$(dirname "$SCRIPT_DIR")" PROJECT_DIR="$(dirname "$SCRIPT_DIR")"
# Deactivate any existing virtual environment to avoid conflicts # Deactivate any existing venv
if [ -n "$VIRTUAL_ENV" ]; then if [ -n "$VIRTUAL_ENV" ]; then
deactivate 2>/dev/null || true deactivate 2>/dev/null || true
fi fi
@@ -24,83 +25,72 @@ if [ -f "$HOME/.ardupilot_env" ]; then
source "$HOME/.ardupilot_env" source "$HOME/.ardupilot_env"
fi fi
# Activate ArduPilot venv (has empy and other dependencies)
if [ -f "$HOME/venv-ardupilot/bin/activate" ]; then if [ -f "$HOME/venv-ardupilot/bin/activate" ]; then
source "$HOME/venv-ardupilot/bin/activate" source "$HOME/venv-ardupilot/bin/activate"
fi fi
echo "==============================================" echo "=============================================="
echo " ArduPilot Controller" echo " ArduPilot SITL + Controller"
echo "==============================================" echo "=============================================="
echo "" echo ""
# Check dependencies # Check sim_vehicle.py
if ! command -v sim_vehicle.py &> /dev/null; then if ! command -v sim_vehicle.py &> /dev/null; then
echo "[ERROR] sim_vehicle.py not found" echo "[ERROR] sim_vehicle.py not found"
echo "" echo ""
echo "Fix: Add ArduPilot tools to PATH:" echo "Fix: source ~/.ardupilot_env"
echo " export PATH=\$PATH:~/ardupilot/Tools/autotest" echo " Or: export PATH=\$PATH:~/ardupilot/Tools/autotest"
echo ""
echo "Or source the ArduPilot environment:"
echo " source ~/.ardupilot_env"
exit 1 exit 1
fi fi
# Verify empy is available # Check empy
if ! python3 -c "import em" 2>/dev/null; then if ! python3 -c "import em" 2>/dev/null; then
echo "[ERROR] empy not found" echo "[ERROR] empy not found"
echo "" echo ""
echo "Fix: Install empy in ArduPilot venv:" echo "Fix: pip install empy==3.3.4"
echo " source ~/venv-ardupilot/bin/activate"
echo " pip install empy==3.3.4"
exit 1 exit 1
fi fi
echo "[INFO] Environment OK" echo "[OK] Environment ready"
echo "[INFO] Python: $(which python3)"
echo "" echo ""
# Start SITL with console output # Start SITL with console
echo "[INFO] Starting ArduPilot SITL..." echo "[INFO] Starting SITL..."
echo "[INFO] This will build ArduCopter if needed (first run takes ~2 min)" echo "[INFO] Make sure Gazebo is running first!"
echo "" echo ""
# Run sim_vehicle in a way that shows output sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --console &
sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --console --map &
SITL_PID=$! SITL_PID=$!
# Wait for SITL to be ready (check for TCP port 5760) # Wait for SITL to be ready
echo "[INFO] Waiting for SITL to start (TCP 5760)..." echo "[INFO] Waiting for SITL (TCP 5760)..."
TRIES=0 TRIES=0
MAX_TRIES=60 # 60 seconds max
while ! nc -z 127.0.0.1 5760 2>/dev/null; do while ! nc -z 127.0.0.1 5760 2>/dev/null; do
sleep 1 sleep 1
TRIES=$((TRIES + 1)) TRIES=$((TRIES + 1))
if [ $TRIES -ge $MAX_TRIES ]; then if [ $TRIES -ge 60 ]; then
echo "[ERROR] Timeout waiting for SITL on port 5760" echo "[ERROR] SITL timeout"
kill $SITL_PID 2>/dev/null || true kill $SITL_PID 2>/dev/null || true
exit 1 exit 1
fi fi
# Check if SITL process is still running
if ! kill -0 $SITL_PID 2>/dev/null; then if ! kill -0 $SITL_PID 2>/dev/null; then
echo "[ERROR] SITL process died" echo "[ERROR] SITL died - is Gazebo running?"
exit 1 exit 1
fi fi
echo -n "." echo -n "."
done done
echo "" echo ""
echo "[OK] SITL is ready on TCP 5760" echo "[OK] SITL ready"
echo "" echo ""
# Give it a moment more to stabilize # Cleanup on exit
trap "echo ''; echo '[INFO] Stopping...'; kill $SITL_PID 2>/dev/null; exit 0" INT TERM
# Wait a moment for SITL to stabilize
sleep 2 sleep 2
# Trap to kill SITL on exit # Run the controller
trap "echo ''; echo '[INFO] Stopping SITL...'; kill $SITL_PID 2>/dev/null; exit 0" INT TERM echo "[INFO] Starting flight controller..."
# Run controller
echo "[INFO] Starting drone controller..."
echo "[INFO] Press Ctrl+C to stop"
echo "" echo ""
cd "$PROJECT_DIR" cd "$PROJECT_DIR"
python scripts/run_ardupilot.py "$@" python scripts/run_ardupilot.py "$@"

135
scripts/run_ardupilot_sim.sh
View File

@@ -1,96 +1,52 @@
#!/bin/bash #!/bin/bash
# ============================================================================= # =============================================================================
# ArduPilot GPS-Denied Simulation Launcher # ArduPilot Gazebo Simulation Launcher
# ============================================================================= # =============================================================================
# Launches Gazebo with GPU acceleration and camera support # Launches Gazebo with the ArduPilot iris drone model.
# Automatically detects GPU: NVIDIA > Intel/AMD integrated > Software # Start this FIRST, then run SITL in another terminal.
# #
# Usage: # Usage:
# ./scripts/run_ardupilot_sim.sh # Default iris_runway # ./scripts/run_ardupilot_sim.sh # Default runway
# ./scripts/run_ardupilot_sim.sh camera # With camera world # ./scripts/run_ardupilot_sim.sh warehouse # Indoor warehouse
# ============================================================================= # =============================================================================
set -e set -e
echo "==============================================" echo "=============================================="
echo " ArduPilot GPS-Denied Simulation" echo " Gazebo + ArduPilot Simulation"
echo "==============================================" echo "=============================================="
echo ""
# Get script directory
SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
PROJECT_DIR="$(dirname "$SCRIPT_DIR")"
# ============================================================================= # =============================================================================
# GPU DETECTION & CONFIGURATION # GPU DETECTION
# ============================================================================= # =============================================================================
echo ""
echo "[INFO] Detecting GPU..." echo "[INFO] Detecting GPU..."
# Disable software rendering by default
export LIBGL_ALWAYS_SOFTWARE=0 export LIBGL_ALWAYS_SOFTWARE=0
# Check for NVIDIA dedicated GPU if command -v nvidia-smi &> /dev/null && nvidia-smi &> /dev/null 2>&1; then
if command -v nvidia-smi &> /dev/null && nvidia-smi &> /dev/null; then
GPU_NAME=$(nvidia-smi --query-gpu=name --format=csv,noheader 2>/dev/null | head -1) GPU_NAME=$(nvidia-smi --query-gpu=name --format=csv,noheader 2>/dev/null | head -1)
echo "[GPU] NVIDIA (dedicated): $GPU_NAME"
export __GLX_VENDOR_LIBRARY_NAME=nvidia
export __NV_PRIME_RENDER_OFFLOAD=1
export __VK_LAYER_NV_optimus=NVIDIA_only
GPU_TYPE="nvidia"
# Check for Intel integrated GPU
elif [ -d "/sys/class/drm/card0" ] && grep -qi "intel" /sys/class/drm/card0/device/vendor 2>/dev/null; then
echo "[GPU] Intel (integrated)"
export MESA_GL_VERSION_OVERRIDE=3.3
export LIBVA_DRIVER_NAME=iHD # Intel media driver
GPU_TYPE="intel"
# Check via glxinfo
elif command -v glxinfo &> /dev/null; then
GPU_NAME=$(glxinfo 2>/dev/null | grep "OpenGL renderer" | cut -d: -f2 | xargs)
if [[ "$GPU_NAME" == *"NVIDIA"* ]]; then
echo "[GPU] NVIDIA: $GPU_NAME" echo "[GPU] NVIDIA: $GPU_NAME"
export __GLX_VENDOR_LIBRARY_NAME=nvidia export __GLX_VENDOR_LIBRARY_NAME=nvidia
GPU_TYPE="nvidia" export __NV_PRIME_RENDER_OFFLOAD=1
elif [[ "$GPU_NAME" == *"Intel"* ]]; then elif command -v glxinfo &> /dev/null; then
echo "[GPU] Intel: $GPU_NAME" GPU_NAME=$(glxinfo 2>/dev/null | grep "OpenGL renderer" | cut -d: -f2 | xargs)
export MESA_GL_VERSION_OVERRIDE=3.3
GPU_TYPE="intel"
elif [[ "$GPU_NAME" == *"AMD"* ]] || [[ "$GPU_NAME" == *"Radeon"* ]]; then
echo "[GPU] AMD: $GPU_NAME"
export MESA_GL_VERSION_OVERRIDE=3.3
GPU_TYPE="amd"
elif [[ "$GPU_NAME" == *"llvmpipe"* ]] || [[ "$GPU_NAME" == *"software"* ]]; then
echo "[GPU] Software rendering (llvmpipe) - SLOW!"
echo "[WARN] Install GPU drivers for better performance"
GPU_TYPE="software"
else
echo "[GPU] $GPU_NAME" echo "[GPU] $GPU_NAME"
export MESA_GL_VERSION_OVERRIDE=3.3 export MESA_GL_VERSION_OVERRIDE=3.3
GPU_TYPE="other"
fi
# Fallback to Mesa defaults
else else
echo "[GPU] Unknown - using Mesa defaults" echo "[GPU] Unknown - using Mesa defaults"
export MESA_GL_VERSION_OVERRIDE=3.3 export MESA_GL_VERSION_OVERRIDE=3.3
GPU_TYPE="unknown"
fi fi
# For integrated graphics, ensure Mesa works well
if [[ "$GPU_TYPE" == "intel" ]] || [[ "$GPU_TYPE" == "amd" ]] || [[ "$GPU_TYPE" == "other" ]]; then
export MESA_LOADER_DRIVER_OVERRIDE=""
export DRI_PRIME=0 # Use primary GPU (integrated)
fi
echo "[INFO] GPU type: $GPU_TYPE"
# ============================================================================= # =============================================================================
# GAZEBO PATHS # GAZEBO PATHS
# ============================================================================= # =============================================================================
export GZ_SIM_SYSTEM_PLUGIN_PATH="${HOME}/ardupilot_gazebo/build:${GZ_SIM_SYSTEM_PLUGIN_PATH}" 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}" export GZ_SIM_RESOURCE_PATH="${HOME}/ardupilot_gazebo/models:${HOME}/ardupilot_gazebo/worlds:${GZ_SIM_RESOURCE_PATH}"
# Add local models
SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
PROJECT_DIR="$(dirname "$SCRIPT_DIR")"
export GZ_SIM_RESOURCE_PATH="${PROJECT_DIR}/gazebo/models:${GZ_SIM_RESOURCE_PATH}" export GZ_SIM_RESOURCE_PATH="${PROJECT_DIR}/gazebo/models:${GZ_SIM_RESOURCE_PATH}"
# ============================================================================= # =============================================================================
@@ -114,77 +70,48 @@ fi
WORLD_ARG="${1:-runway}" WORLD_ARG="${1:-runway}"
case "$WORLD_ARG" in case "$WORLD_ARG" in
runway|iris|default) runway|default)
WORLD="${HOME}/ardupilot_gazebo/worlds/iris_runway.sdf" WORLD="${HOME}/ardupilot_gazebo/worlds/iris_runway.sdf"
;; ;;
warehouse) warehouse)
WORLD="${HOME}/ardupilot_gazebo/worlds/iris_warehouse.sdf" WORLD="${HOME}/ardupilot_gazebo/worlds/iris_warehouse.sdf"
;; ;;
gimbal)
WORLD="${HOME}/ardupilot_gazebo/worlds/gimbal.sdf"
;;
zephyr|plane)
WORLD="${HOME}/ardupilot_gazebo/worlds/zephyr_runway.sdf"
;;
parachute)
WORLD="${HOME}/ardupilot_gazebo/worlds/zephyr_parachute.sdf"
;;
custom)
WORLD="${PROJECT_DIR}/gazebo/worlds/custom_landing.sdf"
;;
*) *)
if [ -f "$WORLD_ARG" ]; then if [ -f "$WORLD_ARG" ]; then
WORLD="$WORLD_ARG" WORLD="$WORLD_ARG"
elif [ -f "${PROJECT_DIR}/gazebo/worlds/${WORLD_ARG}" ]; then
WORLD="${PROJECT_DIR}/gazebo/worlds/${WORLD_ARG}"
elif [ -f "${PROJECT_DIR}/gazebo/worlds/${WORLD_ARG}.sdf" ]; then
WORLD="${PROJECT_DIR}/gazebo/worlds/${WORLD_ARG}.sdf"
elif [ -f "${HOME}/ardupilot_gazebo/worlds/${WORLD_ARG}" ]; then
WORLD="${HOME}/ardupilot_gazebo/worlds/${WORLD_ARG}"
elif [ -f "${HOME}/ardupilot_gazebo/worlds/${WORLD_ARG}.sdf" ]; then elif [ -f "${HOME}/ardupilot_gazebo/worlds/${WORLD_ARG}.sdf" ]; then
WORLD="${HOME}/ardupilot_gazebo/worlds/${WORLD_ARG}.sdf" WORLD="${HOME}/ardupilot_gazebo/worlds/${WORLD_ARG}.sdf"
else else
WORLD="" echo "[ERROR] World not found: $WORLD_ARG"
echo ""
echo "Available worlds:"
echo " runway - Outdoor runway (default)"
echo " warehouse - Indoor warehouse"
exit 1
fi fi
;; ;;
esac esac
if [ -z "$WORLD" ] || [ ! -f "$WORLD" ]; then
echo "[ERROR] World not found: $WORLD_ARG"
echo ""
echo "Built-in worlds:"
echo " runway - Iris on runway (default)"
echo " warehouse - Iris in warehouse"
echo " custom - Custom landing pad"
echo ""
echo "Local worlds in gazebo/worlds/:"
ls -1 "${PROJECT_DIR}/gazebo/worlds/"*.sdf 2>/dev/null | xargs -n1 basename || echo " (none)"
echo ""
echo "Or specify full path to .sdf file"
exit 1
fi
echo "[INFO] World: $(basename "$WORLD")" echo "[INFO] World: $(basename "$WORLD")"
echo ""
# ============================================================================= # =============================================================================
# INSTRUCTIONS # INSTRUCTIONS
# ============================================================================= # =============================================================================
echo ""
echo "==============================================" echo "=============================================="
echo " Starting Gazebo" echo " STEP 1: Gazebo Starting..."
echo "==============================================" echo "=============================================="
echo "" echo ""
echo "World: $WORLD" echo "After Gazebo loads, run SITL in another terminal:"
echo ""
echo "After Gazebo starts, in another terminal run:"
echo "" echo ""
echo " source ~/venv-ardupilot/bin/activate"
echo " sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --console" echo " sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --console"
echo "" echo ""
echo "For GPS-DENIED mode, in MAVProxy console:" echo "Then run the flight controller:"
echo "" echo ""
echo " param set ARMING_CHECK 0" echo " source ~/venv-ardupilot/bin/activate"
echo " mode stabilize" echo " cd ~/RDC_Simulation"
echo " arm throttle force" echo " python scripts/run_ardupilot.py --pattern square"
echo "" echo ""
echo "==============================================" echo "=============================================="
echo "" echo ""

114
scripts/run_flight_demo.sh Executable file
View File

@@ -0,0 +1,114 @@
#!/bin/bash
# =============================================================================
# ArduPilot Flight Demo with Camera Viewer
# =============================================================================
# Runs the drone controller and camera viewer together.
#
# Usage: ./scripts/run_flight_demo.sh [options]
#
# Options:
# --pattern square|circle|hover Flight pattern (default: square)
# --altitude HEIGHT Flight altitude in meters (default: 5)
# --size SIZE Pattern size in meters (default: 5)
# --no-camera Skip camera viewer
# =============================================================================
set -e
SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
PROJECT_DIR="$(dirname "$SCRIPT_DIR")"
# Parse arguments
PATTERN="square"
ALTITUDE="5"
SIZE="5"
SHOW_CAMERA=true
while [[ $# -gt 0 ]]; do
case $1 in
--pattern|-p)
PATTERN="$2"
shift 2
;;
--altitude|-a)
ALTITUDE="$2"
shift 2
;;
--size|-s)
SIZE="$2"
shift 2
;;
--no-camera)
SHOW_CAMERA=false
shift
;;
*)
shift
;;
esac
done
# Deactivate any existing venv
if [ -n "$VIRTUAL_ENV" ]; then
deactivate 2>/dev/null || true
fi
# Source ArduPilot environment
if [ -f "$HOME/.ardupilot_env" ]; then
source "$HOME/.ardupilot_env"
fi
if [ -f "$HOME/venv-ardupilot/bin/activate" ]; then
source "$HOME/venv-ardupilot/bin/activate"
fi
echo "=============================================="
echo " ArduPilot Flight Demo"
echo "=============================================="
echo ""
echo " Pattern: $PATTERN"
echo " Altitude: ${ALTITUDE}m"
echo " Size: ${SIZE}m"
echo " Camera: $SHOW_CAMERA"
echo ""
echo "=============================================="
echo ""
# Check if ROS 2 is available for camera viewer
ROS2_AVAILABLE=false
if command -v ros2 &> /dev/null; then
ROS2_AVAILABLE=true
fi
# Start camera viewer in background if requested and ROS 2 is available
CAMERA_PID=""
if [ "$SHOW_CAMERA" = true ]; then
if [ "$ROS2_AVAILABLE" = true ]; then
echo "[INFO] Starting camera viewer..."
source /opt/ros/humble/setup.bash 2>/dev/null || source /opt/ros/jazzy/setup.bash 2>/dev/null || true
python "$PROJECT_DIR/src/camera_viewer.py" --topic /camera/image_raw &
CAMERA_PID=$!
sleep 2
else
echo "[WARN] ROS 2 not available - camera viewer disabled"
echo "[INFO] To view camera, run Gazebo with camera in the world"
fi
fi
# Cleanup function
cleanup() {
echo ""
echo "[INFO] Cleaning up..."
if [ -n "$CAMERA_PID" ]; then
kill $CAMERA_PID 2>/dev/null || true
fi
}
trap cleanup EXIT INT TERM
# Run the flight controller
echo "[INFO] Starting flight controller..."
cd "$PROJECT_DIR"
python scripts/run_ardupilot.py --pattern "$PATTERN" --altitude "$ALTITUDE" --size "$SIZE"
echo ""
echo "[OK] Flight demo complete!"

760
src/drone_controller.py
View File

@@ -1,500 +1,286 @@
#!/usr/bin/env python3 #!/usr/bin/env python3
""" """
DroneController - GPS-denied landing with 3-phase state machine. Simple Drone Controller - Takeoff, Fly Pattern, Land
=====================================================
A minimal controller that demonstrates drone movement in simulation.
Phases: Usage:
1. SEARCH - Find QR code on rover using camera Terminal 1: ./scripts/run_ardupilot_sim.sh runway
2. COMMAND - Send commands to rover Terminal 2: sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --console
3. LAND - Land on the rover Terminal 3: python scripts/run_ardupilot.py
""" """
import json
import math
import base64
from enum import Enum, auto
from typing import Dict, Any, Optional
import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
from geometry_msgs.msg import Twist, PoseStamped, TwistStamped
from sensor_msgs.msg import Imu, BatteryState, NavSatFix, Image
from std_msgs.msg import String
try:
from config import CONTROLLER, DRONE, LANDING
CONFIG_LOADED = True
except ImportError:
CONFIG_LOADED = False
CONTROLLER = {"Kp_z": 0.5, "Kd_z": 0.3, "Kp_xy": 0.3, "Kd_xy": 0.2, "rate": 50}
DRONE = {"max_thrust": 1.0, "max_pitch": 0.5, "max_roll": 0.5}
LANDING = {"height_threshold": 0.1, "success_velocity": 0.1}
try:
import cv2
import numpy as np
CV2_AVAILABLE = True
except ImportError:
CV2_AVAILABLE = False
class Phase(Enum):
SEARCH = auto()
COMMAND = auto()
LAND = auto()
COMPLETE = auto()
def quaternion_to_euler(x: float, y: float, z: float, w: float) -> tuple:
sinr_cosp = 2.0 * (w * x + y * z)
cosr_cosp = 1.0 - 2.0 * (x * x + y * y)
roll = math.atan2(sinr_cosp, cosr_cosp)
sinp = 2.0 * (w * y - z * x)
if abs(sinp) >= 1:
pitch = math.copysign(math.pi / 2, sinp)
else:
pitch = math.asin(sinp)
siny_cosp = 2.0 * (w * z + x * y)
cosy_cosp = 1.0 - 2.0 * (y * y + z * z)
yaw = math.atan2(siny_cosp, cosy_cosp)
return roll, pitch, yaw
class DroneController(Node):
def __init__(self, use_ardupilot_topics: bool = True):
super().__init__('drone_controller')
self._control_rate = CONTROLLER.get("rate", 50.0)
self._Kp_z = CONTROLLER.get("Kp_z", 0.5)
self._Kd_z = CONTROLLER.get("Kd_z", 0.3)
self._Kp_xy = CONTROLLER.get("Kp_xy", 0.3)
self._Kd_xy = CONTROLLER.get("Kd_xy", 0.2)
self._max_thrust = DRONE.get("max_thrust", 1.0)
self._max_pitch = DRONE.get("max_pitch", 0.5)
self._max_roll = DRONE.get("max_roll", 0.5)
self._landing_height = LANDING.get("height_threshold", 0.1)
self._landing_velocity = LANDING.get("success_velocity", 0.1)
self._use_ardupilot = use_ardupilot_topics
self._phase = Phase.SEARCH
self._phase_start_time = self.get_clock().now()
self._qr_detected = False
self._qr_data: Optional[str] = None
self._qr_position: Optional[Dict[str, float]] = None
self._search_pattern_angle = 0.0
self._commands_sent = False
self._command_acknowledged = False
self._landing_complete = False
self._latest_telemetry: Optional[Dict[str, Any]] = None
self._rover_telemetry: Optional[Dict[str, Any]] = None
self._latest_camera_image: Optional[bytes] = None
self._telemetry_received = False
self._ap_pose: Optional[PoseStamped] = None
self._ap_twist: Optional[TwistStamped] = None
self._ap_imu: Optional[Imu] = None
self._ap_battery: Optional[BatteryState] = None
self.get_logger().info('=' * 50)
self.get_logger().info('Drone Controller - 3 Phase GPS-Denied Landing')
self.get_logger().info(f' Phase 1: SEARCH | Phase 2: COMMAND | Phase 3: LAND')
self.get_logger().info(f' Mode: {"ArduPilot DDS" if use_ardupilot_topics else "Legacy JSON"}')
self.get_logger().info('=' * 50)
sensor_qos = QoSProfile(
reliability=ReliabilityPolicy.BEST_EFFORT,
history=HistoryPolicy.KEEP_LAST,
depth=1
)
if use_ardupilot_topics:
self._setup_ardupilot_subscriptions(sensor_qos)
else:
self._setup_legacy_subscriptions()
self._rover_telemetry_sub = self.create_subscription(
String, '/rover/telemetry', self._rover_telemetry_callback, 10)
self._camera_sub = self.create_subscription(
Image, '/drone/camera', self._camera_callback, sensor_qos)
self._cmd_vel_pub = self.create_publisher(Twist, '/cmd_vel', 10)
self._rover_cmd_pub = self.create_publisher(String, '/rover/command', 10)
self._control_timer = self.create_timer(1.0 / self._control_rate, self._control_loop)
self.get_logger().info(f'Current Phase: {self._phase.name}')
def _setup_ardupilot_subscriptions(self, qos: QoSProfile):
self._ap_pose_sub = self.create_subscription(
PoseStamped, '/ap/pose/filtered', self._ap_pose_callback, qos)
self._ap_twist_sub = self.create_subscription(
TwistStamped, '/ap/twist/filtered', self._ap_twist_callback, qos)
self._ap_imu_sub = self.create_subscription(
Imu, '/ap/imu/filtered', self._ap_imu_callback, qos)
self._ap_battery_sub = self.create_subscription(
BatteryState, '/ap/battery', self._ap_battery_callback, qos)
def _setup_legacy_subscriptions(self):
self._telemetry_sub = self.create_subscription(
String, '/drone/telemetry', self._telemetry_callback, 10)
def _ap_pose_callback(self, msg: PoseStamped):
self._ap_pose = msg
if not self._telemetry_received:
self._telemetry_received = True
self._warmup_count = 0
self.get_logger().info('First ArduPilot pose received!')
self._build_telemetry_from_ardupilot()
def _ap_twist_callback(self, msg: TwistStamped):
self._ap_twist = msg
self._build_telemetry_from_ardupilot()
def _ap_imu_callback(self, msg: Imu):
self._ap_imu = msg
self._build_telemetry_from_ardupilot()
def _ap_battery_callback(self, msg: BatteryState):
self._ap_battery = msg
self._build_telemetry_from_ardupilot()
def _telemetry_callback(self, msg: String) -> None:
try:
self._latest_telemetry = json.loads(msg.data)
if not self._telemetry_received:
self._telemetry_received = True
self._warmup_count = 0
self.get_logger().info('First telemetry received!')
except json.JSONDecodeError as e:
self.get_logger().warning(f'Failed to parse telemetry: {e}')
def _rover_telemetry_callback(self, msg: String) -> None:
try:
self._rover_telemetry = json.loads(msg.data)
except json.JSONDecodeError:
pass
def _camera_callback(self, msg: Image) -> None:
try:
self._latest_camera_image = bytes(msg.data)
self._camera_width = msg.width
self._camera_height = msg.height
self._camera_encoding = msg.encoding
except Exception as e:
self.get_logger().warning(f'Camera callback error: {e}')
def _build_telemetry_from_ardupilot(self):
telemetry = {}
if self._ap_pose:
pos = self._ap_pose.pose.position
ori = self._ap_pose.pose.orientation
roll, pitch, yaw = quaternion_to_euler(ori.x, ori.y, ori.z, ori.w)
telemetry['position'] = {'x': pos.x, 'y': pos.y, 'z': pos.z}
telemetry['altimeter'] = {
'altitude': pos.z,
'vertical_velocity': self._ap_twist.twist.linear.z if self._ap_twist else 0.0
}
telemetry['imu'] = {
'orientation': {'roll': roll, 'pitch': pitch, 'yaw': yaw},
'angular_velocity': {'x': 0, 'y': 0, 'z': 0}
}
if self._ap_twist:
twist = self._ap_twist.twist
telemetry['velocity'] = {
'x': twist.linear.x, 'y': twist.linear.y, 'z': twist.linear.z
}
if 'altimeter' in telemetry:
telemetry['altimeter']['vertical_velocity'] = twist.linear.z
if self._ap_imu:
if 'imu' not in telemetry:
telemetry['imu'] = {}
telemetry['imu']['angular_velocity'] = {
'x': self._ap_imu.angular_velocity.x,
'y': self._ap_imu.angular_velocity.y,
'z': self._ap_imu.angular_velocity.z
}
if self._ap_battery:
telemetry['battery'] = {
'voltage': self._ap_battery.voltage,
'remaining': self._ap_battery.percentage * 100
}
if self._qr_position:
telemetry['landing_pad'] = self._qr_position
elif self._rover_telemetry and self._ap_pose:
rover_pos = self._rover_telemetry.get('position', {})
rx, ry = rover_pos.get('x', 0), rover_pos.get('y', 0)
dx = self._ap_pose.pose.position.x
dy = self._ap_pose.pose.position.y
dz = self._ap_pose.pose.position.z
rel_x, rel_y = rx - dx, ry - dy
distance = math.sqrt(rel_x**2 + rel_y**2 + dz**2)
telemetry['landing_pad'] = {
'relative_x': rel_x, 'relative_y': rel_y,
'distance': distance, 'confidence': 1.0 if distance < 10.0 else 0.0
}
self._latest_telemetry = telemetry
def _control_loop(self) -> None:
if self._latest_telemetry is None:
return
if self._phase == Phase.COMPLETE:
return
if not hasattr(self, '_warmup_count'):
self._warmup_count = 0
self._warmup_count += 1
if self._warmup_count < 50:
return
if self._phase == Phase.SEARCH:
thrust, pitch, roll, yaw = self._execute_search_phase()
elif self._phase == Phase.COMMAND:
thrust, pitch, roll, yaw = self._execute_command_phase()
elif self._phase == Phase.LAND:
thrust, pitch, roll, yaw = self._execute_land_phase()
else:
thrust, pitch, roll, yaw = 0.0, 0.0, 0.0, 0.0
thrust = max(min(thrust, self._max_thrust), -self._max_thrust)
pitch = max(min(pitch, self._max_pitch), -self._max_pitch)
roll = max(min(roll, self._max_roll), -self._max_roll)
yaw = max(min(yaw, 0.5), -0.5)
self._publish_command(thrust, pitch, roll, yaw)
def _execute_search_phase(self) -> tuple:
qr_result = self.detect_qr_code()
if qr_result is not None:
self._qr_detected = True
self._qr_data = qr_result.get('data')
self._qr_position = qr_result.get('position')
self.get_logger().info(f'QR CODE DETECTED: {self._qr_data}')
self._transition_to_phase(Phase.COMMAND)
return (0.0, 0.0, 0.0, 0.0)
return self.calculate_search_maneuver(self._latest_telemetry)
def detect_qr_code(self) -> Optional[Dict[str, Any]]:
if not CV2_AVAILABLE or self._latest_camera_image is None:
return None
try:
if self._camera_encoding == 'rgb8':
img = np.frombuffer(self._latest_camera_image, dtype=np.uint8)
img = img.reshape((self._camera_height, self._camera_width, 3))
img = cv2.cvtColor(img, cv2.COLOR_RGB2BGR)
elif self._camera_encoding == 'bgr8':
img = np.frombuffer(self._latest_camera_image, dtype=np.uint8)
img = img.reshape((self._camera_height, self._camera_width, 3))
else:
return None
detector = cv2.QRCodeDetector()
data, points, _ = detector.detectAndDecode(img)
if data and points is not None:
cx = np.mean(points[0][:, 0])
cy = np.mean(points[0][:, 1])
rel_x = (cx - self._camera_width / 2) / (self._camera_width / 2)
rel_y = (cy - self._camera_height / 2) / (self._camera_height / 2)
qr_size = np.linalg.norm(points[0][0] - points[0][2])
altitude = self._latest_telemetry.get('altimeter', {}).get('altitude', 5.0)
return {
'data': data,
'position': {
'relative_x': rel_x * altitude * 0.5,
'relative_y': rel_y * altitude * 0.5,
'distance': altitude,
'confidence': min(qr_size / 100, 1.0)
}
}
except Exception as e:
self.get_logger().debug(f'QR detection error: {e}')
return None
def calculate_search_maneuver(self, telemetry: Dict[str, Any]) -> tuple:
altimeter = telemetry.get('altimeter', {})
altitude = altimeter.get('altitude', 5.0)
vertical_vel = altimeter.get('vertical_velocity', 0.0)
target_altitude = 5.0
altitude_error = target_altitude - altitude
thrust = self._Kp_z * altitude_error - self._Kd_z * vertical_vel
self._search_pattern_angle += 0.01
yaw = 0.1
velocity = telemetry.get('velocity', {})
pitch = -self._Kd_xy * velocity.get('x', 0)
roll = -self._Kd_xy * velocity.get('y', 0)
return (thrust, pitch, roll, yaw)
def _execute_command_phase(self) -> tuple:
if not self._commands_sent:
command = self.generate_rover_command(self._qr_data)
self._send_rover_command(command)
self._commands_sent = True
self._command_time = self.get_clock().now()
elapsed = (self.get_clock().now() - self._command_time).nanoseconds / 1e9
if self._command_acknowledged or elapsed > 5.0:
self.get_logger().info('Command phase complete, transitioning to LAND')
self._transition_to_phase(Phase.LAND)
return self.calculate_hover_maneuver(self._latest_telemetry)
def generate_rover_command(self, qr_data: Optional[str]) -> Dict[str, Any]:
return {
'type': 'prepare_landing',
'qr_data': qr_data,
'drone_position': self._latest_telemetry.get('position', {})
}
def _send_rover_command(self, command: Dict[str, Any]) -> None:
msg = String()
msg.data = json.dumps(command)
self._rover_cmd_pub.publish(msg)
self.get_logger().info(f'Sent rover command: {command.get("type")}')
def calculate_hover_maneuver(self, telemetry: Dict[str, Any]) -> tuple:
altimeter = telemetry.get('altimeter', {})
altitude = altimeter.get('altitude', 5.0)
vertical_vel = altimeter.get('vertical_velocity', 0.0)
altitude_error = 0
thrust = self._Kp_z * altitude_error - self._Kd_z * vertical_vel
velocity = telemetry.get('velocity', {})
pitch = -self._Kd_xy * velocity.get('x', 0)
roll = -self._Kd_xy * velocity.get('y', 0)
return (thrust, pitch, roll, 0.0)
def _execute_land_phase(self) -> tuple:
if self._check_landing_complete():
self._landing_complete = True
self.get_logger().info('LANDING COMPLETE!')
self._transition_to_phase(Phase.COMPLETE)
return (0.0, 0.0, 0.0, 0.0)
return self.calculate_landing_maneuver(
self._latest_telemetry,
self._rover_telemetry
)
def calculate_landing_maneuver(
self,
telemetry: Dict[str, Any],
rover_telemetry: Optional[Dict[str, Any]] = None
) -> tuple:
altimeter = telemetry.get('altimeter', {})
altitude = altimeter.get('altitude', 5.0)
vertical_vel = altimeter.get('vertical_velocity', 0.0)
velocity = telemetry.get('velocity', {'x': 0, 'y': 0, 'z': 0})
vel_x = velocity.get('x', 0.0)
vel_y = velocity.get('y', 0.0)
landing_pad = telemetry.get('landing_pad', None)
if altitude > 1.0:
target_descent_rate = -0.5
else:
target_descent_rate = -0.2
descent_error = target_descent_rate - vertical_vel
thrust = self._Kp_z * descent_error
if landing_pad is not None:
target_x = landing_pad.get('relative_x', 0.0)
target_y = landing_pad.get('relative_y', 0.0)
pitch = self._Kp_xy * target_x - self._Kd_xy * vel_x
roll = self._Kp_xy * target_y - self._Kd_xy * vel_y
else:
pitch = -self._Kd_xy * vel_x
roll = -self._Kd_xy * vel_y
yaw = 0.0
return (thrust, pitch, roll, yaw)
def _transition_to_phase(self, new_phase: Phase) -> None:
old_phase = self._phase
self._phase = new_phase
self._phase_start_time = self.get_clock().now()
self.get_logger().info(f'Phase: {old_phase.name} -> {new_phase.name}')
def _check_landing_complete(self) -> bool:
if self._latest_telemetry is None:
return False
try:
altimeter = self._latest_telemetry.get('altimeter', {})
altitude = altimeter.get('altitude', float('inf'))
vertical_velocity = abs(altimeter.get('vertical_velocity', float('inf')))
return altitude < self._landing_height and vertical_velocity < self._landing_velocity
except (KeyError, TypeError):
return False
def _publish_command(self, thrust: float, pitch: float, roll: float, yaw: float) -> None:
msg = Twist()
msg.linear.z = thrust
msg.linear.x = pitch
msg.linear.y = roll
msg.angular.z = yaw
self._cmd_vel_pub.publish(msg)
def get_current_phase(self) -> Phase:
return self._phase
def main(args=None):
import sys import sys
import os
import time
import math
import argparse
use_ardupilot = '--ardupilot' in sys.argv or '-a' in sys.argv sys.path.insert(0, os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
filtered_args = [a for a in (args or sys.argv) if a not in ['--ardupilot', '-a']]
rclpy.init(args=filtered_args)
controller = None
try: try:
controller = DroneController(use_ardupilot_topics=use_ardupilot) from pymavlink import mavutil
rclpy.spin(controller) except ImportError:
print("ERROR: pymavlink not installed")
print("Run: pip install pymavlink")
sys.exit(1)
from config import MAVLINK
class SimpleDroneController:
"""Simple drone controller with takeoff, fly pattern, and land."""
def __init__(self, connection_string=None):
self.connection_string = connection_string or MAVLINK["connection_string"]
self.mav = None
self.armed = False
self.mode = "UNKNOWN"
self.altitude = 0
self.position = {"x": 0, "y": 0, "z": 0}
def connect(self, timeout=30):
"""Connect to ArduPilot SITL."""
print(f"[INFO] Connecting to {self.connection_string}...")
try:
self.mav = mavutil.mavlink_connection(self.connection_string)
self.mav.wait_heartbeat(timeout=timeout)
print(f"[OK] Connected to system {self.mav.target_system}")
return True
except Exception as e:
print(f"[ERROR] Connection failed: {e}")
return False
def update_state(self):
"""Update drone state from MAVLink messages."""
while True:
msg = self.mav.recv_match(blocking=False)
if msg is None:
break
msg_type = msg.get_type()
if msg_type == "HEARTBEAT":
self.armed = (msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED) != 0
try:
self.mode = mavutil.mode_string_v10(msg)
except:
self.mode = str(msg.custom_mode)
elif msg_type == "LOCAL_POSITION_NED":
self.position = {"x": msg.x, "y": msg.y, "z": msg.z}
self.altitude = -msg.z # NED: down is positive
def set_mode(self, mode_name):
"""Set flight mode."""
mode_map = self.mav.mode_mapping()
if mode_name.upper() not in mode_map:
print(f"[ERROR] Unknown mode: {mode_name}")
return False
mode_id = mode_map[mode_name.upper()]
self.mav.set_mode(mode_id)
time.sleep(0.5)
self.update_state()
print(f"[OK] Mode: {self.mode}")
return True
def arm(self):
"""Force arm the drone."""
print("[INFO] Arming...")
for attempt in range(3):
self.mav.mav.command_long_send(
self.mav.target_system,
self.mav.target_component,
mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
0, 1, 21196, 0, 0, 0, 0, 0 # 21196 = force arm
)
time.sleep(1)
self.update_state()
if self.armed:
print("[OK] Armed")
return True
print(f"[WARN] Arm attempt {attempt + 1}/3...")
print("[ERROR] Failed to arm")
return False
def takeoff(self, altitude=5.0):
"""Takeoff to altitude."""
print(f"[INFO] Taking off to {altitude}m...")
self.mav.mav.command_long_send(
self.mav.target_system,
self.mav.target_component,
mavutil.mavlink.MAV_CMD_NAV_TAKEOFF,
0, 0, 0, 0, 0, 0, 0, altitude
)
# Wait for altitude
for i in range(100): # 10 seconds max
self.update_state()
if self.altitude > altitude * 0.9:
print(f"[OK] Reached {self.altitude:.1f}m")
return True
print(f"\r Climbing... {self.altitude:.1f}m", end="")
time.sleep(0.1)
print(f"\n[WARN] Takeoff timeout at {self.altitude:.1f}m")
return False
def goto(self, x, y, z):
"""Go to position (NED frame, z is down so negative = up)."""
print(f"[INFO] Going to ({x:.1f}, {y:.1f}, alt={-z:.1f}m)...")
self.mav.mav.set_position_target_local_ned_send(
0,
self.mav.target_system,
self.mav.target_component,
mavutil.mavlink.MAV_FRAME_LOCAL_NED,
0b0000111111111000, # Position only
x, y, z, # Position (NED)
0, 0, 0, # Velocity
0, 0, 0, # Acceleration
0, 0 # Yaw, yaw_rate
)
def fly_to_and_wait(self, x, y, altitude, tolerance=0.5, timeout=30):
"""Fly to position and wait until reached."""
z = -altitude # NED
self.goto(x, y, z)
for i in range(int(timeout * 10)):
self.update_state()
dx = x - self.position["x"]
dy = y - self.position["y"]
dist = math.sqrt(dx*dx + dy*dy)
if dist < tolerance:
print(f"[OK] Reached waypoint ({x:.1f}, {y:.1f})")
return True
if i % 10 == 0:
print(f"\r Distance: {dist:.1f}m", end="")
time.sleep(0.1)
print(f"\n[WARN] Timeout reaching waypoint")
return False
def land(self):
"""Land the drone."""
print("[INFO] Landing...")
return self.set_mode("LAND")
def fly_square(self, size=5, altitude=5):
"""Fly a square pattern."""
waypoints = [
(size, 0),
(size, size),
(0, size),
(0, 0),
]
print(f"\n[INFO] Flying square pattern ({size}m x {size}m)")
for i, (x, y) in enumerate(waypoints):
print(f"\n--- Waypoint {i+1}/4 ---")
self.fly_to_and_wait(x, y, altitude)
time.sleep(1)
print("\n[OK] Square pattern complete!")
def fly_circle(self, radius=5, altitude=5, points=8):
"""Fly a circular pattern."""
print(f"\n[INFO] Flying circle pattern (radius={radius}m)")
for i in range(points + 1):
angle = 2 * math.pi * i / points
x = radius * math.cos(angle)
y = radius * math.sin(angle)
print(f"\n--- Point {i+1}/{points+1} ---")
self.fly_to_and_wait(x, y, altitude)
time.sleep(0.5)
print("\n[OK] Circle pattern complete!")
def main():
parser = argparse.ArgumentParser(description="Simple Drone Controller")
parser.add_argument("--pattern", "-p", choices=["square", "circle", "hover"],
default="square", help="Flight pattern")
parser.add_argument("--altitude", "-a", type=float, default=5.0,
help="Flight altitude (meters)")
parser.add_argument("--size", "-s", type=float, default=5.0,
help="Pattern size/radius (meters)")
parser.add_argument("--connection", "-c", default=None,
help="MAVLink connection string")
args = parser.parse_args()
print("\n" + "=" * 50)
print(" Simple Drone Controller")
print("=" * 50)
print(f" Pattern: {args.pattern}")
print(f" Altitude: {args.altitude}m")
print(f" Size: {args.size}m")
print("=" * 50 + "\n")
drone = SimpleDroneController(args.connection)
if not drone.connect():
print("\n[ERROR] Could not connect to SITL")
print("Make sure sim_vehicle.py is running")
sys.exit(1)
try:
# Setup
print("\n--- SETUP ---")
drone.set_mode("GUIDED")
time.sleep(1)
if not drone.arm():
print("[ERROR] Could not arm")
sys.exit(1)
# Takeoff
print("\n--- TAKEOFF ---")
if not drone.takeoff(args.altitude):
print("[WARN] Takeoff may have failed")
time.sleep(2) # Stabilize
# Fly pattern
print("\n--- FLY PATTERN ---")
if args.pattern == "square":
drone.fly_square(size=args.size, altitude=args.altitude)
elif args.pattern == "circle":
drone.fly_circle(radius=args.size, altitude=args.altitude)
elif args.pattern == "hover":
print("[INFO] Hovering for 10 seconds...")
time.sleep(10)
# Land
print("\n--- LAND ---")
drone.land()
# Wait for landing
print("[INFO] Waiting for landing...")
for i in range(100):
drone.update_state()
if drone.altitude < 0.3:
print("[OK] Landed!")
break
print(f"\r Altitude: {drone.altitude:.1f}m", end="")
time.sleep(0.1)
print("\n\n[OK] Mission complete!")
except KeyboardInterrupt: except KeyboardInterrupt:
print('\nShutting down...') print("\n\n[INFO] Interrupted - Landing...")
finally: drone.land()
if controller:
controller.destroy_node()
if rclpy.ok():
rclpy.shutdown()
if __name__ == '__main__': if __name__ == "__main__":
main() main()