diff --git a/README.md b/README.md index 6bf91f6..c641b6d 100644 --- a/README.md +++ b/README.md @@ -1,30 +1,8 @@ -# Drone Landing Simulation (GPS-Denied) +# GPS-Denied Drone Landing Simulation -Land a drone on a moving platform using only relative sensors. +ArduPilot + ROS 2 + Gazebo (ARG) simulation for landing on a moving platform. -## Quick Start - -### Standalone (1 Terminal) - -```bash -source activate.sh -python standalone_simulation.py --pattern circular -``` - -### Gazebo + ROS 2 (2 Terminals) - -**Terminal 1:** -```bash -ros2 launch gazebo/launch/drone_landing.launch.py -``` - -**Terminal 2:** -```bash -source activate.sh -python run_gazebo.py --pattern circular -``` - -### ArduPilot GPS-Denied (2 Terminals) +## Quick Start (2 Terminals) **Terminal 1 - Gazebo:** ```bash @@ -36,38 +14,53 @@ python run_gazebo.py --pattern circular ./scripts/run_ardupilot_controller.sh ``` +## World Options + +```bash +./scripts/run_ardupilot_sim.sh runway # Default +./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 +``` + ## Installation ```bash ./setup/install_ubuntu.sh -./setup/install_ardupilot.sh # Optional for ArduPilot -source activate.sh +./setup/install_ardupilot.sh +source ~/.bashrc ``` -## Files +## Project Structure -| File | Description | -|------|-------------| -| `drone_controller.py` | **Your landing algorithm (used everywhere)** | -| `standalone_simulation.py` | PyBullet simulation | -| `run_gazebo.py` | Gazebo + ROS 2 interface | -| `run_ardupilot.py` | ArduPilot + MAVLink interface | -| `config.py` | Configuration settings | -| `camera_viewer.py` | Camera feed window | +``` +simulation/ +├── config.py +├── src/ +│ └── drone_controller.py # Your algorithm +├── scripts/ +│ ├── run_ardupilot_sim.sh +│ └── run_ardupilot_controller.sh +├── gazebo/ +│ ├── worlds/ # Your worlds +│ └── models/ # Your models +├── setup/ +└── docs/ +``` -## Sensors (GPS-Denied) +## 3-Phase Mission -| Sensor | Data | -|--------|------| -| IMU | Orientation, angular velocity | -| Altimeter | Altitude, vertical velocity | -| Camera | Downward image | -| Landing Pad | Relative position (when visible) | +| Phase | Action | +|-------|--------| +| SEARCH | Find QR code on rover | +| COMMAND | Send commands to rover | +| LAND | Land on rover | ## Documentation - [Installation](docs/installation.md) -- [Architecture](docs/architecture.md) - [ArduPilot Guide](docs/ardupilot.md) -- [Gazebo Guide](docs/gazebo.md) -- [Drone Logic Guide](docs/drone_guide.md) \ No newline at end of file +- [Drone Controller](docs/drone_guide.md) +- [Custom Worlds](docs/gazebo_worlds.md) +- [Blender Models](docs/blender_models.md) +- [Architecture](docs/architecture.md) \ No newline at end of file diff --git a/docs/architecture.md b/docs/architecture.md index db8124f..922785b 100644 --- a/docs/architecture.md +++ b/docs/architecture.md @@ -1,98 +1,67 @@ -# Architecture Overview +# Architecture -## Modes - -### 1. Standalone (1 Terminal) - -```bash -python standalone_simulation.py --pattern circular -``` +## System Overview ``` -┌────────────────────────────────────────┐ -│ standalone_simulation.py │ -│ PyBullet + Controllers + Camera │ -└────────────────────────────────────────┘ +┌─────────────────┐ ┌─────────────────┐ ┌─────────────────┐ +│ Gazebo │◄───►│ ArduPilot SITL │◄───►│ Controller │ +│ (Physics) │JSON │ (Flight Ctrl) │MAV │ (Your Logic) │ +└─────────────────┘ └─────────────────┘ └─────────────────┘ + │ │ + ▼ ▼ + ~/ardupilot_gazebo src/drone_controller.py ``` -### 2. Gazebo + ROS 2 (2 Terminals) +## Terminal Layout +**Terminal 1:** ``` -Terminal 1 Terminal 2 -┌───────────────────┐ ┌───────────────────┐ -│ Gazebo + Bridge │◄──────►│ run_gazebo.py │ -└───────────────────┘ ROS └───────────────────┘ +./scripts/run_ardupilot_sim.sh + │ + ▼ +Gazebo + ArduPilot Plugin ``` -### 3. ArduPilot (2 Terminals) - +**Terminal 2:** ``` -Terminal 1 Terminal 2 -┌───────────────────┐ ┌────────────────────────────┐ -│ Gazebo + │◄──────►│ run_ardupilot_controller.sh│ -│ ArduPilot Plugin │ JSON │ ┌──────────────────┐ │ -└───────────────────┘ │ │ ArduPilot SITL │ │ - │ └─────────┬────────┘ │ - │ │ MAVLink │ - │ ┌─────────▼────────┐ │ - │ │ run_ardupilot.py │ │ - │ └──────────────────┘ │ - └────────────────────────────┘ +./scripts/run_ardupilot_controller.sh + │ + ├── ArduPilot SITL (background) + │ + └── run_ardupilot.py + │ + └── src/drone_controller.py ``` ## Data Flow -### Standalone ``` -Controller → PyBullet → Telemetry → Controller -``` - -### Gazebo -``` -Controller → /cmd_vel → Gazebo → /odometry → Controller -``` - -### ArduPilot -``` -Gazebo ◄─── JSON ───► SITL ◄─── MAVLink ───► Controller +Gazebo ◄─── JSON/UDP ───► SITL ◄─── MAVLink ───► Controller + │ │ │ + │ Physics │ Flight control │ Your logic + │ Sensors │ EKF │ 3-phase mission + │ Rendering │ Stabilization │ QR detection + ▼ ▼ ▼ + Display Attitude/Position Commands ``` ## Key Files | File | Purpose | |------|---------| -| `drone_controller.py` | **Your landing algorithm (used in ALL modes)** | -| `run_ardupilot.py` | MAVLink interface for ArduPilot | -| `run_gazebo.py` | ROS 2 interface for Gazebo | -| `standalone_simulation.py` | PyBullet simulation engine | -| `config.py` | Shared configuration | +| `src/drone_controller.py` | 3-phase mission logic | +| `scripts/run_ardupilot.py` | MAVLink interface | +| `src/mavlink_bridge.py` | MAVLink utilities | +| `src/gazebo_bridge.py` | Gazebo ROS bridge | +| `config.py` | Configuration | -## GPS-Denied Sensors +## 3-Phase Mission -The controller receives this standardized telemetry structure in all modes: - -```python -telemetry = { - "altimeter": { - "altitude": float, # Meters - "vertical_velocity": float # m/s (positive = up) - }, - "velocity": { # Body or Local frame - "x": float, - "y": float, - "z": float - }, - "imu": { - "orientation": { - "roll": float, - "pitch": float, - "yaw": float - } - }, - "landing_pad": { # If visible (None otherwise) - "relative_x": float, - "relative_y": float, - "distance": float - } -} +``` +┌────────┐ QR Found ┌─────────┐ Timeout ┌──────┐ +│ SEARCH │───────────────►│ COMMAND │──────────────►│ LAND │ +└────────┘ └─────────┘ └──────┘ + │ │ │ + ▼ ▼ ▼ + Detect QR Send to rover Track & descend ``` diff --git a/docs/ardupilot.md b/docs/ardupilot.md index 297cff2..6195fd0 100644 --- a/docs/ardupilot.md +++ b/docs/ardupilot.md @@ -1,38 +1,7 @@ -# ArduPilot GPS-Denied Simulation - -Realistic flight controller simulation with your drone logic. +# ArduPilot + Gazebo Simulation ## Quick Start (2 Terminals) -**Terminal 1 - Gazebo:** -```bash -./scripts/run_ardupilot_sim.sh runway -# Options: runway, warehouse, zephyr -``` - -**Terminal 2 - Controller + SITL:** -```bash -./scripts/run_ardupilot_controller.sh -``` - -## How It Works - -The `run_ardupilot_controller.sh` script starts ArduPilot SITL in the background and connects your controller to it via MAVLink. - -``` -┌─────────────────┐ ┌─────────────────┐ ┌─────────────────┐ -│ Gazebo │◄───►│ ArduPilot SITL │◄───►│ run_ardupilot.py│ -│ (Physics) │JSON │ (Hidden) │MAV │ (Your Logic) │ -└─────────────────┘ └─────────────────┘ └─────────────────┘ - │ - ▼ - drone_controller.py -``` - -## Manual Mode (Debugging) - -If you need to debug with MAVProxy console (3 Terminals): - **Terminal 1:** ```bash ./scripts/run_ardupilot_sim.sh runway @@ -40,50 +9,64 @@ If you need to debug with MAVProxy console (3 Terminals): **Terminal 2:** ```bash -sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --console +./scripts/run_ardupilot_controller.sh ``` -**Terminal 3:** +## Architecture + +``` +┌─────────────────┐ ┌─────────────────┐ ┌─────────────────┐ +│ Gazebo │◄───►│ ArduPilot SITL │◄───►│ run_ardupilot.py│ +│ (Physics) │JSON │ (Flight Ctrl) │MAV │ (Your Logic) │ +└─────────────────┘ └─────────────────┘ └─────────────────┘ +``` + +## World Options + ```bash -# Debug commands in MAVProxy: +./scripts/run_ardupilot_sim.sh runway # Default +./scripts/run_ardupilot_sim.sh warehouse # Indoor +./scripts/run_ardupilot_sim.sh zephyr # Fixed-wing +``` + +## GPS-Denied Mode + +The simulation runs in GPS-denied mode by default. + +For manual debugging with MAVProxy: +```bash +sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --console + +# In MAVProxy: param set ARMING_CHECK 0 -mode guided +mode stabilize arm throttle force ``` -## Installation +## Controller Options ```bash -./setup/install_ardupilot.sh -source ~/.bashrc +./scripts/run_ardupilot_controller.sh # Auto takeoff +./scripts/run_ardupilot_controller.sh --no-takeoff # Manual +./scripts/run_ardupilot_controller.sh -a 10 # 10m altitude ``` -## Configuration +## Files -Your `drone_controller.py` receives telemetry and returns control inputs. - -The simulation translates your inputs: -- `thrust` → Vertical velocity -- `pitch/roll` → Horizontal velocity -- `yaw` → Yaw rate +| File | Purpose | +|------|---------| +| `scripts/run_ardupilot_sim.sh` | Gazebo + GPU detection | +| `scripts/run_ardupilot_controller.sh` | SITL + Controller | +| `scripts/run_ardupilot.py` | MAVLink interface | +| `src/drone_controller.py` | Your algorithm | ## Troubleshooting -### "SITL failed to start" -Check if `sim_vehicle.py` is in your PATH: -```bash -export PATH=$PATH:~/ardupilot/Tools/autotest -``` - -### Drone drift -ArduPilot in GUIDED mode requires good position estimation. Without GPS, it relies on optical flow or visual odometry (not yet implemented in default setup). The drone might drift if relying only on IMU. - ### "No JSON sensor message" -Ensure Gazebo (Terminal 1) is running before starting the controller. +Start Gazebo (Terminal 1) before the controller. -## Visualizing Camera +### Drone doesn't respond +Check mode is GUIDED in MAVProxy. -```bash -python camera_viewer.py --topic /drone/camera -``` -(Requires bridging the topic if using ROS 2 bridge) +### Simulation laggy +Check GPU: `glxinfo | grep "OpenGL renderer"` diff --git a/docs/blender_models.md b/docs/blender_models.md new file mode 100644 index 0000000..5b2f306 --- /dev/null +++ b/docs/blender_models.md @@ -0,0 +1,168 @@ +# Blender Models in Gazebo + +Import 3D models from Blender into the ARG simulation. + +## Workflow + +``` +Blender (.blend) → Export COLLADA (.dae) → Gazebo Model → World +``` + +## Step 1: Create Model in Blender + +1. Create your 3D model +2. Apply all transforms: `Ctrl+A` → All Transforms +3. Set origin to geometry center + +## Step 2: Export from Blender + +1. File → Export → COLLADA (.dae) +2. Settings: + - Selection Only (if needed) + - Include Armatures: OFF + - Include Animations: OFF + - Triangulate: ON +3. Save as `model.dae` + +## Step 3: Create Gazebo Model + +``` +gazebo/models/my_model/ +├── model.config +├── model.sdf +├── meshes/ +│ └── model.dae +└── materials/ + └── textures/ + └── texture.png +``` + +### model.config + +```xml + + + My Model + 1.0 + model.sdf + Custom Blender model + +``` + +### model.sdf + +```xml + + + + true + + + + + meshes/model.dae + 1 1 1 + + + + + + + meshes/model.dae + 1 1 1 + + + + + + +``` + +## Step 4: Add to World + +```xml + + model://my_model + my_model_instance + 5 3 0 0 0 0 + +``` + +## Step 5: Set Model Path + +```bash +export GZ_SIM_RESOURCE_PATH=$PWD/gazebo/models:$GZ_SIM_RESOURCE_PATH +``` + +## Common Issues + +### Model Not Found +```bash +export GZ_SIM_RESOURCE_PATH=/full/path/to/gazebo/models:$GZ_SIM_RESOURCE_PATH +``` + +### Scale Wrong +In Blender, check unit settings: Properties → Scene → Units + +Adjust in SDF: +```xml +0.01 0.01 0.01 +``` + +### Textures Not Showing +Put textures in `materials/textures/` and reference in DAE file. + +Or add material in SDF: +```xml + + + meshes/model.dae + + + 0.8 0.2 0.2 1 + + +``` + +### Model Orientation Wrong +Blender uses Z-up, Gazebo uses Z-up. Should match. + +If rotated, fix in Blender or use pose: +```xml +0 0 0 1.5708 0 0 +``` + +## Simplified Collision + +For complex meshes, use simple collision: + +```xml + + + 2 2 3 + + + + + meshes/complex_model.dae + + +``` + +## Template + +Copy the template: +```bash +cp -r gazebo/models/custom_object gazebo/models/my_model +``` + +Then: +1. Edit `model.config` with your name +2. Edit `model.sdf` with your model name +3. Put your `model.dae` in `meshes/` + +## Test Model + +```bash +gz sim -v4 gazebo/worlds/custom_landing.sdf +``` diff --git a/docs/drone_guide.md b/docs/drone_guide.md index 67db852..93d29b4 100644 --- a/docs/drone_guide.md +++ b/docs/drone_guide.md @@ -1,73 +1,79 @@ # DroneController Guide -Implement your GPS-denied landing algorithm. +## 3-Phase Mission -## Quick Start +``` +SEARCH ──► COMMAND ──► LAND ──► COMPLETE +``` -1. Edit `drone_controller.py` -2. Find `calculate_landing_maneuver()` -3. Implement your algorithm -4. Test with any simulation mode +| Phase | Action | +|-------|--------| +| SEARCH | Find QR code on rover | +| COMMAND | Send commands to rover | +| LAND | Land on rover | -## Function to Implement +## 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): - # Your logic here return (thrust, pitch, roll, yaw) ``` -## Sensors (GPS-Denied) +## Telemetry ```python -# Altitude -altitude = telemetry['altimeter']['altitude'] -vertical_vel = telemetry['altimeter']['vertical_velocity'] - -# Velocity -vel_x = telemetry['velocity']['x'] -vel_y = telemetry['velocity']['y'] - -# Landing Pad (may be None!) -landing_pad = telemetry.get('landing_pad') -if landing_pad: - relative_x = landing_pad['relative_x'] - relative_y = landing_pad['relative_y'] +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 | Up/down (positive = up) | +| thrust | ±1.0 | Vertical velocity | | pitch | ±0.5 | Forward/back | | roll | ±0.5 | Left/right | | yaw | ±0.5 | Rotation | -Note: In ArduPilot mode, these are scaled to velocities: -- Thrust → Z velocity -- Pitch/Roll → X/Y velocity +## Configuration -## Example Algorithm (PD Control) +Edit `config.py`: ```python -def calculate_landing_maneuver(self, telemetry, rover_telemetry): - alt = telemetry.get('altimeter', {}) - altitude = alt.get('altitude', 5.0) - vert_vel = alt.get('vertical_velocity', 0.0) - - # Altitude PD control - thrust = 0.5 * (target_alt - altitude) - 0.3 * vert_vel - - # Horizontal control - pad = telemetry.get('landing_pad') - if pad: - pitch = 0.3 * pad['relative_x'] - roll = 0.3 * pad['relative_y'] - else: - # Hover - pitch = 0 - roll = 0 - - return (thrust, pitch, roll, 0.0) +CONTROLLER = { + "Kp_z": 0.5, + "Kd_z": 0.3, + "Kp_xy": 0.3, + "Kd_xy": 0.2, + "rate": 50, +} +``` + +## Testing + +```bash +./scripts/run_ardupilot_sim.sh runway +./scripts/run_ardupilot_controller.sh ``` diff --git a/docs/gazebo.md b/docs/gazebo.md deleted file mode 100644 index 4f94b6e..0000000 --- a/docs/gazebo.md +++ /dev/null @@ -1,66 +0,0 @@ -# Gazebo Simulation Guide - -## Quick Start (2 Terminals) - -**Terminal 1:** -```bash -ros2 launch gazebo/launch/drone_landing.launch.py -``` - -**Terminal 2:** -```bash -source activate.sh -python run_gazebo.py --pattern circular -``` - -## Camera Feed - -```bash -python camera_viewer.py --topic /drone/camera -``` - -## Options - -```bash ---pattern stationary, linear, circular, square, random ---speed Rover speed in m/s (default: 0.5) ---no-rover Disable rover movement -``` - -## Sensors - -| Sensor | Source | -|--------|--------| -| IMU | Gazebo odometry | -| Altimeter | Z position | -| Camera | Camera sensor | -| Landing Pad | Relative position | - -## ROS 2 Topics - -| Topic | Direction | -|-------|-----------| -| `/cmd_vel` | Your commands → Drone | -| `/drone/telemetry` | Sensors → You | -| `/drone/camera` | Camera images | - -## Headless Mode (WSL2) - -```bash -ign gazebo -s gazebo/worlds/drone_landing.sdf -``` - -## Troubleshooting - -**Drone falls:** -Check `run_gazebo.py` is running - -**No camera:** -```bash -python camera_viewer.py --list # Find topics -``` - -**Model not found:** -```bash -export GZ_SIM_RESOURCE_PATH=$PWD/gazebo/models:$GZ_SIM_RESOURCE_PATH -``` diff --git a/docs/gazebo_worlds.md b/docs/gazebo_worlds.md new file mode 100644 index 0000000..648f869 --- /dev/null +++ b/docs/gazebo_worlds.md @@ -0,0 +1,167 @@ +# Custom Gazebo Worlds + +Create custom environments for the ARG simulation. + +## Quick Start + +1. Copy template world: +```bash +cp gazebo/worlds/custom_landing.sdf gazebo/worlds/my_world.sdf +``` + +2. Edit the world file + +3. Run: +```bash +./scripts/run_ardupilot_sim.sh gazebo/worlds/my_world.sdf +``` + +## World Structure + +```xml + + + + + + + + + + + + + + 0.001 + 1.0 + + + + + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + + + + + true + + + 0 0 1 + + + 0 0 1100 100 + + + + + + + + + model://iris_with_ardupilot + iris + 0 0 0.195 0 0 0 + + + + +``` + +## Adding Objects + +### Basic Shapes + +```xml + + true + 5 0 0.5 0 0 0 + + + 1 1 1 + + + 1 1 1 + + 0.8 0.2 0.2 1 + + + + +``` + +### Cylinder + +```xml + + true + 0 5 1 0 0 0 + + + 0.52 + + + 0.52 + + + +``` + +### Include Model + +```xml + + model://my_custom_model + obstacle_1 + 3 4 0 0 0 0.5 + +``` + +## Landing Pad with Rover + +```xml + + false + 0 0 0.05 0 0 0 + + 10 + + 0.750.1 + + + 0.750.1 + 0.2 0.8 0.2 1 + + + + /landing_pad/cmd_vel + + +``` + +## Camera Sensor + +Add to drone or create camera model: + +```xml + + 0 0 -0.1 0 1.5708 0 + true + 30 + + 1.047 + + 640 + 480 + + + /drone/camera + +``` + +## Tips + +- Use `true` for non-moving objects +- Pose format: `x y z roll pitch yaw` +- Angles are in radians +- Colors are RGBA (0-1 range) diff --git a/docs/installation.md b/docs/installation.md index eed61b5..d617ae9 100644 --- a/docs/installation.md +++ b/docs/installation.md @@ -1,110 +1,62 @@ -# Installation Guide +# Installation ## Quick Install ```bash ./setup/install_ubuntu.sh -source activate.sh -python standalone_simulation.py -``` - -## Scripts - -| Platform | Command | -|----------|---------| -| Ubuntu/Debian | `./setup/install_ubuntu.sh` | -| ArduPilot SITL | `./setup/install_ardupilot.sh` | -| macOS | `./setup/install_macos.sh` | -| Windows | `.\setup\install_windows.ps1` | - -## Platform Support - -| Mode | Ubuntu | macOS | Windows | -|------|--------|-------|---------| -| Standalone | ✅ | ✅ | ✅ | -| Gazebo | ✅ | ❌ | WSL2 | -| ArduPilot | ✅ | ❌ | WSL2 | - ---- - -## Ubuntu/Debian - -```bash -./setup/install_ubuntu.sh -source activate.sh -``` - -Installs: ROS 2, Gazebo, PyBullet, OpenCV, pymavlink - ---- - -## ArduPilot SITL - -```bash ./setup/install_ardupilot.sh source ~/.bashrc ``` -Installs: ArduPilot SITL, ardupilot_gazebo, MAVProxy +## What Gets Installed -**Run:** -```bash -./scripts/run_ardupilot_sim.sh camera -``` +| Component | Location | +|-----------|----------| +| ArduPilot SITL | `~/ardupilot` | +| ardupilot_gazebo | `~/ardupilot_gazebo` | +| Gazebo Harmonic | System | +| ROS 2 | System | +| MAVProxy | `~/.local/bin` | ---- - -## GPU Support - -The simulation auto-detects GPU: - -| Priority | GPU Type | Notes | -|----------|----------|-------| -| 1 | NVIDIA | Best performance | -| 2 | Intel integrated | Good for laptops | -| 3 | AMD | Good performance | -| 4 | Software (llvmpipe) | Slow fallback | - -Check your GPU: -```bash -glxinfo | grep "OpenGL renderer" -``` - ---- - -## Manual Install +## Dependencies ```bash -python3 -m venv venv -source venv/bin/activate pip install -r requirements.txt -python standalone_simulation.py ``` ---- +- pybullet +- numpy +- pillow +- opencv-python +- pymavlink +- pexpect + +## Verify Installation + +```bash +sim_vehicle.py --help +gz sim --help +``` ## Troubleshooting -### Simulation is laggy - -```bash -# Check GPU (should NOT show "llvmpipe") -glxinfo | grep "OpenGL renderer" - -# Install GPU drivers -sudo apt install mesa-utils # Intel/AMD -sudo apt install nvidia-driver-535 # NVIDIA -``` - -### MAVProxy not found - -```bash -pip3 install --user mavproxy -export PATH=$PATH:~/.local/bin -``` - ### sim_vehicle.py not found - ```bash export PATH=$PATH:~/ardupilot/Tools/autotest ``` + +### mavproxy.py not found +```bash +export PATH=$PATH:~/.local/bin +``` + +### pexpect error +```bash +pip install pexpect +``` + +### Gazebo slow +```bash +glxinfo | grep "OpenGL renderer" +``` +Should show GPU, not "llvmpipe". diff --git a/docs/protocol.md b/docs/protocol.md deleted file mode 100644 index 7d1c53b..0000000 --- a/docs/protocol.md +++ /dev/null @@ -1,71 +0,0 @@ -# Communication Protocol - -Message formats for drone operation. - -## Commands - -```json -{ - "thrust": 0.5, - "pitch": 0.1, - "roll": -0.2, - "yaw": 0.0 -} -``` - -| Field | Range | Effect | -|-------|-------|--------| -| thrust | ±1.0 | Up/down | -| pitch | ±0.5 | Forward/back | -| roll | ±0.5 | Left/right | -| yaw | ±0.5 | Rotation | - -## Telemetry - -```json -{ - "imu": { - "orientation": {"roll": 0.0, "pitch": 0.0, "yaw": 0.0}, - "angular_velocity": {"x": 0.0, "y": 0.0, "z": 0.0} - }, - "altimeter": { - "altitude": 5.0, - "vertical_velocity": -0.1 - }, - "velocity": {"x": 0.0, "y": 0.0, "z": -0.1}, - "landing_pad": { - "relative_x": 0.5, - "relative_y": -0.2, - "distance": 4.5, - "confidence": 0.85 - }, - "camera": { - "width": 320, - "height": 240, - "image": "" - } -} -``` - -## Sensors - -| Sensor | Fields | -|--------|--------| -| IMU | orientation (roll, pitch, yaw), angular_velocity | -| Altimeter | altitude, vertical_velocity | -| Velocity | x, y, z (m/s) | -| Landing Pad | relative_x, relative_y, distance, confidence | -| Camera | Base64 JPEG image | - -## Decoding Camera - -```python -import base64 -import cv2 -import numpy as np - -image_b64 = telemetry['camera']['image'] -image_bytes = base64.b64decode(image_b64) -nparr = np.frombuffer(image_bytes, np.uint8) -image = cv2.imdecode(nparr, cv2.IMREAD_COLOR) -``` diff --git a/docs/pybullet.md b/docs/pybullet.md deleted file mode 100644 index a63b5ba..0000000 --- a/docs/pybullet.md +++ /dev/null @@ -1,68 +0,0 @@ -# PyBullet Simulation Guide - -## Standalone Mode (1 Terminal) - -No ROS 2 required. Works on Windows, macOS, Linux: - -```bash -source activate.sh -python standalone_simulation.py --pattern circular -``` - -## ROS 2 Mode (2 Terminals) - -**Terminal 1 - Simulator:** -```bash -python simulation_host.py -``` - -**Terminal 2 - Controllers:** -```bash -python run_bridge.py --pattern circular -``` - -## Options - -```bash ---pattern, -p stationary, linear, circular, square, random ---speed, -s Rover speed in m/s (default: 0.5) ---amplitude, -a Movement radius (default: 2.0) -``` - -## Remote Setup - -**Machine 1:** `python simulation_host.py` -**Machine 2:** `python run_bridge.py --host ` - -## Sensors - -| Sensor | Description | -|--------|-------------| -| IMU | Orientation, angular velocity | -| Altimeter | Altitude, vertical velocity | -| Velocity | Estimated velocity (x, y, z) | -| Camera | 320x240 downward JPEG | -| Landing Pad | Relative position | - -## Configuration - -Edit `config.py`: - -```python -CONTROLLER = { - "Kp_z": 0.5, - "Kd_z": 0.3, - "Kp_xy": 0.3, - "Kd_xy": 0.2, -} -``` - -## Troubleshooting - -**"Cannot connect to X server":** -```bash -xvfb-run python standalone_simulation.py -``` - -**Drone flies erratically:** -Reduce gains in `config.py` diff --git a/docs/rover_controller.md b/docs/rover_controller.md deleted file mode 100644 index a14c292..0000000 --- a/docs/rover_controller.md +++ /dev/null @@ -1,61 +0,0 @@ -# Rover Controller - -The RoverController creates a moving landing pad target. - -## Usage - -```bash -# Stationary (default) -python standalone_simulation.py --pattern stationary - -# Moving -python standalone_simulation.py --pattern circular --speed 0.3 -``` - -## Options - -| Option | Default | Description | -|--------|---------|-------------| -| `--pattern, -p` | stationary | Movement pattern | -| `--speed, -s` | 0.5 | Speed in m/s | -| `--amplitude, -a` | 2.0 | Radius in meters | - -## Patterns - -| Pattern | Description | -|---------|-------------| -| stationary | Stays at origin | -| linear | Oscillates along X-axis | -| circular | Circular path | -| square | Square with sharp turns | -| random | Random positions | - -## Difficulty Levels - -| Level | Pattern | Speed | -|-------|---------|-------| -| Beginner | stationary | 0.0 | -| Easy | linear | 0.2 | -| Medium | circular | 0.3 | -| Hard | random | 0.3 | -| Expert | square | 0.5 | - -## Progressive Testing - -```bash -# 1. Static target -python standalone_simulation.py --pattern stationary - -# 2. Slow circular -python standalone_simulation.py --pattern circular --speed 0.2 - -# 3. Faster circular -python standalone_simulation.py --pattern circular --speed 0.4 - -# 4. Random -python standalone_simulation.py --pattern random --speed 0.3 -``` - -## Note - -The drone cannot access rover position directly (GPS-denied). It must detect the landing pad visually via the camera. diff --git a/gazebo/models/custom_object/model.config b/gazebo/models/custom_object/model.config new file mode 100644 index 0000000..37833b9 --- /dev/null +++ b/gazebo/models/custom_object/model.config @@ -0,0 +1,10 @@ + + + Custom Object + 1.0 + model.sdf + + Your Name + + Template for custom Gazebo model + diff --git a/gazebo/models/custom_object/model.sdf b/gazebo/models/custom_object/model.sdf new file mode 100644 index 0000000..5dc3a72 --- /dev/null +++ b/gazebo/models/custom_object/model.sdf @@ -0,0 +1,24 @@ + + + + true + + + + + meshes/model.dae + 1 1 1 + + + + + + + meshes/model.dae + 1 1 1 + + + + + + diff --git a/gazebo/worlds/custom_landing.sdf b/gazebo/worlds/custom_landing.sdf new file mode 100644 index 0000000..dabd585 --- /dev/null +++ b/gazebo/worlds/custom_landing.sdf @@ -0,0 +1,126 @@ + + + + + + 0.001 + 1.0 + 1000 + + + + + ogre2 + + + + + + + 0.4 0.4 0.4 1 + 0.7 0.8 0.9 1 + true + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.3 0.3 0.3 1 + 0.5 0.5 0.5 1 + + + + + + + false + 0 0 0.05 0 0 0 + + + 10 + + + + + 0.75 + 0.1 + + + + + + + 0.75 + 0.1 + + + + 0.1 0.6 0.1 1 + 0.2 0.8 0.2 1 + + + + 0 0 0.06 0 0 0 + + + 0.1 0.5 0.02 + + + + 1 1 1 1 + 1 1 1 1 + + + + 0 0 0.06 0 0 1.5708 + + + 0.1 0.25 0.02 + + + + 1 1 1 1 + 1 1 1 1 + + + + + /landing_pad/cmd_vel + + + /landing_pad/odom + + + + + model://iris_with_ardupilot + iris + 0 0 0.195 0 0 0 + + + + diff --git a/build_exe.py b/legacy/build_exe.py similarity index 100% rename from build_exe.py rename to legacy/build_exe.py diff --git a/controllers.py b/legacy/controllers.py similarity index 100% rename from controllers.py rename to legacy/controllers.py diff --git a/ros_bridge.py b/legacy/ros_bridge.py similarity index 100% rename from ros_bridge.py rename to legacy/ros_bridge.py diff --git a/run_bridge.py b/legacy/run_bridge.py similarity index 100% rename from run_bridge.py rename to legacy/run_bridge.py diff --git a/run_gazebo.py b/legacy/run_gazebo.py similarity index 100% rename from run_gazebo.py rename to legacy/run_gazebo.py diff --git a/simulation_host.py b/legacy/simulation_host.py similarity index 100% rename from simulation_host.py rename to legacy/simulation_host.py diff --git a/standalone_simulation.py b/legacy/standalone_simulation.py similarity index 100% rename from standalone_simulation.py rename to legacy/standalone_simulation.py diff --git a/requirements.txt b/requirements.txt index 62b4dd1..28fa92c 100644 --- a/requirements.txt +++ b/requirements.txt @@ -1,16 +1,7 @@ -# Drone Simulation Requirements -# Install with: pip install -r requirements.txt - -# Core simulation pybullet>=3.2.5 numpy>=1.20.0 pillow>=9.0.0 - -# Camera viewer opencv-python>=4.5.0 - -# ArduPilot/MAVLink (optional - for ArduPilot mode) pymavlink>=2.4.0 - -# Build executables (optional) +pexpect>=4.8.0 pyinstaller>=6.0.0 diff --git a/run_ardupilot.py b/scripts/run_ardupilot.py similarity index 98% rename from run_ardupilot.py rename to scripts/run_ardupilot.py index 5e7bf58..79aef12 100644 --- a/run_ardupilot.py +++ b/scripts/run_ardupilot.py @@ -15,8 +15,11 @@ This script: 3. Runs your drone_controller.calculate_landing_maneuver() 4. Sends velocity commands to ArduPilot """ - import sys +import os + +sys.path.insert(0, os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) + import time import math import argparse @@ -29,7 +32,7 @@ except ImportError: sys.exit(1) from config import ARDUPILOT, CONTROLLER, MAVLINK -from drone_controller import DroneController +from src.drone_controller import DroneController class ArduPilotInterface: diff --git a/scripts/run_ardupilot_sim.sh b/scripts/run_ardupilot_sim.sh index b0dbb5d..8d096aa 100755 --- a/scripts/run_ardupilot_sim.sh +++ b/scripts/run_ardupilot_sim.sh @@ -113,7 +113,6 @@ fi # ============================================================================= WORLD_ARG="${1:-runway}" -# Map friendly names to actual world files case "$WORLD_ARG" in runway|iris|default) WORLD="${HOME}/ardupilot_gazebo/worlds/iris_runway.sdf" @@ -130,10 +129,16 @@ case "$WORLD_ARG" in parachute) WORLD="${HOME}/ardupilot_gazebo/worlds/zephyr_parachute.sdf" ;; + custom) + WORLD="${PROJECT_DIR}/gazebo/worlds/custom_landing.sdf" + ;; *) - # Try as full path or filename 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 @@ -147,12 +152,13 @@ esac if [ -z "$WORLD" ] || [ ! -f "$WORLD" ]; then echo "[ERROR] World not found: $WORLD_ARG" echo "" - echo "Available options:" - echo " runway - Iris drone on runway (default)" + echo "Built-in worlds:" + echo " runway - Iris on runway (default)" echo " warehouse - Iris in warehouse" - echo " gimbal - Gimbal test" - echo " zephyr - Zephyr plane" - echo " parachute - Parachute test" + 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 diff --git a/setup/install_ardupilot.sh b/setup/install_ardupilot.sh index 6e9ae2b..476bba5 100755 --- a/setup/install_ardupilot.sh +++ b/setup/install_ardupilot.sh @@ -131,8 +131,8 @@ else echo "[OK] Environment already configured" fi -# Install MAVProxy -pip3 install --user pymavlink mavproxy +# Install MAVProxy and Python dependencies +pip3 install --user pymavlink mavproxy pexpect # ----------------------------------------------------------------------------- # Verification diff --git a/setup/install_ubuntu.sh b/setup/install_ubuntu.sh index ef2b59c..ac4df57 100755 --- a/setup/install_ubuntu.sh +++ b/setup/install_ubuntu.sh @@ -151,7 +151,7 @@ pip install --upgrade pip if [ -f "$PROJECT_ROOT/requirements.txt" ]; then pip install -r "$PROJECT_ROOT/requirements.txt" else - pip install pybullet numpy pillow opencv-python pymavlink + pip install pybullet numpy pillow opencv-python pymavlink pexpect fi echo "[INFO] Python packages installed" diff --git a/src/__init__.py b/src/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/camera_viewer.py b/src/camera_viewer.py similarity index 100% rename from camera_viewer.py rename to src/camera_viewer.py diff --git a/drone_controller.py b/src/drone_controller.py similarity index 55% rename from drone_controller.py rename to src/drone_controller.py index 1b218ac..21a1aa0 100644 --- a/drone_controller.py +++ b/src/drone_controller.py @@ -1,27 +1,26 @@ #!/usr/bin/env python3 """ -DroneController - Template for GPS-denied landing logic. +DroneController - GPS-denied landing with 3-phase state machine. -Supports multiple telemetry sources: - 1. Custom JSON telemetry (/drone/telemetry) - PyBullet/Gazebo bridge - 2. ArduPilot ROS 2 DDS topics (/ap/*) - Official ArduPilot integration - -Implement your algorithm in calculate_landing_maneuver(). -Uses sensors: IMU, altimeter, camera, and landing pad detection. +Phases: + 1. SEARCH - Find QR code on rover using camera + 2. COMMAND - Send commands to rover + 3. LAND - Land on the rover """ import json import math +import base64 +from enum import Enum, auto from typing import Dict, Any, Optional import rclpy from rclpy.node import Node from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy from geometry_msgs.msg import Twist, PoseStamped, TwistStamped -from sensor_msgs.msg import Imu, BatteryState, NavSatFix +from sensor_msgs.msg import Imu, BatteryState, NavSatFix, Image from std_msgs.msg import String -# Load configuration try: from config import CONTROLLER, DRONE, LANDING CONFIG_LOADED = True @@ -31,22 +30,32 @@ except ImportError: DRONE = {"max_thrust": 1.0, "max_pitch": 0.5, "max_roll": 0.5} LANDING = {"height_threshold": 0.1, "success_velocity": 0.1} +try: + import cv2 + import numpy as np + CV2_AVAILABLE = True +except ImportError: + CV2_AVAILABLE = False + + +class Phase(Enum): + SEARCH = auto() + COMMAND = auto() + LAND = auto() + COMPLETE = auto() + def quaternion_to_euler(x: float, y: float, z: float, w: float) -> tuple: - """Convert quaternion to Euler angles (roll, pitch, yaw).""" - # Roll (x-axis rotation) 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) - # Pitch (y-axis rotation) sinp = 2.0 * (w * y - z * x) if abs(sinp) >= 1: pitch = math.copysign(math.pi / 2, sinp) else: pitch = math.asin(sinp) - # Yaw (z-axis rotation) 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) @@ -55,18 +64,9 @@ def quaternion_to_euler(x: float, y: float, z: float, w: float) -> tuple: class DroneController(Node): - """ - Drone controller for GPS-denied landing. - - Supports multiple telemetry sources: - - Legacy: /drone/telemetry (JSON String) - - ArduPilot DDS: /ap/pose/filtered, /ap/imu/filtered, etc. - """ - def __init__(self, use_ardupilot_topics: bool = True): super().__init__('drone_controller') - # Load from config 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) @@ -78,29 +78,37 @@ class DroneController(Node): self._landing_height = LANDING.get("height_threshold", 0.1) self._landing_velocity = LANDING.get("success_velocity", 0.1) - # Mode selection self._use_ardupilot = use_ardupilot_topics - self.get_logger().info('=' * 50) - self.get_logger().info('Drone Controller Starting (GPS-Denied)...') - if CONFIG_LOADED: - self.get_logger().info(' Configuration loaded from config.py') - self.get_logger().info(f' Mode: {"ArduPilot DDS" if use_ardupilot_topics else "Legacy JSON"}') - self.get_logger().info('=' * 50) + 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 - # State variables - self._latest_telemetry: Optional[Dict[str, Any]] = None - self._rover_telemetry: Optional[Dict[str, Any]] = None - self._telemetry_received = False self._landing_complete = False - # ArduPilot state (built from DDS topics) + 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 - # QoS for sensor topics + 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, @@ -112,59 +120,33 @@ class DroneController(Node): else: self._setup_legacy_subscriptions() - # Always subscribe to rover telemetry (for moving landing pad) self._rover_telemetry_sub = self.create_subscription( - String, '/rover/telemetry', self._rover_telemetry_callback, 10 - ) - self.get_logger().info(' Subscribed to: /rover/telemetry') + String, '/rover/telemetry', self._rover_telemetry_callback, 10) + + self._camera_sub = self.create_subscription( + Image, '/drone/camera', self._camera_callback, sensor_qos) - # Command publisher self._cmd_vel_pub = self.create_publisher(Twist, '/cmd_vel', 10) - self.get_logger().info(' Publishing to: /cmd_vel') + self._rover_cmd_pub = self.create_publisher(String, '/rover/command', 10) - # Control loop - control_period = 1.0 / self._control_rate - self._control_timer = self.create_timer(control_period, self._control_loop) - self.get_logger().info(f' Control loop: {self._control_rate} Hz') + self._control_timer = self.create_timer(1.0 / self._control_rate, self._control_loop) - self.get_logger().info('Drone Controller Ready!') - self.get_logger().info('Sensors: IMU, Altimeter, Velocity, Landing Pad Detection') + self.get_logger().info(f'Current Phase: {self._phase.name}') def _setup_ardupilot_subscriptions(self, qos: QoSProfile): - """Set up subscriptions to official ArduPilot ROS 2 topics.""" - - # Pose (position + orientation) self._ap_pose_sub = self.create_subscription( - PoseStamped, '/ap/pose/filtered', self._ap_pose_callback, qos - ) - self.get_logger().info(' Subscribed to: /ap/pose/filtered') - - # Twist (velocity) + 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.get_logger().info(' Subscribed to: /ap/twist/filtered') - - # IMU + 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.get_logger().info(' Subscribed to: /ap/imu/filtered') - - # Battery + Imu, '/ap/imu/filtered', self._ap_imu_callback, qos) self._ap_battery_sub = self.create_subscription( - BatteryState, '/ap/battery', self._ap_battery_callback, qos - ) - self.get_logger().info(' Subscribed to: /ap/battery') + BatteryState, '/ap/battery', self._ap_battery_callback, qos) def _setup_legacy_subscriptions(self): - """Set up subscriptions to legacy JSON telemetry.""" self._telemetry_sub = self.create_subscription( - String, '/drone/telemetry', self._telemetry_callback, 10 - ) - self.get_logger().info(' Subscribed to: /drone/telemetry') + String, '/drone/telemetry', self._telemetry_callback, 10) - # ArduPilot topic callbacks def _ap_pose_callback(self, msg: PoseStamped): self._ap_pose = msg if not self._telemetry_received: @@ -185,82 +167,6 @@ class DroneController(Node): self._ap_battery = msg self._build_telemetry_from_ardupilot() - def _build_telemetry_from_ardupilot(self): - """Build telemetry dict from ArduPilot topics.""" - telemetry = {} - - # Position and orientation from pose - 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} - } - - # Velocity from twist - 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 - - # Angular velocity from IMU - 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 - } - - # Battery - if self._ap_battery: - telemetry['battery'] = { - 'voltage': self._ap_battery.voltage, - 'remaining': self._ap_battery.percentage * 100 - } - - # Landing pad detection (placeholder - would need vision processing) - # For now, calculate from rover telemetry if available - if self._rover_telemetry and self._ap_pose: - rover_pos = self._rover_telemetry.get('position', {}) - rx = rover_pos.get('x', 0) - ry = 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 = rx - dx - rel_y = 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 - - # Legacy callbacks def _telemetry_callback(self, msg: String) -> None: try: self._latest_telemetry = json.loads(msg.data) @@ -276,33 +182,97 @@ class DroneController(Node): self._rover_telemetry = json.loads(msg.data) except json.JSONDecodeError: pass - + + def _camera_callback(self, msg: Image) -> None: + try: + self._latest_camera_image = bytes(msg.data) + self._camera_width = msg.width + self._camera_height = msg.height + self._camera_encoding = msg.encoding + except Exception as e: + self.get_logger().warning(f'Camera callback error: {e}') + + def _build_telemetry_from_ardupilot(self): + telemetry = {} + + if self._ap_pose: + pos = self._ap_pose.pose.position + ori = self._ap_pose.pose.orientation + roll, pitch, yaw = quaternion_to_euler(ori.x, ori.y, ori.z, ori.w) + + telemetry['position'] = {'x': pos.x, 'y': pos.y, 'z': pos.z} + telemetry['altimeter'] = { + 'altitude': pos.z, + 'vertical_velocity': self._ap_twist.twist.linear.z if self._ap_twist else 0.0 + } + telemetry['imu'] = { + 'orientation': {'roll': roll, 'pitch': pitch, 'yaw': yaw}, + 'angular_velocity': {'x': 0, 'y': 0, 'z': 0} + } + + if self._ap_twist: + twist = self._ap_twist.twist + telemetry['velocity'] = { + 'x': twist.linear.x, 'y': twist.linear.y, 'z': twist.linear.z + } + if 'altimeter' in telemetry: + telemetry['altimeter']['vertical_velocity'] = twist.linear.z + + if self._ap_imu: + if 'imu' not in telemetry: + telemetry['imu'] = {} + telemetry['imu']['angular_velocity'] = { + 'x': self._ap_imu.angular_velocity.x, + 'y': self._ap_imu.angular_velocity.y, + 'z': self._ap_imu.angular_velocity.z + } + + if self._ap_battery: + telemetry['battery'] = { + 'voltage': self._ap_battery.voltage, + 'remaining': self._ap_battery.percentage * 100 + } + + if self._qr_position: + telemetry['landing_pad'] = self._qr_position + elif self._rover_telemetry and self._ap_pose: + rover_pos = self._rover_telemetry.get('position', {}) + rx, ry = rover_pos.get('x', 0), rover_pos.get('y', 0) + dx = self._ap_pose.pose.position.x + dy = self._ap_pose.pose.position.y + dz = self._ap_pose.pose.position.z + + rel_x, rel_y = rx - dx, ry - dy + distance = math.sqrt(rel_x**2 + rel_y**2 + dz**2) + + telemetry['landing_pad'] = { + 'relative_x': rel_x, 'relative_y': rel_y, + 'distance': distance, 'confidence': 1.0 if distance < 10.0 else 0.0 + } + + self._latest_telemetry = telemetry + def _control_loop(self) -> None: if self._latest_telemetry is None: return - if self._landing_complete: + if self._phase == Phase.COMPLETE: return - # Warmup period - wait for stable telemetry before checking landing if not hasattr(self, '_warmup_count'): self._warmup_count = 0 self._warmup_count += 1 - if self._warmup_count < 100: # Wait ~2 seconds at 50Hz - # Still fly with controller, just don't check for landing yet - pass - elif self._check_landing_complete(): - self._landing_complete = True - self.get_logger().info('=' * 50) - self.get_logger().info('LANDING COMPLETE!') - self.get_logger().info('=' * 50) - self._publish_command(0.0, 0.0, 0.0, 0.0) + if self._warmup_count < 50: return - thrust, pitch, roll, yaw = self.calculate_landing_maneuver( - self._latest_telemetry, - self._rover_telemetry - ) + 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) @@ -310,48 +280,140 @@ class DroneController(Node): yaw = max(min(yaw, 0.5), -0.5) self._publish_command(thrust, pitch, roll, yaw) - - def _check_landing_complete(self) -> bool: - if self._latest_telemetry is None: - return False + + 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: - 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 - + 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[float, float, float, float]: - """ - Calculate control commands for GPS-denied landing. - - Available sensors in telemetry: - - imu.orientation: {roll, pitch, yaw} in radians - - imu.angular_velocity: {x, y, z} in rad/s - - altimeter.altitude: height in meters - - altimeter.vertical_velocity: vertical speed in m/s - - velocity: {x, y, z} estimated velocity in m/s - - landing_pad: relative position to pad (may be None) - - relative_x, relative_y: offset in body frame - - distance: vertical distance to pad - - confidence: detection confidence (0-1) - - camera: {width, height, fov, image} - - image: base64 encoded JPEG (or None) - - landed: bool - - Args: - telemetry: Sensor data from drone - rover_telemetry: Rover state (for moving target) - may be None - - Returns: - Tuple of (thrust, pitch, roll, yaw) control values (-1.0 to 1.0) - """ - # Extract sensor data + ) -> tuple: altimeter = telemetry.get('altimeter', {}) altitude = altimeter.get('altitude', 5.0) vertical_vel = altimeter.get('vertical_velocity', 0.0) @@ -361,11 +423,7 @@ class DroneController(Node): vel_y = velocity.get('y', 0.0) landing_pad = telemetry.get('landing_pad', None) - - # camera = telemetry.get('camera', None) - - # Descent control if altitude > 1.0: target_descent_rate = -0.5 else: @@ -374,7 +432,6 @@ class DroneController(Node): 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) @@ -388,6 +445,23 @@ class DroneController(Node): yaw = 0.0 return (thrust, pitch, roll, yaw) + + def _transition_to_phase(self, new_phase: Phase) -> None: + old_phase = self._phase + self._phase = new_phase + self._phase_start_time = self.get_clock().now() + self.get_logger().info(f'Phase: {old_phase.name} -> {new_phase.name}') + + def _check_landing_complete(self) -> bool: + if self._latest_telemetry is None: + return False + try: + altimeter = self._latest_telemetry.get('altimeter', {}) + altitude = altimeter.get('altitude', float('inf')) + vertical_velocity = abs(altimeter.get('vertical_velocity', float('inf'))) + return altitude < self._landing_height and vertical_velocity < self._landing_velocity + except (KeyError, TypeError): + return False def _publish_command(self, thrust: float, pitch: float, roll: float, yaw: float) -> None: msg = Twist() @@ -396,15 +470,15 @@ class DroneController(Node): msg.linear.y = roll msg.angular.z = yaw self._cmd_vel_pub.publish(msg) + + def get_current_phase(self) -> Phase: + return self._phase def main(args=None): import sys - # Check for --ardupilot flag use_ardupilot = '--ardupilot' in sys.argv or '-a' in sys.argv - - # Remove our custom args before passing to rclpy filtered_args = [a for a in (args or sys.argv) if a not in ['--ardupilot', '-a']] rclpy.init(args=filtered_args) diff --git a/gazebo_bridge.py b/src/gazebo_bridge.py similarity index 100% rename from gazebo_bridge.py rename to src/gazebo_bridge.py diff --git a/mavlink_bridge.py b/src/mavlink_bridge.py similarity index 100% rename from mavlink_bridge.py rename to src/mavlink_bridge.py diff --git a/rover_controller.py b/src/rover_controller.py similarity index 100% rename from rover_controller.py rename to src/rover_controller.py