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:**
```bash
source activate.sh
./scripts/run_ardupilot_sim.sh runway
```
### Prerequisites
- Ubuntu 22.04/24.04 (or WSL2 on Windows)
- Python 3.10+
- ~10GB disk space
**Terminal 2 - Controller:**
```bash
source activate.sh
./scripts/run_ardupilot_controller.sh
```
## World Options
### Installation
```bash
./scripts/run_ardupilot_sim.sh runway # Default (outdoor)
./scripts/run_ardupilot_sim.sh warehouse # Indoor
./scripts/run_ardupilot_sim.sh custom # Custom landing pad
./scripts/run_ardupilot_sim.sh my_world # gazebo/worlds/my_world.sdf
```
# Clone the repository
git clone <repo-url> RDC_Simulation
cd RDC_Simulation
## Installation
# Install everything (20-30 minutes)
./setup/install_ubuntu.sh --with-ardupilot
### Ubuntu (Native or WSL2)
```bash
./setup/install_ubuntu.sh
./setup/install_ardupilot.sh
# Reload environment
source ~/.bashrc
```
### Windows (via WSL2)
### Run the Simulation
```powershell
# PowerShell as Administrator
wsl --install -d Ubuntu-24.04
# Restart, then in Ubuntu terminal:
./setup/install_ubuntu.sh --with-ardupilot
You need **3 terminals**:
**Terminal 1 - Gazebo:**
```bash
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
./setup/install_macos.sh
python standalone_simulation.py # Gazebo not supported
```
# Fly a square pattern (default)
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
```
simulation/
├── config.py
├── src/
│ └── drone_controller.py # Your algorithm
RDC_Simulation/
├── scripts/
│ ├── run_ardupilot_sim.sh
│ └── run_ardupilot_controller.sh
├── gazebo/
│ ├── worlds/ # Your worlds
│ └── models/ # Your models
│ ├── run_ardupilot_sim.sh # Launch Gazebo
│ ├── run_ardupilot_controller.sh # Launch SITL + Controller
│ └── run_ardupilot.py # Flight controller
├── src/
│ ├── drone_controller.py # Simple drone controller
│ ├── rover_controller.py # Moving landing pad (ROS 2)
│ └── camera_viewer.py # Camera viewer (ROS 2)
├── 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
**sim_vehicle.py not found:**
```bash
source ~/.ardupilot_env
```
### "No JSON sensor message received"
Gazebo isn't sending data to SITL.
- **Fix**: Start Gazebo FIRST, wait for it to fully load, THEN start SITL.
**empy not found:**
```bash
source ~/venv-ardupilot/bin/activate
pip install empy==3.3.4
```
### "empy not found"
Wrong Python environment.
- **Fix**: `source ~/venv-ardupilot/bin/activate`
**Gazebo slow (software rendering):**
```bash
glxinfo | grep "OpenGL renderer"
# Should show GPU, not "llvmpipe"
```
### "sim_vehicle.py not found"
ArduPilot tools not in PATH.
- **Fix**: `source ~/.ardupilot_env`
### 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
- [Installation](docs/installation.md) - Full setup for all platforms + WSL
- [ArduPilot Guide](docs/ardupilot.md) - SITL setup and troubleshooting
- [Drone Controller](docs/drone_guide.md) - Landing algorithm
- [Custom Worlds](docs/gazebo_worlds.md) - Create Gazebo environments
- [Blender Models](docs/blender_models.md) - 3D model workflow
- [Architecture](docs/architecture.md) - System design
- [Installation Guide](docs/installation.md)
- [ArduPilot Guide](docs/ardupilot.md)
- [Architecture](docs/architecture.md)
- [Gazebo Worlds](docs/gazebo_worlds.md)
## 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:
```bash
source activate.sh
# 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
```
This project uses:
- **ArduPilot SITL**: Software-in-the-loop flight controller
- **Gazebo**: 3D physics simulation
- **MAVLink**: Communication protocol
## Architecture
```
┌─────────────────┐ ┌─────────────────┐ ┌─────────────────┐
│ Gazebo │◄───►│ ArduPilot SITL │◄───►│ run_ardupilot.py│
│ (Physics) │JSON │ (Flight Ctrl) │MAV │ (Your Logic) │
└─────────────────┘ └─────────────────┘ └─────────────────┘
┌─────────────┐ JSON (UDP 9002) ┌─────────────┐ MAVLink (TCP 5760) ┌─────────────┐
│ Gazebo │◄──────────────────────►│ ArduPilot │◄────────────────────────►│ Your │
│ (Physics) │ sensor data │ SITL │ commands/telemetry │ Controller │
└─────────────┘ └─────────────┘ └─────────────┘
```
**Communication:**
- Gazebo ↔ SITL: JSON sensor data over UDP (port 9002)
- SITL ↔ Your Code: MAVLink over TCP (port 5760)
## Quick Start
## World Options
### Step 1: Start Gazebo
```bash
./scripts/run_ardupilot_sim.sh runway # Default (outdoor runway)
./scripts/run_ardupilot_sim.sh warehouse # Indoor warehouse
./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
cd ~/RDC_Simulation
./scripts/run_ardupilot_sim.sh runway
```
## 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.
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
### Step 2: Start SITL
```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
sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --console
```
# Gazebo plugin paths
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
You should see telemetry scrolling (NOT "No JSON sensor message").
### 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
### "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:**
1. Start Gazebo first (Terminal 1)
2. Wait for world to fully load
3. Then start controller (Terminal 2)
1. Make sure Gazebo is running FIRST
2. Wait for the world to fully load
3. Then start SITL
### "sim_vehicle.py not found"
**Cause:** ArduPilot tools not in PATH.
### Drone won't arm
**Fix:**
```bash
source ~/.ardupilot_env
# or
export PATH=$PATH:~/ardupilot/Tools/autotest
# Check pre-arm status in MAVProxy
arm status
# Force arm
arm throttle force
```
### "empy not found" during waf build
**Cause:** Wrong Python environment.
### Wrong Python environment
**Fix:**
```bash
source ~/venv-ardupilot/bin/activate
pip install empy==3.3.4
cd ~/ardupilot
./waf configure --board sitl
./waf copter
which python3 # Should be ~/venv-ardupilot/bin/python3
```
### Drone doesn't respond to commands
**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.
### Gazebo plugin not loading
**Fix:**
```bash
# Rebuild the ArduPilot Gazebo plugin
cd ~/ardupilot_gazebo/build
cmake .. -DCMAKE_BUILD_TYPE=Release
make -j4
# Check plugin exists
ls ~/ardupilot_gazebo/build/libArduPilotPlugin.so
# 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
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
# Run directly
# List Gazebo topics
gz topic -l
# Check SITL status
sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --console --map
# If display errors:
export DISPLAY=:0 # Linux/WSL with X server
```
## 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
# View camera (if available)
gz topic -e -t /camera
```

241
docs/drone_guide.md
View File

@@ -1,79 +1,176 @@
# DroneController Guide
# Drone Controller Guide
## 3-Phase Mission
## Overview
```
SEARCH ──► COMMAND ──► LAND ──► COMPLETE
```
The drone controller (`src/drone_controller.py`) provides a simple interface for flying the drone in simulation.
| Phase | Action |
|-------|--------|
| SEARCH | Find QR code on rover |
| COMMAND | Send commands to rover |
| LAND | Land on rover |
## Usage
## Your Code
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
### Command Line
```bash
./scripts/run_ardupilot_sim.sh runway
./scripts/run_ardupilot_controller.sh
source ~/venv-ardupilot/bin/activate
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
./setup/install_ubuntu.sh
./setup/install_ardupilot.sh
# Clone repository
git clone <repo-url> RDC_Simulation
cd RDC_Simulation
# Run installer (includes ArduPilot)
./setup/install_ubuntu.sh --with-ardupilot
# Reload shell
source ~/.bashrc
```
## What Gets Installed
This installs:
- Python 3 + virtual environment
- Gazebo Harmonic
- ArduPilot SITL
- ArduPilot Gazebo plugin
- MAVProxy
| Component | Location |
|-----------|----------|
| ArduPilot SITL | `~/ardupilot` |
| ArduPilot venv | `~/venv-ardupilot` |
| ardupilot_gazebo | `~/ardupilot_gazebo` |
| Gazebo Harmonic | System |
| ROS 2 | System |
| MAVProxy | `~/.local/bin` |
## Windows (WSL2)
## Platform-Specific Installation
### 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
### Step 1: Install WSL2
Open PowerShell as Administrator:
```powershell
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
# Clone the project (or copy from Windows)
git clone https://github.com/your-repo/RDC_Simulation.git
cd RDC_Simulation
# Update system
sudo apt update && sudo apt upgrade -y
# Run installation
./setup/install_ubuntu.sh
./setup/install_ardupilot.sh
# Clone and install
git clone <repo-url> ~/RDC_Simulation
cd ~/RDC_Simulation
./setup/install_ubuntu.sh --with-ardupilot
# Reload
source ~/.bashrc
```
#### Step 4: GPU Acceleration in WSL
### WSL2 Tips
For best performance with Gazebo:
```bash
# Check GPU availability
glxinfo | grep "OpenGL renderer"
# Should show your GPU, not "llvmpipe"
# If using NVIDIA, install NVIDIA GPU driver for WSL:
# https://developer.nvidia.com/cuda/wsl
```
#### WSL Tips
- **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:
- **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)
- **Access Windows files**: `/mnt/c/Users/YourName/`
- **Performance**: Clone repo to Linux filesystem (not `/mnt/c/`)
## macOS
macOS doesn't support Gazebo Harmonic natively. Options:
1. **Docker** (recommended): Use Linux container
2. **VM**: Use UTM or Parallels with Ubuntu
3. **Standalone**: Run PyBullet-only simulation
```bash
# Install basic dependencies
./setup/install_macos.sh
# Use standalone simulation
python standalone_simulation.py
```
For full simulation, use a Linux VM (UTM for Apple Silicon) or Docker.
### Arch Linux
## Arch Linux
```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
```bash
# Source the environment
source activate.sh
# Check ArduPilot tools
# Check ArduPilot
source ~/venv-ardupilot/bin/activate
python -c "import em; print('empy OK')"
sim_vehicle.py --help
which mavproxy.py
# 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
cd ~/RDC_Simulation
source activate.sh
# or
source ~/.bashrc # if ArduPilot was installed
```
The activation script sets up:
- ROS 2 environment
- ArduPilot Python venv (with empy, pymavlink, etc.)
- Gazebo resource paths
- ArduPilot tools path
This sources:
- ROS 2 (if installed)
- ArduPilot virtual environment
- Gazebo paths
### 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
### "empy not found" or waf configure fails
### "empy not found"
The ArduPilot venv isn't activated. Run:
```bash
source ~/venv-ardupilot/bin/activate
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
source ~/.ardupilot_env
# or
# OR
export PATH=$PATH:~/ardupilot/Tools/autotest
```
### mavproxy.py not found
```bash
export PATH=$PATH:~/.local/bin
```
### pexpect error
```bash
pip install pexpect
```
### Gazebo slow / Software rendering
### Gazebo crashes / slow
```bash
# Check GPU
glxinfo | grep "OpenGL renderer"
```
Should show your GPU, not "llvmpipe". Install proper GPU drivers:
- **NVIDIA:** `sudo apt install nvidia-driver-XXX`
- **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
# Try software rendering (slow but works)
export LIBGL_ALWAYS_SOFTWARE=1
gz sim -v4 -r ~/ardupilot_gazebo/worlds/iris_runway.sdf
```
### WSL: Display not working
```bash
# Check DISPLAY variable
echo $DISPLAY
# Check WSLg
ls /mnt/wslg
# For WSLg (Windows 11), should be auto-set
# For Windows 10 with X server:
export DISPLAY=:0
# If missing, update WSL
wsl --update
```
### 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
"""
ArduPilot Drone Controller
==========================
Connects your drone_controller.py logic to ArduPilot SITL via MAVLink.
ArduPilot Flight Demo
=====================
Simple script to make the drone takeoff, fly a pattern, and land.
Usage:
Terminal 1: ./scripts/run_ardupilot_sim.sh runway
Terminal 2: sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --console
Terminal 3: python run_ardupilot.py
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
Terminal 3: python scripts/run_ardupilot.py --pattern square
"""
import sys
import os
# Add project root to path
sys.path.insert(0, os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
import time
import math
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")
# Import the simplified drone controller
from src.drone_controller import main
if __name__ == "__main__":
main()

68
scripts/run_ardupilot_controller.sh
View File

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

135
scripts/run_ardupilot_sim.sh
View File

@@ -1,96 +1,52 @@
#!/bin/bash
# =============================================================================
# ArduPilot GPS-Denied Simulation Launcher
# ArduPilot Gazebo Simulation Launcher
# =============================================================================
# Launches Gazebo with GPU acceleration and camera support
# Automatically detects GPU: NVIDIA > Intel/AMD integrated > Software
# Launches Gazebo with the ArduPilot iris drone model.
# Start this FIRST, then run SITL in another terminal.
#
# Usage:
# ./scripts/run_ardupilot_sim.sh # Default iris_runway
# ./scripts/run_ardupilot_sim.sh camera # With camera world
# ./scripts/run_ardupilot_sim.sh # Default runway
# ./scripts/run_ardupilot_sim.sh warehouse # Indoor warehouse
# =============================================================================
set -e
echo "=============================================="
echo " ArduPilot GPS-Denied Simulation"
echo " Gazebo + ArduPilot Simulation"
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..."
# Disable software rendering by default
export LIBGL_ALWAYS_SOFTWARE=0
# Check for NVIDIA dedicated GPU
if command -v nvidia-smi &> /dev/null && nvidia-smi &> /dev/null; then
if command -v nvidia-smi &> /dev/null && nvidia-smi &> /dev/null 2>&1; then
GPU_NAME=$(nvidia-smi --query-gpu=name --format=csv,noheader 2>/dev/null | head -1)
echo "[GPU] NVIDIA (dedicated): $GPU_NAME"
echo "[GPU] NVIDIA: $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"
export __GLX_VENDOR_LIBRARY_NAME=nvidia
GPU_TYPE="nvidia"
elif [[ "$GPU_NAME" == *"Intel"* ]]; then
echo "[GPU] Intel: $GPU_NAME"
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"
export MESA_GL_VERSION_OVERRIDE=3.3
GPU_TYPE="other"
fi
# Fallback to Mesa defaults
echo "[GPU] $GPU_NAME"
export MESA_GL_VERSION_OVERRIDE=3.3
else
echo "[GPU] Unknown - using Mesa defaults"
export MESA_GL_VERSION_OVERRIDE=3.3
GPU_TYPE="unknown"
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
# =============================================================================
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}"
# 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}"
# =============================================================================
@@ -114,77 +70,48 @@ fi
WORLD_ARG="${1:-runway}"
case "$WORLD_ARG" in
runway|iris|default)
runway|default)
WORLD="${HOME}/ardupilot_gazebo/worlds/iris_runway.sdf"
;;
warehouse)
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
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
WORLD="${HOME}/ardupilot_gazebo/worlds/${WORLD_ARG}.sdf"
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
;;
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 ""
# =============================================================================
# INSTRUCTIONS
# =============================================================================
echo ""
echo "=============================================="
echo " Starting Gazebo"
echo " STEP 1: Gazebo Starting..."
echo "=============================================="
echo ""
echo "World: $WORLD"
echo ""
echo "After Gazebo starts, in another terminal run:"
echo "After Gazebo loads, run SITL in another terminal:"
echo ""
echo " source ~/venv-ardupilot/bin/activate"
echo " sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --console"
echo ""
echo "For GPS-DENIED mode, in MAVProxy console:"
echo "Then run the flight controller:"
echo ""
echo " param set ARMING_CHECK 0"
echo " mode stabilize"
echo " arm throttle force"
echo " source ~/venv-ardupilot/bin/activate"
echo " cd ~/RDC_Simulation"
echo " python scripts/run_ardupilot.py --pattern square"
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!"

702
src/drone_controller.py
View File

@@ -1,500 +1,286 @@
#!/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:
1. SEARCH - Find QR code on rover using camera
2. COMMAND - Send commands to rover
3. LAND - Land on the rover
Usage:
Terminal 1: ./scripts/run_ardupilot_sim.sh runway
Terminal 2: sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --console
Terminal 3: python scripts/run_ardupilot.py
"""
import json
import sys
import os
import time
import math
import base64
from enum import Enum, auto
from typing import Dict, Any, Optional
import argparse
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
sys.path.insert(0, os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
try:
from config import CONTROLLER, DRONE, LANDING
CONFIG_LOADED = True
from pymavlink import mavutil
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}
print("ERROR: pymavlink not installed")
print("Run: pip install pymavlink")
sys.exit(1)
try:
import cv2
import numpy as np
CV2_AVAILABLE = True
except ImportError:
CV2_AVAILABLE = False
from config import MAVLINK
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)
class SimpleDroneController:
"""Simple drone controller with takeoff, fly pattern, and land."""
sinp = 2.0 * (w * y - z * x)
if abs(sinp) >= 1:
pitch = math.copysign(math.pi / 2, sinp)
else:
pitch = math.asin(sinp)
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
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)
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
return roll, pitch, yaw
class DroneController(Node):
def __init__(self, use_ardupilot_topics: bool = True):
super().__init__('drone_controller')
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
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)
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...")
self._use_ardupilot = use_ardupilot_topics
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...")
self._phase = Phase.SEARCH
self._phase_start_time = self.get_clock().now()
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._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
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
)
if use_ardupilot_topics:
self._setup_ardupilot_subscriptions(sensor_qos)
else:
self._setup_legacy_subscriptions()
# 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)
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}')
print(f"\n[WARN] Takeoff timeout at {self.altitude:.1f}m")
return False
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 = {}
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)...")
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
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 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)
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)
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)
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)
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
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)
yaw = 0.0
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),
]
return (thrust, pitch, roll, yaw)
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 _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 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(args=None):
import sys
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()
use_ardupilot = '--ardupilot' in sys.argv or '-a' in sys.argv
filtered_args = [a for a in (args or sys.argv) if a not in ['--ardupilot', '-a']]
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")
rclpy.init(args=filtered_args)
controller = None
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:
controller = DroneController(use_ardupilot_topics=use_ardupilot)
rclpy.spin(controller)
# 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:
print('\nShutting down...')
finally:
if controller:
controller.destroy_node()
if rclpy.ok():
rclpy.shutdown()
print("\n\n[INFO] Interrupted - Landing...")
drone.land()
if __name__ == '__main__':
if __name__ == "__main__":
main()