diff --git a/README.md b/README.md index 75062e3..a744d6e 100644 --- a/README.md +++ b/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 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 \ No newline at end of file +- [Installation Guide](docs/installation.md) +- [ArduPilot Guide](docs/ardupilot.md) +- [Architecture](docs/architecture.md) +- [Gazebo Worlds](docs/gazebo_worlds.md) + +## License + +MIT License \ No newline at end of file diff --git a/docs/ardupilot.md b/docs/ardupilot.md index 8925578..f5da70d 100644 --- a/docs/ardupilot.md +++ b/docs/ardupilot.md @@ -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 ``` diff --git a/docs/drone_guide.md b/docs/drone_guide.md index 93d29b4..e4f6311 100644 --- a/docs/drone_guide.md +++ b/docs/drone_guide.md @@ -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" ``` diff --git a/docs/installation.md b/docs/installation.md index 7ce282f..6c36b34 100644 --- a/docs/installation.md +++ b/docs/installation.md @@ -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 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 ~/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` diff --git a/scripts/run_ardupilot.py b/scripts/run_ardupilot.py index 1f7dc2f..6054694 100644 --- a/scripts/run_ardupilot.py +++ b/scripts/run_ardupilot.py @@ -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() diff --git a/scripts/run_ardupilot_controller.sh b/scripts/run_ardupilot_controller.sh index 9dec03f..5caac5b 100755 --- a/scripts/run_ardupilot_controller.sh +++ b/scripts/run_ardupilot_controller.sh @@ -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 "$@" diff --git a/scripts/run_ardupilot_sim.sh b/scripts/run_ardupilot_sim.sh index 8d096aa..f446f48 100755 --- a/scripts/run_ardupilot_sim.sh +++ b/scripts/run_ardupilot_sim.sh @@ -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 "" diff --git a/scripts/run_flight_demo.sh b/scripts/run_flight_demo.sh new file mode 100755 index 0000000..e898782 --- /dev/null +++ b/scripts/run_flight_demo.sh @@ -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!" diff --git a/src/drone_controller.py b/src/drone_controller.py index 21a1aa0..11e1f91 100644 --- a/src/drone_controller.py +++ b/src/drone_controller.py @@ -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()