feat: Simplify drone controller to basic flight pattern, overhaul installation guide, and update related scripts.
This commit is contained in:
178
README.md
178
README.md
@@ -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
|
||||
@@ -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
|
||||
```
|
||||
|
||||
@@ -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"
|
||||
```
|
||||
|
||||
@@ -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`
|
||||
|
||||
@@ -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()
|
||||
|
||||
@@ -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 "$@"
|
||||
|
||||
@@ -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
114
scripts/run_flight_demo.sh
Executable 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!"
|
||||
@@ -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()
|
||||
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 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)
|
||||
def connect(self, timeout=30):
|
||||
"""Connect to ArduPilot SITL."""
|
||||
print(f"[INFO] Connecting to {self.connection_string}...")
|
||||
|
||||
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
|
||||
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:
|
||||
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):
|
||||
print(f"[ERROR] Connection failed: {e}")
|
||||
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 update_state(self):
|
||||
"""Update drone state from MAVLink messages."""
|
||||
while True:
|
||||
msg = self.mav.recv_match(blocking=False)
|
||||
if msg is None:
|
||||
break
|
||||
|
||||
def get_current_phase(self) -> Phase:
|
||||
return self._phase
|
||||
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(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()
|
||||
|
||||
Reference in New Issue
Block a user