Code reorganization and Drone Logic Update
This commit is contained in:
85
README.md
85
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)
|
||||
- [Drone Controller](docs/drone_guide.md)
|
||||
- [Custom Worlds](docs/gazebo_worlds.md)
|
||||
- [Blender Models](docs/blender_models.md)
|
||||
- [Architecture](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
|
||||
```
|
||||
|
||||
@@ -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"`
|
||||
|
||||
168
docs/blender_models.md
Normal file
168
docs/blender_models.md
Normal file
@@ -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
|
||||
<?xml version="1.0"?>
|
||||
<model>
|
||||
<name>My Model</name>
|
||||
<version>1.0</version>
|
||||
<sdf version="1.9">model.sdf</sdf>
|
||||
<description>Custom Blender model</description>
|
||||
</model>
|
||||
```
|
||||
|
||||
### model.sdf
|
||||
|
||||
```xml
|
||||
<?xml version="1.0"?>
|
||||
<sdf version="1.9">
|
||||
<model name="my_model">
|
||||
<static>true</static>
|
||||
<link name="link">
|
||||
<collision name="collision">
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>meshes/model.dae</uri>
|
||||
<scale>1 1 1</scale>
|
||||
</mesh>
|
||||
</geometry>
|
||||
</collision>
|
||||
<visual name="visual">
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>meshes/model.dae</uri>
|
||||
<scale>1 1 1</scale>
|
||||
</mesh>
|
||||
</geometry>
|
||||
</visual>
|
||||
</link>
|
||||
</model>
|
||||
</sdf>
|
||||
```
|
||||
|
||||
## Step 4: Add to World
|
||||
|
||||
```xml
|
||||
<include>
|
||||
<uri>model://my_model</uri>
|
||||
<name>my_model_instance</name>
|
||||
<pose>5 3 0 0 0 0</pose>
|
||||
</include>
|
||||
```
|
||||
|
||||
## 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
|
||||
<scale>0.01 0.01 0.01</scale>
|
||||
```
|
||||
|
||||
### Textures Not Showing
|
||||
Put textures in `materials/textures/` and reference in DAE file.
|
||||
|
||||
Or add material in SDF:
|
||||
```xml
|
||||
<visual name="visual">
|
||||
<geometry>
|
||||
<mesh><uri>meshes/model.dae</uri></mesh>
|
||||
</geometry>
|
||||
<material>
|
||||
<diffuse>0.8 0.2 0.2 1</diffuse>
|
||||
</material>
|
||||
</visual>
|
||||
```
|
||||
|
||||
### Model Orientation Wrong
|
||||
Blender uses Z-up, Gazebo uses Z-up. Should match.
|
||||
|
||||
If rotated, fix in Blender or use pose:
|
||||
```xml
|
||||
<pose>0 0 0 1.5708 0 0</pose>
|
||||
```
|
||||
|
||||
## Simplified Collision
|
||||
|
||||
For complex meshes, use simple collision:
|
||||
|
||||
```xml
|
||||
<collision name="collision">
|
||||
<geometry>
|
||||
<box><size>2 2 3</size></box>
|
||||
</geometry>
|
||||
</collision>
|
||||
<visual name="visual">
|
||||
<geometry>
|
||||
<mesh><uri>meshes/complex_model.dae</uri></mesh>
|
||||
</geometry>
|
||||
</visual>
|
||||
```
|
||||
|
||||
## 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
|
||||
```
|
||||
@@ -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
|
||||
```
|
||||
|
||||
@@ -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
|
||||
```
|
||||
167
docs/gazebo_worlds.md
Normal file
167
docs/gazebo_worlds.md
Normal file
@@ -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
|
||||
<?xml version="1.0" ?>
|
||||
<sdf version="1.9">
|
||||
<world name="my_world">
|
||||
|
||||
<!-- Required plugins -->
|
||||
<plugin filename="gz-sim-physics-system" name="gz::sim::systems::Physics"/>
|
||||
<plugin filename="gz-sim-sensors-system" name="gz::sim::systems::Sensors"/>
|
||||
<plugin filename="gz-sim-user-commands-system" name="gz::sim::systems::UserCommands"/>
|
||||
<plugin filename="gz-sim-scene-broadcaster-system" name="gz::sim::systems::SceneBroadcaster"/>
|
||||
<plugin filename="gz-sim-imu-system" name="gz::sim::systems::Imu"/>
|
||||
|
||||
<!-- Physics -->
|
||||
<physics name="1ms" type="ode">
|
||||
<max_step_size>0.001</max_step_size>
|
||||
<real_time_factor>1.0</real_time_factor>
|
||||
</physics>
|
||||
|
||||
<!-- Lighting -->
|
||||
<light type="directional" name="sun">
|
||||
<pose>0 0 10 0 0 0</pose>
|
||||
<diffuse>0.8 0.8 0.8 1</diffuse>
|
||||
</light>
|
||||
|
||||
<!-- Ground -->
|
||||
<model name="ground">
|
||||
<static>true</static>
|
||||
<link name="link">
|
||||
<collision name="collision">
|
||||
<geometry><plane><normal>0 0 1</normal></plane></geometry>
|
||||
</collision>
|
||||
<visual name="visual">
|
||||
<geometry><plane><normal>0 0 1</normal><size>100 100</size></plane></geometry>
|
||||
</visual>
|
||||
</link>
|
||||
</model>
|
||||
|
||||
<!-- Your models here -->
|
||||
|
||||
<!-- ArduPilot drone (required) -->
|
||||
<include>
|
||||
<uri>model://iris_with_ardupilot</uri>
|
||||
<name>iris</name>
|
||||
<pose>0 0 0.195 0 0 0</pose>
|
||||
</include>
|
||||
|
||||
</world>
|
||||
</sdf>
|
||||
```
|
||||
|
||||
## Adding Objects
|
||||
|
||||
### Basic Shapes
|
||||
|
||||
```xml
|
||||
<model name="box">
|
||||
<static>true</static>
|
||||
<pose>5 0 0.5 0 0 0</pose>
|
||||
<link name="link">
|
||||
<collision name="collision">
|
||||
<geometry><box><size>1 1 1</size></box></geometry>
|
||||
</collision>
|
||||
<visual name="visual">
|
||||
<geometry><box><size>1 1 1</size></box></geometry>
|
||||
<material>
|
||||
<ambient>0.8 0.2 0.2 1</ambient>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
</model>
|
||||
```
|
||||
|
||||
### Cylinder
|
||||
|
||||
```xml
|
||||
<model name="cylinder">
|
||||
<static>true</static>
|
||||
<pose>0 5 1 0 0 0</pose>
|
||||
<link name="link">
|
||||
<collision name="collision">
|
||||
<geometry><cylinder><radius>0.5</radius><length>2</length></cylinder></geometry>
|
||||
</collision>
|
||||
<visual name="visual">
|
||||
<geometry><cylinder><radius>0.5</radius><length>2</length></cylinder></geometry>
|
||||
</visual>
|
||||
</link>
|
||||
</model>
|
||||
```
|
||||
|
||||
### Include Model
|
||||
|
||||
```xml
|
||||
<include>
|
||||
<uri>model://my_custom_model</uri>
|
||||
<name>obstacle_1</name>
|
||||
<pose>3 4 0 0 0 0.5</pose>
|
||||
</include>
|
||||
```
|
||||
|
||||
## Landing Pad with Rover
|
||||
|
||||
```xml
|
||||
<model name="landing_pad">
|
||||
<static>false</static>
|
||||
<pose>0 0 0.05 0 0 0</pose>
|
||||
<link name="base">
|
||||
<inertial><mass>10</mass></inertial>
|
||||
<collision name="collision">
|
||||
<geometry><cylinder><radius>0.75</radius><length>0.1</length></cylinder></geometry>
|
||||
</collision>
|
||||
<visual name="visual">
|
||||
<geometry><cylinder><radius>0.75</radius><length>0.1</length></cylinder></geometry>
|
||||
<material><diffuse>0.2 0.8 0.2 1</diffuse></material>
|
||||
</visual>
|
||||
</link>
|
||||
<plugin filename="gz-sim-velocity-control-system" name="gz::sim::systems::VelocityControl">
|
||||
<topic>/landing_pad/cmd_vel</topic>
|
||||
</plugin>
|
||||
</model>
|
||||
```
|
||||
|
||||
## Camera Sensor
|
||||
|
||||
Add to drone or create camera model:
|
||||
|
||||
```xml
|
||||
<sensor name="camera" type="camera">
|
||||
<pose>0 0 -0.1 0 1.5708 0</pose>
|
||||
<always_on>true</always_on>
|
||||
<update_rate>30</update_rate>
|
||||
<camera>
|
||||
<horizontal_fov>1.047</horizontal_fov>
|
||||
<image>
|
||||
<width>640</width>
|
||||
<height>480</height>
|
||||
</image>
|
||||
</camera>
|
||||
<topic>/drone/camera</topic>
|
||||
</sensor>
|
||||
```
|
||||
|
||||
## Tips
|
||||
|
||||
- Use `<static>true</static>` for non-moving objects
|
||||
- Pose format: `x y z roll pitch yaw`
|
||||
- Angles are in radians
|
||||
- Colors are RGBA (0-1 range)
|
||||
@@ -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".
|
||||
|
||||
@@ -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": "<base64 JPEG>"
|
||||
}
|
||||
}
|
||||
```
|
||||
|
||||
## 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)
|
||||
```
|
||||
@@ -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 <IP>`
|
||||
|
||||
## 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`
|
||||
@@ -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.
|
||||
10
gazebo/models/custom_object/model.config
Normal file
10
gazebo/models/custom_object/model.config
Normal file
@@ -0,0 +1,10 @@
|
||||
<?xml version="1.0"?>
|
||||
<model>
|
||||
<name>Custom Object</name>
|
||||
<version>1.0</version>
|
||||
<sdf version="1.9">model.sdf</sdf>
|
||||
<author>
|
||||
<name>Your Name</name>
|
||||
</author>
|
||||
<description>Template for custom Gazebo model</description>
|
||||
</model>
|
||||
24
gazebo/models/custom_object/model.sdf
Normal file
24
gazebo/models/custom_object/model.sdf
Normal file
@@ -0,0 +1,24 @@
|
||||
<?xml version="1.0"?>
|
||||
<sdf version="1.9">
|
||||
<model name="custom_object">
|
||||
<static>true</static>
|
||||
<link name="link">
|
||||
<collision name="collision">
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>meshes/model.dae</uri>
|
||||
<scale>1 1 1</scale>
|
||||
</mesh>
|
||||
</geometry>
|
||||
</collision>
|
||||
<visual name="visual">
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>meshes/model.dae</uri>
|
||||
<scale>1 1 1</scale>
|
||||
</mesh>
|
||||
</geometry>
|
||||
</visual>
|
||||
</link>
|
||||
</model>
|
||||
</sdf>
|
||||
126
gazebo/worlds/custom_landing.sdf
Normal file
126
gazebo/worlds/custom_landing.sdf
Normal file
@@ -0,0 +1,126 @@
|
||||
<?xml version="1.0" ?>
|
||||
<sdf version="1.9">
|
||||
<world name="custom_landing">
|
||||
|
||||
<physics name="1ms" type="ode">
|
||||
<max_step_size>0.001</max_step_size>
|
||||
<real_time_factor>1.0</real_time_factor>
|
||||
<real_time_update_rate>1000</real_time_update_rate>
|
||||
</physics>
|
||||
|
||||
<plugin filename="gz-sim-physics-system" name="gz::sim::systems::Physics"/>
|
||||
<plugin filename="gz-sim-sensors-system" name="gz::sim::systems::Sensors">
|
||||
<render_engine>ogre2</render_engine>
|
||||
</plugin>
|
||||
<plugin filename="gz-sim-user-commands-system" name="gz::sim::systems::UserCommands"/>
|
||||
<plugin filename="gz-sim-scene-broadcaster-system" name="gz::sim::systems::SceneBroadcaster"/>
|
||||
<plugin filename="gz-sim-imu-system" name="gz::sim::systems::Imu"/>
|
||||
|
||||
<scene>
|
||||
<ambient>0.4 0.4 0.4 1</ambient>
|
||||
<background>0.7 0.8 0.9 1</background>
|
||||
<shadows>true</shadows>
|
||||
</scene>
|
||||
|
||||
<light type="directional" name="sun">
|
||||
<cast_shadows>true</cast_shadows>
|
||||
<pose>0 0 10 0 0 0</pose>
|
||||
<diffuse>0.8 0.8 0.8 1</diffuse>
|
||||
<specular>0.2 0.2 0.2 1</specular>
|
||||
<direction>-0.5 0.1 -0.9</direction>
|
||||
</light>
|
||||
|
||||
<model name="ground_plane">
|
||||
<static>true</static>
|
||||
<link name="link">
|
||||
<collision name="collision">
|
||||
<geometry>
|
||||
<plane>
|
||||
<normal>0 0 1</normal>
|
||||
<size>100 100</size>
|
||||
</plane>
|
||||
</geometry>
|
||||
</collision>
|
||||
<visual name="visual">
|
||||
<geometry>
|
||||
<plane>
|
||||
<normal>0 0 1</normal>
|
||||
<size>100 100</size>
|
||||
</plane>
|
||||
</geometry>
|
||||
<material>
|
||||
<ambient>0.3 0.3 0.3 1</ambient>
|
||||
<diffuse>0.5 0.5 0.5 1</diffuse>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
</model>
|
||||
|
||||
<model name="landing_pad">
|
||||
<static>false</static>
|
||||
<pose>0 0 0.05 0 0 0</pose>
|
||||
<link name="base">
|
||||
<inertial>
|
||||
<mass>10</mass>
|
||||
</inertial>
|
||||
<collision name="collision">
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<radius>0.75</radius>
|
||||
<length>0.1</length>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
</collision>
|
||||
<visual name="pad">
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<radius>0.75</radius>
|
||||
<length>0.1</length>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
<material>
|
||||
<ambient>0.1 0.6 0.1 1</ambient>
|
||||
<diffuse>0.2 0.8 0.2 1</diffuse>
|
||||
</material>
|
||||
</visual>
|
||||
<visual name="h_bar">
|
||||
<pose>0 0 0.06 0 0 0</pose>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.1 0.5 0.02</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<material>
|
||||
<ambient>1 1 1 1</ambient>
|
||||
<diffuse>1 1 1 1</diffuse>
|
||||
</material>
|
||||
</visual>
|
||||
<visual name="h_cross">
|
||||
<pose>0 0 0.06 0 0 1.5708</pose>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.1 0.25 0.02</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<material>
|
||||
<ambient>1 1 1 1</ambient>
|
||||
<diffuse>1 1 1 1</diffuse>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
<plugin filename="gz-sim-velocity-control-system" name="gz::sim::systems::VelocityControl">
|
||||
<topic>/landing_pad/cmd_vel</topic>
|
||||
</plugin>
|
||||
<plugin filename="gz-sim-odometry-publisher-system" name="gz::sim::systems::OdometryPublisher">
|
||||
<odom_topic>/landing_pad/odom</odom_topic>
|
||||
</plugin>
|
||||
</model>
|
||||
|
||||
<include>
|
||||
<uri>model://iris_with_ardupilot</uri>
|
||||
<name>iris</name>
|
||||
<pose>0 0 0.195 0 0 0</pose>
|
||||
</include>
|
||||
|
||||
</world>
|
||||
</sdf>
|
||||
@@ -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
|
||||
|
||||
@@ -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:
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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"
|
||||
|
||||
0
src/__init__.py
Normal file
0
src/__init__.py
Normal file
@@ -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)
|
||||
Reference in New Issue
Block a user