SirBlob/RDC_Simulation
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Initial Attempt

Browse Source
This commit is contained in:
default
2025-12-31 23:50:26 +00:00
commit f489bfbad9
25 changed files with 4179 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
.gitignore vendored Normal file
View File

@@ -0,0 +1,4 @@
venv/
__pycache__/

96
README.md Normal file
View File

@@ -0,0 +1,96 @@
# Drone Landing Simulation (GPS-Denied)
A GPS-denied drone landing simulation using relative sensors (IMU, altimeter, camera, landing pad detection) with **PyBullet** and **Gazebo** simulators.
## Quick Start
```bash
# Install (Ubuntu)
./setup/install_ubuntu.sh
source activate.sh
# Run PyBullet simulation
python simulation_host.py # Terminal 1: Simulator
python ros_bridge.py # Terminal 2: ROS bridge
python controllers.py # Terminal 3: Drone + Rover controllers
# With moving rover
python controllers.py --pattern circular --speed 0.3
```
## Architecture
```
┌─────────────────────────────────────────────────────────────────────────┐
│ ┌──────────────────┐ ┌──────────────────────────┐ │
│ │ simulation_host │◄── UDP:5555 ──────►│ ros_bridge.py │ │
│ │ (PyBullet) │ └────────────┬─────────────┘ │
│ └──────────────────┘ │ │
│ OR │ │
│ ┌──────────────────┐ ┌────────────┴─────────────┐ │
│ │ Gazebo │◄── ROS Topics ────►│ gazebo_bridge.py │ │
│ └──────────────────┘ └────────────┬─────────────┘ │
│ │ │
│ ┌────────────▼─────────────┐ │
│ │ controllers.py │ │
│ │ ┌─────────────────────┐ │ │
│ │ │ DroneController │ │ │
│ │ │ RoverController │ │ │
│ │ └─────────────────────┘ │ │
│ └──────────────────────────┘ │
└─────────────────────────────────────────────────────────────────────────┘
```
## Files
| File | Description |
|------|-------------|
| `simulation_host.py` | PyBullet physics simulator |
| `ros_bridge.py` | UDP ↔ ROS 2 bridge |
| `gazebo_bridge.py` | Gazebo ↔ ROS 2 bridge |
| `controllers.py` | **Runs drone + rover together** |
| `drone_controller.py` | Drone landing logic (edit this) |
| `rover_controller.py` | Moving landing pad |
## Controller Options
```bash
python controllers.py --help
Options:
--pattern, -p Rover pattern: stationary, linear, circular, random, square
--speed, -s Rover speed in m/s (default: 0.5)
--amplitude, -a Rover amplitude in meters (default: 2.0)
```
## GPS-Denied Sensors
The drone has no GPS. Available sensors:
| Sensor | Data |
|--------|------|
| **IMU** | Orientation, angular velocity |
| **Altimeter** | Altitude, vertical velocity |
| **Velocity** | Estimated horizontal velocity |
| **Camera** | 320x240 downward-facing image (base64 JPEG) |
| **Landing Pad** | Relative position when visible (may be null) |
## Documentation
| Document | Description |
|----------|-------------|
| [Installation](docs/installation.md) | Setup for Ubuntu, macOS, Windows |
| [Architecture](docs/architecture.md) | System components and data flow |
| [Protocol](docs/protocol.md) | Sensor data formats |
| [Drone Guide](docs/drone_guide.md) | How to implement landing logic |
| [Rover Controller](docs/rover_controller.md) | Movement patterns |
| [PyBullet](docs/pybullet.md) | PyBullet-specific setup |
| [Gazebo](docs/gazebo.md) | Gazebo-specific setup |
## Getting Started
1. Read [docs/drone_guide.md](docs/drone_guide.md)
2. Edit `drone_controller.py`
3. Implement `calculate_landing_maneuver()`
4. Run: `python controllers.py --pattern stationary`
5. Increase difficulty: `python controllers.py --pattern circular`

26
activate.sh Executable file
View File

@@ -0,0 +1,26 @@
#!/bin/bash
# =============================================================================
# Drone Competition - Environment Activation Script
# =============================================================================
# This script activates both ROS 2 and the Python virtual environment.
#
# Usage:
# source activate.sh
# =============================================================================
# Get the directory where this script is located
SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
# Source ROS 2
source /opt/ros/jazzy/setup.bash
echo "[OK] ROS 2 jazzy sourced"
# Activate Python virtual environment
source "$SCRIPT_DIR/venv/bin/activate"
echo "[OK] Python venv activated"
echo ""
echo "Environment ready! You can now run:"
echo " python simulation_host.py"
echo " python ros_bridge.py"
echo ""

93
build_exe.py Normal file
View File

@@ -0,0 +1,93 @@
#!/usr/bin/env python3
"""
PyInstaller build script for simulation_host.py.
Creates a standalone executable that includes PyBullet and pybullet_data.
"""
import os
import platform
import sys
from pathlib import Path
try:
import PyInstaller.__main__
import pybullet_data
except ImportError as e:
print(f"Missing dependency: {e}")
print("Install with: pip install pyinstaller pybullet")
sys.exit(1)
def get_pybullet_data_path() -> str:
return pybullet_data.getDataPath()
def build_executable():
print("=" * 60)
print(" DRONE SIMULATION - BUILD EXECUTABLE")
print("=" * 60)
script_dir = Path(__file__).parent
source_file = script_dir / "simulation_host.py"
if not source_file.exists():
print(f"Error: {source_file} not found!")
sys.exit(1)
system = platform.system().lower()
print(f"Platform: {system}")
pybullet_path = get_pybullet_data_path()
print(f"PyBullet data path: {pybullet_path}")
if system == 'windows':
separator = ';'
else:
separator = ':'
data_spec = f"{pybullet_path}{separator}pybullet_data"
build_args = [
str(source_file),
'--onefile',
'--clean',
'--name=simulation_host',
f'--add-data={data_spec}',
]
if system == 'windows':
build_args.append('--windowed')
elif system == 'darwin':
build_args.append('--windowed')
else:
build_args.append('--console')
print("\nBuild configuration:")
for arg in build_args:
print(f" {arg}")
print("\nStarting PyInstaller build...")
print("-" * 60)
try:
PyInstaller.__main__.run(build_args)
print("-" * 60)
print("\nBuild completed successfully!")
dist_dir = script_dir / "dist"
if system == 'windows':
exe_path = dist_dir / "simulation_host.exe"
elif system == 'darwin':
exe_path = dist_dir / "simulation_host.app"
else:
exe_path = dist_dir / "simulation_host"
print(f"\nExecutable location: {exe_path}")
except Exception as e:
print(f"\nBuild failed: {e}")
sys.exit(1)
if __name__ == '__main__':
build_executable()

109
controllers.py Normal file
View File

@@ -0,0 +1,109 @@
#!/usr/bin/env python3
"""
Controllers - Runs DroneController and RoverController together.
Usage: python controllers.py [--pattern PATTERN] [--speed SPEED] [--amplitude AMP]
"""
import argparse
import sys
import rclpy
from rclpy.executors import MultiThreadedExecutor
from drone_controller import DroneController
from rover_controller import RoverController, MovementPattern
def parse_args():
parser = argparse.ArgumentParser(
description='Run drone and rover controllers together',
formatter_class=argparse.RawDescriptionHelpFormatter,
epilog="""
Rover Movement Patterns:
stationary - Rover stays at origin (easiest)
linear - Back and forth along X axis
circular - Circular path around origin
random - Random movement within bounds
square - Square pattern around origin
Examples:
python controllers.py
python controllers.py --pattern circular --speed 0.3
python controllers.py --pattern random --speed 0.5 --amplitude 3.0
"""
)
parser.add_argument(
'--pattern', '-p', type=str,
choices=[p.value for p in MovementPattern],
default='stationary',
help='Rover movement pattern (default: stationary)'
)
parser.add_argument(
'--speed', '-s', type=float, default=0.5,
help='Rover speed in m/s (default: 0.5)'
)
parser.add_argument(
'--amplitude', '-a', type=float, default=2.0,
help='Rover movement amplitude in meters (default: 2.0)'
)
args, _ = parser.parse_known_args()
return args
def main(args=None):
cli_args = parse_args()
print("\n" + "=" * 60)
print(" DRONE LANDING SIMULATION - CONTROLLERS")
print("=" * 60)
print(f" Rover Pattern: {cli_args.pattern}")
print(f" Rover Speed: {cli_args.speed} m/s")
print(f" Rover Amplitude: {cli_args.amplitude} m")
print("=" * 60 + "\n")
rclpy.init(args=args)
drone_controller = None
rover_controller = None
executor = None
try:
# Create drone controller
drone_controller = DroneController()
# Create rover controller with specified pattern
pattern = MovementPattern(cli_args.pattern)
rover_controller = RoverController(
pattern=pattern,
speed=cli_args.speed,
amplitude=cli_args.amplitude
)
# Use multi-threaded executor to run both nodes
executor = MultiThreadedExecutor(num_threads=4)
executor.add_node(drone_controller)
executor.add_node(rover_controller)
print("\nBoth controllers running. Press Ctrl+C to stop.\n")
executor.spin()
except KeyboardInterrupt:
print('\nShutting down controllers...')
except Exception as e:
print(f'Error: {e}')
sys.exit(1)
finally:
if executor:
executor.shutdown()
if drone_controller:
drone_controller.destroy_node()
if rover_controller:
rover_controller.destroy_node()
if rclpy.ok():
rclpy.shutdown()
print('Controllers stopped.')
if __name__ == '__main__':
main()

119
docs/architecture.md Normal file
View File

@@ -0,0 +1,119 @@
# Architecture Overview
GPS-denied drone landing simulation with camera vision.
## System Diagram
```
┌─────────────────────────────────────────────────────────────────────────┐
│ Simulation System │
├─────────────────────────────────────────────────────────────────────────┤
│ │
│ ┌──────────────────┐ ┌──────────────────────────┐ │
│ │ simulation_host │◄── UDP:5555 ──────►│ ros_bridge.py │ │
│ │ (PyBullet) │ │ (UDP ↔ ROS Bridge) │ │
│ └──────────────────┘ └────────────┬─────────────┘ │
│ OR │ │
│ ┌──────────────────┐ ┌────────────┴─────────────┐ │
│ │ Gazebo │◄── ROS Topics ────►│ gazebo_bridge.py │ │
│ │ (Ignition) │ │ (Gazebo ↔ ROS Bridge) │ │
│ └──────────────────┘ └────────────┬─────────────┘ │
│ │ │
│ ┌────────────▼─────────────┐ │
│ │ controllers.py │ │
│ │ ┌─────────────────────┐ │ │
│ │ │ DroneController │ │ │
│ │ │ RoverController │ │ │
│ │ └─────────────────────┘ │ │
│ └──────────────────────────┘ │
└─────────────────────────────────────────────────────────────────────────┘
```
## Components
### Simulators
| Component | Description |
|-----------|-------------|
| **PyBullet** (`simulation_host.py`) | Standalone physics, UDP networking, camera rendering |
| **Gazebo** | Full robotics simulator, native ROS 2 integration, camera sensor |
### Bridges
| Component | Description |
|-----------|-------------|
| **ros_bridge.py** | Connects PyBullet ↔ ROS 2 via UDP |
| **gazebo_bridge.py** | Connects Gazebo ↔ ROS 2, provides same interface |
### Controllers
| Component | Description |
|-----------|-------------|
| **controllers.py** | Runs drone + rover controllers together |
| **drone_controller.py** | GPS-denied landing logic |
| **rover_controller.py** | Moving landing pad patterns |
## ROS Topics
| Topic | Type | Publisher | Subscriber |
|-------|------|-----------|------------|
| `/cmd_vel` | `Twist` | DroneController | Bridge |
| `/drone/telemetry` | `String` | Bridge | DroneController |
| `/rover/telemetry` | `String` | RoverController | DroneController |
| `/rover/cmd_vel` | `Twist` | RoverController | (internal) |
| `/rover/position` | `Point` | RoverController | (debug) |
## GPS-Denied Sensor Flow
```
Simulator Bridge DroneController
│ │ │
│ Render Camera │ │
│ Compute Physics │ │
│──────────────────────►│ │
│ │ │
│ │ GPS-Denied Sensors: │
│ │ - IMU │
│ │ - Altimeter │
│ │ - Velocity │
│ │ - Camera Image (JPEG) │
│ │ - Landing Pad Detection │
│ │─────────────────────────►│
│ │ │
│ │ /cmd_vel │
│◄──────────────────────│◄─────────────────────────│
```
## Camera System
Both simulators provide a downward-facing camera:
| Property | Value |
|----------|-------|
| Resolution | 320 x 240 |
| FOV | 60 degrees |
| Format | Base64 JPEG |
| Update Rate | ~5 Hz |
| Direction | Downward |
## Data Flow
### PyBullet Mode
```
DroneController → /cmd_vel → ros_bridge → UDP:5555 → simulation_host
simulation_host → UDP:5556 → ros_bridge → /drone/telemetry → DroneController
```
### Gazebo Mode
```
DroneController → /cmd_vel → gazebo_bridge → /drone/cmd_vel → Gazebo
Gazebo → /model/drone/odometry → gazebo_bridge → /drone/telemetry → DroneController
Gazebo → /drone/camera → gazebo_bridge → (encoded in telemetry)
```
## UDP Protocol
| Port | Direction | Content |
|------|-----------|---------|
| 5555 | Bridge → Simulator | Command JSON |
| 5556 | Simulator → Bridge | Telemetry JSON (includes camera image) |

187
docs/drone_guide.md Normal file
View File

@@ -0,0 +1,187 @@
# DroneController Guide (GPS-Denied)
Implement your landing algorithm in `drone_controller.py`.
## Quick Start
1. Edit `drone_controller.py`
2. Find `calculate_landing_maneuver()`
3. Implement your algorithm
4. Test: `python controllers.py --pattern stationary`
## GPS-Denied Challenge
No GPS available. You must use:
| Sensor | Data |
|--------|------|
| **IMU** | Orientation, angular velocity |
| **Altimeter** | Altitude, vertical velocity |
| **Velocity** | Estimated from optical flow |
| **Camera** | 320x240 downward image (base64 JPEG) |
| **Landing Pad** | Relative position (may be null!) |
## Function to Implement
```python
def calculate_landing_maneuver(self, telemetry, rover_telemetry):
# Your code here
return (thrust, pitch, roll, yaw)
```
## Sensor Data
### IMU
```python
imu = telemetry['imu']
roll = imu['orientation']['roll']
pitch = imu['orientation']['pitch']
yaw = imu['orientation']['yaw']
angular_vel = imu['angular_velocity'] # {x, y, z}
```
### Altimeter
```python
altimeter = telemetry['altimeter']
altitude = altimeter['altitude']
vertical_vel = altimeter['vertical_velocity']
```
### Velocity
```python
velocity = telemetry['velocity'] # {x, y, z} in m/s
```
### Camera
The drone has a downward-facing camera providing 320x240 JPEG images.
```python
import base64
from PIL import Image
import io
camera = telemetry['camera']
image_b64 = camera.get('image')
if image_b64:
image_bytes = base64.b64decode(image_b64)
image = Image.open(io.BytesIO(image_bytes))
# Process image for custom vision algorithms
```
### Landing Pad (Vision)
**Important: May be None if pad not visible!**
```python
landing_pad = telemetry['landing_pad']
if landing_pad is not None:
relative_x = landing_pad['relative_x'] # body frame
relative_y = landing_pad['relative_y'] # body frame
distance = landing_pad['distance'] # vertical
confidence = landing_pad['confidence'] # 0-1
```
## Control Output
| Value | Range | Effect |
|-------|-------|--------|
| thrust | ±1.0 | Up/down |
| pitch | ±0.5 | Forward/back |
| roll | ±0.5 | Left/right |
| yaw | ±0.5 | Rotation |
## Example Algorithm
```python
def calculate_landing_maneuver(self, telemetry, rover_telemetry):
altimeter = telemetry.get('altimeter', {})
altitude = altimeter.get('altitude', 5.0)
vertical_vel = altimeter.get('vertical_velocity', 0.0)
velocity = telemetry.get('velocity', {})
vel_x = velocity.get('x', 0.0)
vel_y = velocity.get('y', 0.0)
landing_pad = telemetry.get('landing_pad')
# Altitude control
thrust = 0.5 * (0 - altitude) - 0.3 * vertical_vel
# Horizontal control
if landing_pad is not None:
pitch = 0.3 * landing_pad['relative_x'] - 0.2 * vel_x
roll = 0.3 * landing_pad['relative_y'] - 0.2 * vel_y
else:
pitch = -0.2 * vel_x
roll = -0.2 * vel_y
return (thrust, pitch, roll, 0.0)
```
## Using the Camera
You can implement custom vision processing on the camera image:
```python
import cv2
import numpy as np
import base64
def process_camera(telemetry):
camera = telemetry.get('camera', {})
image_b64 = camera.get('image')
if not image_b64:
return None
# Decode JPEG
image_bytes = base64.b64decode(image_b64)
nparr = np.frombuffer(image_bytes, np.uint8)
image = cv2.imdecode(nparr, cv2.IMREAD_COLOR)
# Example: detect green landing pad
hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
green_mask = cv2.inRange(hsv, (35, 50, 50), (85, 255, 255))
# Find contours
contours, _ = cv2.findContours(green_mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
if contours:
largest = max(contours, key=cv2.contourArea)
M = cv2.moments(largest)
if M['m00'] > 0:
cx = int(M['m10'] / M['m00'])
cy = int(M['m01'] / M['m00'])
# cx, cy is center of detected pad in image coordinates
return (cx, cy)
return None
```
## Strategies
### When Pad Not Visible
- Maintain altitude and stabilize
- Search by ascending or spiraling
- Dead reckoning from last known position
### State Machine
1. Search → find pad
2. Approach → move above pad
3. Align → center over pad
4. Descend → controlled descent
5. Land → touch down
## Testing
```bash
# Easy
python controllers.py --pattern stationary
# Medium
python controllers.py --pattern circular --speed 0.2
# Hard
python controllers.py --pattern random --speed 0.3
```

158
docs/gazebo.md Normal file
View File

@@ -0,0 +1,158 @@
# Gazebo Simulation
Running the GPS-denied drone simulation with Gazebo.
## Prerequisites
Install Gazebo and ROS-Gazebo bridge:
```bash
./setup/install_ubuntu.sh
source activate.sh
```
## Quick Start
**Terminal 1 - Start Gazebo:**
```bash
source activate.sh
gz sim gazebo/worlds/drone_landing.sdf
```
**Terminal 2 - Spawn drone and start bridge:**
```bash
source activate.sh
# Spawn drone
gz service -s /world/drone_landing_world/create \
--reqtype gz.msgs.EntityFactory \
--reptype gz.msgs.Boolean \
--req 'sdf_filename: "gazebo/models/drone/model.sdf", name: "drone"'
# Start bridge
python gazebo_bridge.py
```
**Terminal 3 - Run controllers:**
```bash
source activate.sh
python controllers.py --pattern circular --speed 0.3
```
## World Description
The `drone_landing.sdf` world contains:
| Object | Description |
|--------|-------------|
| Ground Plane | Infinite flat surface |
| Sun | Directional light with shadows |
| Landing Pad | Green box with "H" marker at origin |
## Drone Model
Quadrotor drone with:
- **Body**: 0.3m × 0.3m × 0.1m, 1.0 kg
- **Rotors**: 4 spinning rotors
- **IMU**: Orientation and angular velocity
- **Camera**: 320x240 downward-facing sensor
- **Odometry**: Position and velocity
### Gazebo Plugins
| Plugin | Function |
|--------|----------|
| MulticopterMotorModel | Motor dynamics |
| MulticopterVelocityControl | Velocity commands |
| OdometryPublisher | Pose and twist |
## Camera System
The drone has a downward-facing camera:
| Property | Value |
|----------|-------|
| Resolution | 320 x 240 |
| FOV | 60 degrees |
| Format | Base64 encoded JPEG |
| Update Rate | 30 Hz (Gazebo) / ~5 Hz (in telemetry) |
| Topic | `/drone/camera` |
## Gazebo Topics
| Topic | Type | Description |
|-------|------|-------------|
| `/drone/cmd_vel` | `gz.msgs.Twist` | Velocity commands |
| `/model/drone/odometry` | `gz.msgs.Odometry` | Drone state |
| `/drone/camera` | `gz.msgs.Image` | Camera images |
| `/drone/imu` | `gz.msgs.IMU` | IMU data |
## GPS-Denied Sensors
The `gazebo_bridge.py` converts Gazebo data to GPS-denied sensor format:
| Sensor | Source |
|--------|--------|
| IMU | Odometry orientation + angular velocity |
| Altimeter | Odometry Z position |
| Velocity | Odometry twist |
| Camera | Camera sensor (base64 JPEG) |
| Landing Pad | Computed from relative position |
## Headless Mode
Run without GUI:
```bash
gz sim -s gazebo/worlds/drone_landing.sdf
```
## Using the Launch File
For ROS 2 packages:
```bash
ros2 launch <package_name> drone_landing.launch.py
```
## Troubleshooting
### "Cannot connect to display"
```bash
export DISPLAY=:0
# or use headless mode
gz sim -s gazebo/worlds/drone_landing.sdf
```
### Drone falls immediately
The velocity controller may need to be enabled:
```bash
gz topic -t /drone/enable -m gz.msgs.Boolean -p 'data: true'
```
### Topics not visible in ROS
Ensure the bridge is running:
```bash
python gazebo_bridge.py
```
### Model not found
Set the model path:
```bash
export GZ_SIM_RESOURCE_PATH=$PWD/gazebo/models:$GZ_SIM_RESOURCE_PATH
```
### Camera image not in telemetry
Ensure PIL/Pillow is installed:
```bash
pip install pillow
```

267
docs/installation.md Normal file
View File

@@ -0,0 +1,267 @@
# Installation Guide
This guide covers installation on Ubuntu, macOS, and Windows.
## Quick Install
### Ubuntu / Debian
```bash
cd simulation
chmod +x setup/install_ubuntu.sh
./setup/install_ubuntu.sh
source activate.sh
```
### macOS
```bash
cd simulation
chmod +x setup/install_macos.sh
./setup/install_macos.sh
source activate.sh
```
### Windows (PowerShell)
```powershell
cd simulation
Set-ExecutionPolicy RemoteSigned -Scope CurrentUser
.\setup\install_windows.ps1
.\activate.bat
```
---
## What Gets Installed
| Component | Description |
|-----------|-------------|
| **ROS 2** | Humble (Ubuntu 22.04) or Jazzy (Ubuntu 24.04) |
| **Gazebo** | Modern Ignition-based simulator |
| **Python venv** | Virtual environment with system site-packages |
| **PyBullet** | Lightweight physics engine |
| **PyInstaller** | Executable bundler |
| **ros_gz_bridge** | ROS 2 ↔ Gazebo topic bridge |
---
## Manual Installation
If the scripts don't work, follow these steps manually.
### Step 1: Install ROS 2
#### Ubuntu 22.04 (Humble)
```bash
sudo apt update && sudo apt install -y locales
sudo locale-gen en_US en_US.UTF-8
sudo update-locale LC_ALL=en_US.UTF-8 LANG=en_US.UTF-8
export LANG=en_US.UTF-8
sudo apt install -y software-properties-common curl
sudo add-apt-repository -y universe
sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key \
-o /usr/share/keyrings/ros-archive-keyring.gpg
echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" \
| sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null
sudo apt update
sudo apt install -y ros-humble-desktop
```
#### Ubuntu 24.04 (Jazzy)
```bash
# Same as above, but install ros-jazzy-desktop
sudo apt install -y ros-jazzy-desktop
```
### Step 2: Install Gazebo
```bash
# Ubuntu 22.04
sudo apt install -y ros-humble-ros-gz ros-humble-ros-gz-bridge
# Ubuntu 24.04
sudo apt install -y ros-jazzy-ros-gz ros-jazzy-ros-gz-bridge
```
### Step 3: Create Python Virtual Environment
```bash
sudo apt install -y python3-venv python3-full
cd /path/to/simulation
python3 -m venv venv --system-site-packages
source venv/bin/activate
```
### Step 4: Install Python Dependencies
```bash
pip install --upgrade pip
pip install pybullet pyinstaller
```
### Step 5: Create Activation Script
Create `activate.sh`:
```bash
#!/bin/bash
SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
# Source ROS 2 (adjust distro as needed)
source /opt/ros/humble/setup.bash
echo "[OK] ROS 2 sourced"
# Activate venv
source "$SCRIPT_DIR/venv/bin/activate"
echo "[OK] Python venv activated"
```
Make executable:
```bash
chmod +x activate.sh
```
---
## Verifying Installation
After installation, verify all components:
```bash
source activate.sh
# Check ROS 2
ros2 --version
# Check PyBullet
python3 -c "import pybullet; print('PyBullet OK')"
# Check rclpy
python3 -c "import rclpy; print('rclpy OK')"
# Check geometry_msgs
python3 -c "from geometry_msgs.msg import Twist; print('geometry_msgs OK')"
# Check Gazebo
gz sim --version
```
---
## Troubleshooting
### "externally-managed-environment" Error
This happens on modern Ubuntu/Debian due to PEP 668. Solution: use the virtual environment.
```bash
source activate.sh # Activates venv
pip install pybullet # Now works
```
### ROS 2 Packages Not Found
Ensure ROS 2 is sourced before activating venv:
```bash
source /opt/ros/humble/setup.bash # or jazzy
source venv/bin/activate
```
The `activate.sh` script handles this automatically.
### Gazebo Not Starting
Check if Gazebo is properly installed:
```bash
which gz
gz sim --version
```
If missing, install the ROS-Gazebo packages:
```bash
sudo apt install ros-humble-ros-gz # or jazzy
```
### PyBullet GUI Not Showing
PyBullet requires a display. Options:
1. Run on machine with monitor
2. Use X11 forwarding: `ssh -X user@host`
3. Use virtual display: `xvfb-run python simulation_host.py`
### Permission Denied on Scripts
Make scripts executable:
```bash
chmod +x setup/*.sh
chmod +x activate.sh
```
---
## Platform-Specific Notes
### Ubuntu
- Full support for both PyBullet and Gazebo
- ROS 2 installed via apt packages
- Recommended platform
### macOS
- PyBullet works well
- Gazebo support is limited
- ROS 2 installed via Homebrew or binary
### Windows
- PyBullet works in GUI mode
- Gazebo not officially supported
- ROS 2 requires Windows-specific binaries
- Consider WSL2 for full Linux experience
---
## Updating
To update the simulation framework:
```bash
cd simulation
git pull # If using git
# Reinstall Python dependencies
source activate.sh
pip install --upgrade pybullet pyinstaller
```
---
## Uninstalling
### Remove Virtual Environment
```bash
rm -rf venv/
rm activate.sh
```
### Remove ROS 2 (Ubuntu)
```bash
sudo apt remove ros-humble-* # or jazzy
sudo rm /etc/apt/sources.list.d/ros2.list
```

184
docs/protocol.md Normal file
View File

@@ -0,0 +1,184 @@
# Communication Protocol (GPS-Denied)
Message formats for GPS-denied drone operation with camera.
## Drone Commands
```json
{
"thrust": 0.5,
"pitch": 0.1,
"roll": -0.2,
"yaw": 0.0
}
```
| Field | Range | Description |
|-------|-------|-------------|
| `thrust` | ±1.0 | Vertical thrust (positive = up) |
| `pitch` | ±0.5 | Forward/backward tilt |
| `roll` | ±0.5 | Left/right tilt |
| `yaw` | ±0.5 | Rotation |
---
## Drone Telemetry
Published on `/drone/telemetry`. **No GPS position available.**
```json
{
"imu": {
"orientation": {"roll": 0.0, "pitch": 0.0, "yaw": 0.0},
"angular_velocity": {"x": 0.0, "y": 0.0, "z": 0.0},
"linear_acceleration": {"x": 0.0, "y": 0.0, "z": 9.81}
},
"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,
"fov": 60.0,
"image": "<base64 encoded JPEG>"
},
"landed": false,
"timestamp": 1.234
}
```
---
## Sensor Details
### IMU
Always available.
| Field | Unit | Description |
|-------|------|-------------|
| `orientation.roll/pitch/yaw` | radians | Euler angles |
| `angular_velocity.x/y/z` | rad/s | Rotation rates |
| `linear_acceleration.x/y/z` | m/s² | Acceleration |
### Altimeter
Always available.
| Field | Unit | Description |
|-------|------|-------------|
| `altitude` | meters | Height above ground |
| `vertical_velocity` | m/s | Vertical speed |
### Velocity
Estimated from optical flow.
| Field | Unit | Description |
|-------|------|-------------|
| `x` | m/s | Forward velocity |
| `y` | m/s | Lateral velocity |
| `z` | m/s | Vertical velocity |
### Landing Pad Detection
**May be null if pad not visible!**
| Field | Unit | Description |
|-------|------|-------------|
| `relative_x` | meters | Forward/back offset (body frame) |
| `relative_y` | meters | Left/right offset (body frame) |
| `distance` | meters | Vertical distance to pad |
| `confidence` | 0-1 | Detection confidence |
### Camera
Always available.
| Field | Description |
|-------|-------------|
| `width` | Image width in pixels |
| `height` | Image height in pixels |
| `fov` | Horizontal field of view in degrees |
| `image` | Base64 encoded JPEG (or null) |
---
## Using the Camera Image
The camera provides a base64-encoded JPEG image of what the drone sees looking down.
### Decoding the Image (Python)
```python
import base64
from PIL import Image
import io
def decode_camera_image(telemetry):
camera = telemetry.get('camera', {})
image_b64 = camera.get('image')
if image_b64 is None:
return None
# Decode base64 to bytes
image_bytes = base64.b64decode(image_b64)
# Load as PIL Image
image = Image.open(io.BytesIO(image_bytes))
return image
```
### Using with OpenCV
```python
import base64
import cv2
import numpy as np
def decode_camera_image_cv2(telemetry):
camera = telemetry.get('camera', {})
image_b64 = camera.get('image')
if image_b64 is None:
return None
# Decode base64 to bytes
image_bytes = base64.b64decode(image_b64)
# Convert to numpy array
nparr = np.frombuffer(image_bytes, np.uint8)
# Decode JPEG
image = cv2.imdecode(nparr, cv2.IMREAD_COLOR)
return image
```
### Image Properties
- **Resolution**: 320 x 240 pixels
- **Format**: JPEG (quality 70)
- **FOV**: 60 degrees
- **Direction**: Downward-facing
- **Update Rate**: ~5 Hz (every 5th telemetry frame)
---
## Rover Telemetry
For internal use by RoverController.
```json
{
"position": {"x": 1.5, "y": 0.8, "z": 0.15},
"velocity": {"x": 0.3, "y": 0.4, "z": 0.0},
"pattern": "circular",
"timestamp": 1.234
}
```

140
docs/pybullet.md Normal file
View File

@@ -0,0 +1,140 @@
# PyBullet Simulation
Running the GPS-denied drone simulation with PyBullet.
## Quick Start
**Terminal 1 - Simulator:**
```bash
source activate.sh
python simulation_host.py
```
**Terminal 2 - ROS Bridge:**
```bash
source activate.sh
python ros_bridge.py
```
**Terminal 3 - Controllers:**
```bash
source activate.sh
python controllers.py --pattern circular --speed 0.3
```
## Remote Setup
Run simulator on one machine, controllers on another.
**Machine 1 (with display):**
```bash
python simulation_host.py
```
**Machine 2 (headless):**
```bash
source activate.sh
python ros_bridge.py --host <MACHINE_1_IP>
python controllers.py
```
## Simulation Parameters
| Parameter | Value |
|-----------|-------|
| Physics Rate | 240 Hz |
| Telemetry Rate | 24 Hz |
| Drone Mass | 1.0 kg |
| Gravity | -9.81 m/s² |
## GPS-Denied Sensors
The simulator provides:
| Sensor | Description |
|--------|-------------|
| IMU | Orientation (roll, pitch, yaw), angular velocity |
| Altimeter | Barometric altitude, vertical velocity |
| Velocity | Optical flow estimate (x, y, z) |
| Camera | 320x240 downward JPEG image |
| Landing Pad | Vision-based relative position (60° FOV, 10m range) |
## Camera System
PyBullet renders a camera image from the drone's perspective:
| Property | Value |
|----------|-------|
| Resolution | 320 x 240 |
| FOV | 60 degrees |
| Format | Base64 encoded JPEG |
| Update Rate | ~5 Hz |
| Direction | Downward-facing |
The image is included in telemetry as `camera.image`.
## World Setup
| Object | Position | Description |
|--------|----------|-------------|
| Ground | z = 0 | Infinite plane |
| Rover | (0, 0, 0.15) | 1m × 1m landing pad |
| Drone | (0, 0, 5) | Starting position |
## UDP Communication
| Port | Direction | Data |
|------|-----------|------|
| 5555 | Bridge → Sim | Commands (JSON) |
| 5556 | Sim → Bridge | Telemetry (JSON with camera) |
## ROS Bridge Options
```bash
python ros_bridge.py --help
Options:
--host, -H Simulator IP (default: 127.0.0.1)
--port, -p Simulator port (default: 5555)
```
## Building Executable
Create standalone executable:
```bash
source activate.sh
python build_exe.py
```
Output: `dist/simulation_host` (or `.exe` on Windows)
## Troubleshooting
### "Cannot connect to X server"
PyBullet requires a display:
- Run on machine with monitor
- Use X11 forwarding: `ssh -X user@host`
- Virtual display: `xvfb-run python simulation_host.py`
### Drone flies erratically
Reduce control gains:
```python
Kp = 0.3
Kd = 0.2
```
### No telemetry received
1. Check simulator is running
2. Verify firewall allows UDP 5555-5556
3. Check IP address in ros_bridge.py
### Camera image not appearing
Ensure PIL/Pillow is installed:
```bash
pip install pillow
```

100
docs/rover_controller.md Normal file
View File

@@ -0,0 +1,100 @@
# Rover Controller
The RoverController creates a moving landing pad target.
## Usage
The rover controller is automatically included when running `controllers.py`:
```bash
# Stationary rover (default)
python controllers.py
# Moving rover
python controllers.py --pattern circular --speed 0.3
```
### Options
| Option | Short | Default | Description |
|--------|-------|---------|-------------|
| `--pattern` | `-p` | stationary | Movement pattern |
| `--speed` | `-s` | 0.5 | Speed in m/s |
| `--amplitude` | `-a` | 2.0 | Amplitude in meters |
## Movement Patterns
### Stationary
```bash
python controllers.py --pattern stationary
```
Rover stays at origin. Best for initial testing.
### Linear
```bash
python controllers.py --pattern linear --speed 0.3 --amplitude 2.0
```
Oscillates along X-axis.
### Circular
```bash
python controllers.py --pattern circular --speed 0.5 --amplitude 2.0
```
Follows circular path of radius `amplitude`.
### Random
```bash
python controllers.py --pattern random --speed 0.3 --amplitude 2.0
```
Moves to random positions. Changes target every 3 seconds.
### Square
```bash
python controllers.py --pattern square --speed 0.5 --amplitude 2.0
```
Square pattern with corners at `(±amplitude, ±amplitude)`.
## Difficulty Levels
| Level | Pattern | Speed | Description |
|-------|---------|-------|-------------|
| Beginner | stationary | 0.0 | Static target |
| Easy | linear | 0.2 | Predictable 1D |
| Medium | circular | 0.3 | Smooth 2D |
| Hard | random | 0.3 | Unpredictable |
| Expert | square | 0.5 | Sharp turns |
## Progressive Testing
Start easy and increase difficulty:
```bash
# Step 1: Static target
python controllers.py --pattern stationary
# Step 2: Slow linear motion
python controllers.py --pattern linear --speed 0.2
# Step 3: Slow circular motion
python controllers.py --pattern circular --speed 0.2
# Step 4: Faster circular
python controllers.py --pattern circular --speed 0.4
# Step 5: Random
python controllers.py --pattern random --speed 0.3
```
## Published Topics
| Topic | Type | Description |
|-------|------|-------------|
| `/rover/cmd_vel` | `Twist` | Velocity commands |
| `/rover/position` | `Point` | Current position |
| `/rover/telemetry` | `String` | Full state (JSON) |
## GPS-Denied Note
In GPS-denied mode, the drone cannot directly access rover position. Instead, it must detect the landing pad visually via `landing_pad` sensor data.
The `/rover/telemetry` topic is used internally by the RoverController but the DroneController should primarily rely on vision-based `landing_pad` detection in the drone telemetry.

214
drone_controller.py Normal file
View File

@@ -0,0 +1,214 @@
#!/usr/bin/env python3
"""
DroneController - Template for GPS-denied landing logic.
Implement your algorithm in calculate_landing_maneuver().
Uses sensors: IMU, altimeter, camera, and landing pad detection.
"""
import json
from typing import Dict, Any, Optional
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
from std_msgs.msg import String
class DroneController(Node):
"""Drone controller for GPS-denied landing."""
CONTROL_RATE = 50.0
LANDING_HEIGHT_THRESHOLD = 0.1
LANDING_VELOCITY_THRESHOLD = 0.1
MAX_THRUST = 1.0
MAX_PITCH = 0.5
MAX_ROLL = 0.5
MAX_YAW = 0.5
def __init__(self):
super().__init__('drone_controller')
self.get_logger().info('=' * 50)
self.get_logger().info('Drone Controller Starting (GPS-Denied)...')
self.get_logger().info('=' * 50)
self._latest_telemetry: Optional[Dict[str, Any]] = None
self._rover_telemetry: Optional[Dict[str, Any]] = None
self._telemetry_received = False
self._landing_complete = False
self._telemetry_sub = self.create_subscription(
String, '/drone/telemetry', self._telemetry_callback, 10
)
self.get_logger().info(' Subscribed to: /drone/telemetry')
self._rover_telemetry_sub = self.create_subscription(
String, '/rover/telemetry', self._rover_telemetry_callback, 10
)
self.get_logger().info(' Subscribed to: /rover/telemetry')
self._cmd_vel_pub = self.create_publisher(Twist, '/cmd_vel', 10)
self.get_logger().info(' Publishing to: /cmd_vel')
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.get_logger().info('Drone Controller Ready!')
self.get_logger().info('Sensors: IMU, Altimeter, Camera, Landing Pad Detection')
def _telemetry_callback(self, msg: String) -> None:
try:
self._latest_telemetry = json.loads(msg.data)
if not self._telemetry_received:
self._telemetry_received = True
self.get_logger().info('First telemetry received!')
except json.JSONDecodeError as e:
self.get_logger().warning(f'Failed to parse telemetry: {e}')
def _rover_telemetry_callback(self, msg: String) -> None:
try:
self._rover_telemetry = json.loads(msg.data)
except json.JSONDecodeError:
pass
def _control_loop(self) -> None:
if self._latest_telemetry is None:
return
if self._landing_complete:
return
if 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)
return
thrust, pitch, roll, yaw = self.calculate_landing_maneuver(
self._latest_telemetry,
self._rover_telemetry
)
thrust = max(min(thrust, self.MAX_THRUST), -self.MAX_THRUST)
pitch = max(min(pitch, self.MAX_PITCH), -self.MAX_PITCH)
roll = max(min(roll, self.MAX_ROLL), -self.MAX_ROLL)
yaw = max(min(yaw, self.MAX_YAW), -self.MAX_YAW)
self._publish_command(thrust, pitch, roll, yaw)
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_THRESHOLD and vertical_velocity < self.LANDING_VELOCITY_THRESHOLD
except (KeyError, TypeError):
return False
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
altimeter = telemetry.get('altimeter', {})
altitude = altimeter.get('altitude', 5.0)
vertical_vel = altimeter.get('vertical_velocity', 0.0)
velocity = telemetry.get('velocity', {'x': 0, 'y': 0, 'z': 0})
vel_x = velocity.get('x', 0.0)
vel_y = velocity.get('y', 0.0)
landing_pad = telemetry.get('landing_pad', None)
# Camera image is available in telemetry['camera']['image']
# Decode with: base64.b64decode(telemetry['camera']['image'])
# Default target
target_x = 0.0
target_y = 0.0
# Use landing pad detection for positioning
if landing_pad is not None:
target_x = landing_pad.get('relative_x', 0.0)
target_y = landing_pad.get('relative_y', 0.0)
# TODO: Implement your GPS-denied landing algorithm
# You can use the camera image for custom vision processing
# Simple PD controller for altitude
target_altitude = 0.0
altitude_error = target_altitude - altitude
Kp_z, Kd_z = 0.5, 0.3
thrust = Kp_z * altitude_error - Kd_z * vertical_vel
# Horizontal control based on landing pad detection
Kp_xy, Kd_xy = 0.3, 0.2
if landing_pad is not None:
pitch = Kp_xy * target_x - Kd_xy * vel_x
roll = Kp_xy * target_y - Kd_xy * vel_y
else:
pitch = -Kd_xy * vel_x
roll = -Kd_xy * vel_y
yaw = 0.0
return (thrust, pitch, roll, yaw)
def _publish_command(self, thrust: float, pitch: float, roll: float, yaw: float) -> None:
msg = Twist()
msg.linear.z = thrust
msg.linear.x = pitch
msg.linear.y = roll
msg.angular.z = yaw
self._cmd_vel_pub.publish(msg)
def main(args=None):
rclpy.init(args=args)
controller = None
try:
controller = DroneController()
rclpy.spin(controller)
except KeyboardInterrupt:
print('\nShutting down...')
finally:
if controller:
controller.destroy_node()
if rclpy.ok():
rclpy.shutdown()
if __name__ == '__main__':
main()

84
gazebo/launch/drone_landing.launch.py Normal file
View File

@@ -0,0 +1,84 @@
#!/usr/bin/env python3
"""
Gazebo Launch File - Drone Landing Simulation
Launches Gazebo with the world and spawns the drone.
"""
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription
from launch.conditions import IfCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch_ros.actions import Node
def generate_launch_description():
"""Generate the launch description."""
script_dir = os.path.dirname(os.path.abspath(__file__))
gazebo_dir = os.path.dirname(script_dir)
world_file = os.path.join(gazebo_dir, 'worlds', 'drone_landing.sdf')
model_path = os.path.join(gazebo_dir, 'models')
drone_model = os.path.join(model_path, 'drone', 'model.sdf')
gz_resource_path = os.environ.get('GZ_SIM_RESOURCE_PATH', '')
if model_path not in gz_resource_path:
os.environ['GZ_SIM_RESOURCE_PATH'] = f"{model_path}:{gz_resource_path}"
use_sim_time = LaunchConfiguration('use_sim_time', default='true')
headless = LaunchConfiguration('headless', default='false')
return LaunchDescription([
DeclareLaunchArgument(
'use_sim_time',
default_value='true',
description='Use simulation (Gazebo) clock'
),
DeclareLaunchArgument(
'headless',
default_value='false',
description='Run Gazebo in headless mode (no GUI)'
),
ExecuteProcess(
cmd=['gz', 'sim', '-r', world_file],
output='screen',
additional_env={'GZ_SIM_RESOURCE_PATH': os.environ.get('GZ_SIM_RESOURCE_PATH', '')}
),
ExecuteProcess(
cmd=[
'gz', 'service', '-s', '/world/drone_landing_world/create',
'--reqtype', 'gz.msgs.EntityFactory',
'--reptype', 'gz.msgs.Boolean',
'--timeout', '5000',
'--req', f'sdf_filename: "{drone_model}", name: "drone"'
],
output='screen'
),
Node(
package='ros_gz_bridge',
executable='parameter_bridge',
name='gz_bridge',
arguments=[
'/drone/cmd_vel@geometry_msgs/msg/Twist@gz.msgs.Twist',
'/model/drone/odometry@nav_msgs/msg/Odometry@gz.msgs.Odometry',
'/drone/imu@sensor_msgs/msg/Imu@gz.msgs.IMU',
'/clock@rosgraph_msgs/msg/Clock@gz.msgs.Clock',
],
parameters=[{'use_sim_time': use_sim_time}],
output='screen'
),
])
if __name__ == '__main__':
print("This is a ROS 2 launch file. Run with:")
print(" ros2 launch <package_name> drone_landing.launch.py")
print("")
print("Or use the standalone script:")
print(" python3 gazebo_bridge.py")

17
gazebo/models/drone/model.config Normal file
View File

@@ -0,0 +1,17 @@
<?xml version="1.0"?>
<model>
<name>competition_drone</name>
<version>1.0</version>
<sdf version="1.8">model.sdf</sdf>
<author>
<name>Competition Framework</name>
<email>competition@example.com</email>
</author>
<description>
A simple quadrotor drone for the drone landing competition.
Features velocity control via /drone/cmd_vel topic and
publishes odometry for state feedback.
</description>
</model>

400
gazebo/models/drone/model.sdf Normal file
View File

@@ -0,0 +1,400 @@
<?xml version="1.0" ?>
<!--
Simple Quadrotor Drone Model for Competition
Features:
- Box body with 4 rotors
- IMU sensor
- MulticopterMotorModel plugin for flight dynamics
- Pose publisher for telemetry
-->
<sdf version="1.8">
<model name="competition_drone">
<pose>0 0 5 0 0 0</pose>
<!-- Main body -->
<link name="base_link">
<inertial>
<mass>1.0</mass>
<inertia>
<ixx>0.029125</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.029125</iyy>
<iyz>0</iyz>
<izz>0.055225</izz>
</inertia>
</inertial>
<collision name="body_collision">
<geometry>
<box>
<size>0.3 0.3 0.1</size>
</box>
</geometry>
</collision>
<visual name="body_visual">
<geometry>
<box>
<size>0.3 0.3 0.1</size>
</box>
</geometry>
<material>
<ambient>0.6 0.1 0.1 1</ambient>
<diffuse>0.8 0.2 0.2 1</diffuse>
<specular>0.2 0.2 0.2 1</specular>
</material>
</visual>
<!-- Front-left arm -->
<visual name="arm_fl">
<pose>0.12 0.12 0 0 0 0.785</pose>
<geometry>
<box>
<size>0.2 0.02 0.02</size>
</box>
</geometry>
<material>
<ambient>0.2 0.2 0.2 1</ambient>
<diffuse>0.3 0.3 0.3 1</diffuse>
</material>
</visual>
<!-- Front-right arm -->
<visual name="arm_fr">
<pose>0.12 -0.12 0 0 0 -0.785</pose>
<geometry>
<box>
<size>0.2 0.02 0.02</size>
</box>
</geometry>
<material>
<ambient>0.2 0.2 0.2 1</ambient>
<diffuse>0.3 0.3 0.3 1</diffuse>
</material>
</visual>
<!-- Back-left arm -->
<visual name="arm_bl">
<pose>-0.12 0.12 0 0 0 -0.785</pose>
<geometry>
<box>
<size>0.2 0.02 0.02</size>
</box>
</geometry>
<material>
<ambient>0.2 0.2 0.2 1</ambient>
<diffuse>0.3 0.3 0.3 1</diffuse>
</material>
</visual>
<!-- Back-right arm -->
<visual name="arm_br">
<pose>-0.12 -0.12 0 0 0 0.785</pose>
<geometry>
<box>
<size>0.2 0.02 0.02</size>
</box>
</geometry>
<material>
<ambient>0.2 0.2 0.2 1</ambient>
<diffuse>0.3 0.3 0.3 1</diffuse>
</material>
</visual>
<!-- IMU Sensor -->
<sensor name="imu_sensor" type="imu">
<always_on>true</always_on>
<update_rate>100</update_rate>
<imu>
<angular_velocity>
<x><noise type="gaussian"><mean>0</mean><stddev>0.01</stddev></noise></x>
<y><noise type="gaussian"><mean>0</mean><stddev>0.01</stddev></noise></y>
<z><noise type="gaussian"><mean>0</mean><stddev>0.01</stddev></noise></z>
</angular_velocity>
<linear_acceleration>
<x><noise type="gaussian"><mean>0</mean><stddev>0.1</stddev></noise></x>
<y><noise type="gaussian"><mean>0</mean><stddev>0.1</stddev></noise></y>
<z><noise type="gaussian"><mean>0</mean><stddev>0.1</stddev></noise></z>
</linear_acceleration>
</imu>
</sensor>
<!-- Downward-facing Camera -->
<sensor name="camera" type="camera">
<pose>0 0 -0.05 0 1.5708 0</pose>
<always_on>true</always_on>
<update_rate>30</update_rate>
<camera>
<horizontal_fov>1.047</horizontal_fov>
<image>
<width>320</width>
<height>240</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.1</near>
<far>20</far>
</clip>
</camera>
<topic>/drone/camera</topic>
</sensor>
</link>
<!-- Rotor 1 (Front-Left, CCW) -->
<link name="rotor_fl">
<pose>0.17 0.17 0.05 0 0 0</pose>
<inertial>
<mass>0.01</mass>
<inertia>
<ixx>0.00001</ixx><ixy>0</ixy><ixz>0</ixz>
<iyy>0.00001</iyy><iyz>0</iyz>
<izz>0.00002</izz>
</inertia>
</inertial>
<visual name="rotor_visual">
<geometry>
<cylinder>
<radius>0.08</radius>
<length>0.01</length>
</cylinder>
</geometry>
<material>
<ambient>0.1 0.1 0.1 1</ambient>
<diffuse>0.2 0.2 0.2 1</diffuse>
</material>
</visual>
</link>
<joint name="rotor_fl_joint" type="revolute">
<parent>base_link</parent>
<child>rotor_fl</child>
<axis>
<xyz>0 0 1</xyz>
<limit><lower>-1e16</lower><upper>1e16</upper></limit>
</axis>
</joint>
<!-- Rotor 2 (Front-Right, CW) -->
<link name="rotor_fr">
<pose>0.17 -0.17 0.05 0 0 0</pose>
<inertial>
<mass>0.01</mass>
<inertia>
<ixx>0.00001</ixx><ixy>0</ixy><ixz>0</ixz>
<iyy>0.00001</iyy><iyz>0</iyz>
<izz>0.00002</izz>
</inertia>
</inertial>
<visual name="rotor_visual">
<geometry>
<cylinder>
<radius>0.08</radius>
<length>0.01</length>
</cylinder>
</geometry>
<material>
<ambient>0.1 0.1 0.1 1</ambient>
<diffuse>0.2 0.2 0.2 1</diffuse>
</material>
</visual>
</link>
<joint name="rotor_fr_joint" type="revolute">
<parent>base_link</parent>
<child>rotor_fr</child>
<axis>
<xyz>0 0 1</xyz>
<limit><lower>-1e16</lower><upper>1e16</upper></limit>
</axis>
</joint>
<!-- Rotor 3 (Back-Left, CW) -->
<link name="rotor_bl">
<pose>-0.17 0.17 0.05 0 0 0</pose>
<inertial>
<mass>0.01</mass>
<inertia>
<ixx>0.00001</ixx><ixy>0</ixy><ixz>0</ixz>
<iyy>0.00001</iyy><iyz>0</iyz>
<izz>0.00002</izz>
</inertia>
</inertial>
<visual name="rotor_visual">
<geometry>
<cylinder>
<radius>0.08</radius>
<length>0.01</length>
</cylinder>
</geometry>
<material>
<ambient>0.1 0.1 0.1 1</ambient>
<diffuse>0.2 0.2 0.2 1</diffuse>
</material>
</visual>
</link>
<joint name="rotor_bl_joint" type="revolute">
<parent>base_link</parent>
<child>rotor_bl</child>
<axis>
<xyz>0 0 1</xyz>
<limit><lower>-1e16</lower><upper>1e16</upper></limit>
</axis>
</joint>
<!-- Rotor 4 (Back-Right, CCW) -->
<link name="rotor_br">
<pose>-0.17 -0.17 0.05 0 0 0</pose>
<inertial>
<mass>0.01</mass>
<inertia>
<ixx>0.00001</ixx><ixy>0</ixy><ixz>0</ixz>
<iyy>0.00001</iyy><iyz>0</iyz>
<izz>0.00002</izz>
</inertia>
</inertial>
<visual name="rotor_visual">
<geometry>
<cylinder>
<radius>0.08</radius>
<length>0.01</length>
</cylinder>
</geometry>
<material>
<ambient>0.1 0.1 0.1 1</ambient>
<diffuse>0.2 0.2 0.2 1</diffuse>
</material>
</visual>
</link>
<joint name="rotor_br_joint" type="revolute">
<parent>base_link</parent>
<child>rotor_br</child>
<axis>
<xyz>0 0 1</xyz>
<limit><lower>-1e16</lower><upper>1e16</upper></limit>
</axis>
</joint>
<!-- MulticopterMotorModel Plugin for each rotor -->
<plugin filename="gz-sim-multicopter-motor-model-system" name="gz::sim::systems::MulticopterMotorModel">
<robotNamespace>drone</robotNamespace>
<jointName>rotor_fl_joint</jointName>
<linkName>rotor_fl</linkName>
<turningDirection>ccw</turningDirection>
<timeConstantUp>0.0125</timeConstantUp>
<timeConstantDown>0.025</timeConstantDown>
<maxRotVelocity>1000</maxRotVelocity>
<motorConstant>8.54858e-06</motorConstant>
<momentConstant>0.016</momentConstant>
<commandSubTopic>command/motor_speed</commandSubTopic>
<motorNumber>0</motorNumber>
<rotorDragCoefficient>0.000806428</rotorDragCoefficient>
<rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
<rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
</plugin>
<plugin filename="gz-sim-multicopter-motor-model-system" name="gz::sim::systems::MulticopterMotorModel">
<robotNamespace>drone</robotNamespace>
<jointName>rotor_fr_joint</jointName>
<linkName>rotor_fr</linkName>
<turningDirection>cw</turningDirection>
<timeConstantUp>0.0125</timeConstantUp>
<timeConstantDown>0.025</timeConstantDown>
<maxRotVelocity>1000</maxRotVelocity>
<motorConstant>8.54858e-06</motorConstant>
<momentConstant>0.016</momentConstant>
<commandSubTopic>command/motor_speed</commandSubTopic>
<motorNumber>1</motorNumber>
<rotorDragCoefficient>0.000806428</rotorDragCoefficient>
<rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
<rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
</plugin>
<plugin filename="gz-sim-multicopter-motor-model-system" name="gz::sim::systems::MulticopterMotorModel">
<robotNamespace>drone</robotNamespace>
<jointName>rotor_bl_joint</jointName>
<linkName>rotor_bl</linkName>
<turningDirection>cw</turningDirection>
<timeConstantUp>0.0125</timeConstantUp>
<timeConstantDown>0.025</timeConstantDown>
<maxRotVelocity>1000</maxRotVelocity>
<motorConstant>8.54858e-06</motorConstant>
<momentConstant>0.016</momentConstant>
<commandSubTopic>command/motor_speed</commandSubTopic>
<motorNumber>2</motorNumber>
<rotorDragCoefficient>0.000806428</rotorDragCoefficient>
<rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
<rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
</plugin>
<plugin filename="gz-sim-multicopter-motor-model-system" name="gz::sim::systems::MulticopterMotorModel">
<robotNamespace>drone</robotNamespace>
<jointName>rotor_br_joint</jointName>
<linkName>rotor_br</linkName>
<turningDirection>ccw</turningDirection>
<timeConstantUp>0.0125</timeConstantUp>
<timeConstantDown>0.025</timeConstantDown>
<maxRotVelocity>1000</maxRotVelocity>
<motorConstant>8.54858e-06</motorConstant>
<momentConstant>0.016</momentConstant>
<commandSubTopic>command/motor_speed</commandSubTopic>
<motorNumber>3</motorNumber>
<rotorDragCoefficient>0.000806428</rotorDragCoefficient>
<rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
<rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
</plugin>
<!-- Multicopter Velocity Controller -->
<plugin filename="gz-sim-multicopter-control-system" name="gz::sim::systems::MulticopterVelocityControl">
<robotNamespace>drone</robotNamespace>
<commandSubTopic>cmd_vel</commandSubTopic>
<enableSubTopic>enable</enableSubTopic>
<comLinkName>base_link</comLinkName>
<velocityGain>2.7 2.7 2.7</velocityGain>
<attitudeGain>2 3 0.15</attitudeGain>
<angularRateGain>0.4 0.52 0.18</angularRateGain>
<maximumLinearAcceleration>2 2 2</maximumLinearAcceleration>
<rotorConfiguration>
<rotor>
<jointName>rotor_fl_joint</jointName>
<forceConstant>8.54858e-06</forceConstant>
<momentConstant>0.016</momentConstant>
<direction>1</direction>
</rotor>
<rotor>
<jointName>rotor_fr_joint</jointName>
<forceConstant>8.54858e-06</forceConstant>
<momentConstant>0.016</momentConstant>
<direction>-1</direction>
</rotor>
<rotor>
<jointName>rotor_bl_joint</jointName>
<forceConstant>8.54858e-06</forceConstant>
<momentConstant>0.016</momentConstant>
<direction>-1</direction>
</rotor>
<rotor>
<jointName>rotor_br_joint</jointName>
<forceConstant>8.54858e-06</forceConstant>
<momentConstant>0.016</momentConstant>
<direction>1</direction>
</rotor>
</rotorConfiguration>
</plugin>
<!-- Odometry Publisher -->
<plugin filename="gz-sim-odometry-publisher-system" name="gz::sim::systems::OdometryPublisher">
<odom_frame>odom</odom_frame>
<robot_base_frame>base_link</robot_base_frame>
<odom_publish_frequency>50</odom_publish_frequency>
</plugin>
<!-- Pose Publisher for telemetry -->
<plugin filename="gz-sim-pose-publisher-system" name="gz::sim::systems::PosePublisher">
<publish_link_pose>true</publish_link_pose>
<publish_nested_model_pose>true</publish_nested_model_pose>
<update_frequency>50</update_frequency>
</plugin>
</model>
</sdf>

138
gazebo/worlds/drone_landing.sdf Normal file
View File

@@ -0,0 +1,138 @@
<?xml version="1.0" ?>
<!--
Drone Landing Competition - Gazebo World
This world contains:
- Ground plane with grid texture
- Landing pad (rover) at origin
- Drone spawned at 5m altitude
- Sun for lighting
-->
<sdf version="1.8">
<world name="drone_landing_world">
<!-- Physics configuration -->
<physics name="1ms" type="ignored">
<max_step_size>0.001</max_step_size>
<real_time_factor>1.0</real_time_factor>
</physics>
<!-- Required plugins -->
<plugin filename="gz-sim-physics-system" name="gz::sim::systems::Physics"/>
<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-sensors-system" name="gz::sim::systems::Sensors">
<render_engine>ogre2</render_engine>
</plugin>
<plugin filename="gz-sim-imu-system" name="gz::sim::systems::Imu"/>
<!-- Lighting -->
<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>
<attenuation>
<range>1000</range>
<constant>0.9</constant>
<linear>0.01</linear>
<quadratic>0.001</quadratic>
</attenuation>
<direction>-0.5 0.1 -0.9</direction>
</light>
<!-- Ground Plane -->
<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.7 0.7 0.7 1</diffuse>
<specular>0.01 0.01 0.01 1</specular>
</material>
</visual>
</link>
</model>
<!-- Landing Pad (Rover) -->
<model name="landing_pad">
<static>true</static>
<pose>0 0 0.15 0 0 0</pose>
<link name="base_link">
<collision name="collision">
<geometry>
<box>
<size>1.0 1.0 0.3</size>
</box>
</geometry>
</collision>
<visual name="visual">
<geometry>
<box>
<size>1.0 1.0 0.3</size>
</box>
</geometry>
<material>
<ambient>0.1 0.4 0.1 1</ambient>
<diffuse>0.2 0.6 0.2 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
</material>
</visual>
<!-- Landing pad "H" marker -->
<visual name="h_marker">
<pose>0 0 0.151 0 0 0</pose>
<geometry>
<box>
<size>0.6 0.1 0.001</size>
</box>
</geometry>
<material>
<ambient>1 1 1 1</ambient>
<diffuse>1 1 1 1</diffuse>
</material>
</visual>
<visual name="h_left">
<pose>-0.25 0 0.151 0 0 0</pose>
<geometry>
<box>
<size>0.1 0.6 0.001</size>
</box>
</geometry>
<material>
<ambient>1 1 1 1</ambient>
<diffuse>1 1 1 1</diffuse>
</material>
</visual>
<visual name="h_right">
<pose>0.25 0 0.151 0 0 0</pose>
<geometry>
<box>
<size>0.1 0.6 0.001</size>
</box>
</geometry>
<material>
<ambient>1 1 1 1</ambient>
<diffuse>1 1 1 1</diffuse>
</material>
</visual>
</link>
</model>
</world>
</sdf>

271
gazebo_bridge.py Normal file
View File

@@ -0,0 +1,271 @@
#!/usr/bin/env python3
"""
Gazebo Bridge - GPS-denied interface with camera.
Provides sensor data: IMU, altimeter, camera image.
"""
import base64
import json
import math
from typing import Optional, Dict, Any
import numpy as np
import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy
from geometry_msgs.msg import Twist
from nav_msgs.msg import Odometry
from sensor_msgs.msg import Image
from std_msgs.msg import String
class GazeboBridge(Node):
"""Bridges Gazebo topics to GPS-denied sensor interface with camera."""
CAMERA_FOV = 60.0
CAMERA_RANGE = 10.0
CAMERA_WIDTH = 320
CAMERA_HEIGHT = 240
def __init__(self):
super().__init__('gazebo_bridge')
self.get_logger().info('=' * 60)
self.get_logger().info('Gazebo Bridge Starting (GPS-Denied + Camera)...')
self.get_logger().info('=' * 60)
self._landed = False
self._step_count = 0
self._rover_pos = [0.0, 0.0, 0.15]
self._last_camera_image = None
self._drone_pos = [0.0, 0.0, 5.0]
self._drone_orn = [0.0, 0.0, 0.0, 1.0]
sensor_qos = QoSProfile(
reliability=ReliabilityPolicy.BEST_EFFORT,
depth=10
)
self._cmd_vel_sub = self.create_subscription(
Twist, '/cmd_vel', self._cmd_vel_callback, 10
)
self.get_logger().info(' Subscribed to: /cmd_vel')
self._odom_sub = self.create_subscription(
Odometry, '/model/drone/odometry', self._odom_callback, sensor_qos
)
self.get_logger().info(' Subscribed to: /model/drone/odometry')
self._camera_sub = self.create_subscription(
Image, '/drone/camera', self._camera_callback, sensor_qos
)
self.get_logger().info(' Subscribed to: /drone/camera')
self._rover_sub = self.create_subscription(
String, '/rover/telemetry', self._rover_callback, 10
)
self.get_logger().info(' Subscribed to: /rover/telemetry')
self._gz_cmd_vel_pub = self.create_publisher(
Twist, '/drone/cmd_vel', 10
)
self.get_logger().info(' Publishing to: /drone/cmd_vel (Gazebo)')
self._telemetry_pub = self.create_publisher(
String, '/drone/telemetry', 10
)
self.get_logger().info(' Publishing to: /drone/telemetry')
self.get_logger().info('Gazebo Bridge Ready!')
def _cmd_vel_callback(self, msg: Twist) -> None:
gz_msg = Twist()
gz_msg.linear.x = msg.linear.x
gz_msg.linear.y = msg.linear.y
gz_msg.linear.z = msg.linear.z
gz_msg.angular.z = msg.angular.z
self._gz_cmd_vel_pub.publish(gz_msg)
def _rover_callback(self, msg: String) -> None:
try:
data = json.loads(msg.data)
pos = data.get('position', {})
self._rover_pos = [
pos.get('x', 0.0),
pos.get('y', 0.0),
pos.get('z', 0.15)
]
except json.JSONDecodeError:
pass
def _camera_callback(self, msg: Image) -> None:
"""Process camera images and encode as base64."""
try:
if msg.encoding == 'rgb8':
image = np.frombuffer(msg.data, dtype=np.uint8).reshape(
msg.height, msg.width, 3
)
elif msg.encoding == 'bgr8':
image = np.frombuffer(msg.data, dtype=np.uint8).reshape(
msg.height, msg.width, 3
)[:, :, ::-1]
else:
return
# Encode as base64 JPEG
try:
from PIL import Image as PILImage
import io
pil_img = PILImage.fromarray(image)
buffer = io.BytesIO()
pil_img.save(buffer, format='JPEG', quality=70)
self._last_camera_image = base64.b64encode(buffer.getvalue()).decode('utf-8')
except ImportError:
self._last_camera_image = base64.b64encode(image.tobytes()).decode('utf-8')
except Exception as e:
self.get_logger().warning(f'Camera processing error: {e}')
def _get_landing_pad_detection(self) -> Optional[Dict[str, float]]:
dx = self._rover_pos[0] - self._drone_pos[0]
dy = self._rover_pos[1] - self._drone_pos[1]
dz = self._rover_pos[2] - self._drone_pos[2]
horizontal_dist = math.sqrt(dx**2 + dy**2)
vertical_dist = -dz
if vertical_dist <= 0 or vertical_dist > self.CAMERA_RANGE:
return None
fov_rad = math.radians(self.CAMERA_FOV / 2)
max_horizontal = vertical_dist * math.tan(fov_rad)
if horizontal_dist > max_horizontal:
return None
roll, pitch, yaw = self._quaternion_to_euler(
self._drone_orn[0], self._drone_orn[1],
self._drone_orn[2], self._drone_orn[3]
)
cos_yaw = math.cos(-yaw)
sin_yaw = math.sin(-yaw)
relative_x = dx * cos_yaw - dy * sin_yaw
relative_y = dx * sin_yaw + dy * cos_yaw
angle = math.atan2(horizontal_dist, vertical_dist)
confidence = max(0.0, 1.0 - (angle / fov_rad))
confidence *= max(0.0, 1.0 - (vertical_dist / self.CAMERA_RANGE))
return {
"relative_x": round(relative_x, 4),
"relative_y": round(relative_y, 4),
"distance": round(vertical_dist, 4),
"confidence": round(confidence, 4)
}
def _odom_callback(self, msg: Odometry) -> None:
self._step_count += 1
pos = msg.pose.pose.position
vel = msg.twist.twist.linear
ang_vel = msg.twist.twist.angular
quat = msg.pose.pose.orientation
self._drone_pos = [pos.x, pos.y, pos.z]
self._drone_orn = [quat.x, quat.y, quat.z, quat.w]
roll, pitch, yaw = self._quaternion_to_euler(quat.x, quat.y, quat.z, quat.w)
landing_height = 0.5
self._landed = (
abs(pos.x - self._rover_pos[0]) < 0.5 and
abs(pos.y - self._rover_pos[1]) < 0.5 and
pos.z < landing_height and
abs(vel.z) < 0.2
)
pad_detection = self._get_landing_pad_detection()
telemetry = {
"imu": {
"orientation": {
"roll": round(roll, 4),
"pitch": round(pitch, 4),
"yaw": round(yaw, 4)
},
"angular_velocity": {
"x": round(ang_vel.x, 4),
"y": round(ang_vel.y, 4),
"z": round(ang_vel.z, 4)
},
"linear_acceleration": {
"x": 0.0,
"y": 0.0,
"z": 9.81
}
},
"altimeter": {
"altitude": round(pos.z, 4),
"vertical_velocity": round(vel.z, 4)
},
"velocity": {
"x": round(vel.x, 4),
"y": round(vel.y, 4),
"z": round(vel.z, 4)
},
"landing_pad": pad_detection,
"camera": {
"width": self.CAMERA_WIDTH,
"height": self.CAMERA_HEIGHT,
"fov": self.CAMERA_FOV,
"image": self._last_camera_image
},
"landed": self._landed,
"timestamp": round(self._step_count * 0.02, 4)
}
telemetry_msg = String()
telemetry_msg.data = json.dumps(telemetry)
self._telemetry_pub.publish(telemetry_msg)
def _quaternion_to_euler(self, x: float, y: float, z: float, w: float) -> tuple:
sinr_cosp = 2 * (w * x + y * z)
cosr_cosp = 1 - 2 * (x * x + y * y)
roll = math.atan2(sinr_cosp, cosr_cosp)
sinp = 2 * (w * y - z * x)
if abs(sinp) >= 1:
pitch = math.copysign(math.pi / 2, sinp)
else:
pitch = math.asin(sinp)
siny_cosp = 2 * (w * z + x * y)
cosy_cosp = 1 - 2 * (y * y + z * z)
yaw = math.atan2(siny_cosp, cosy_cosp)
return roll, pitch, yaw
def main(args=None):
print("\n" + "=" * 60)
print(" GAZEBO BRIDGE (GPS-DENIED + CAMERA)")
print("=" * 60 + "\n")
rclpy.init(args=args)
bridge_node = None
try:
bridge_node = GazeboBridge()
rclpy.spin(bridge_node)
except KeyboardInterrupt:
print('\nShutting down...')
finally:
if bridge_node is not None:
bridge_node.destroy_node()
if rclpy.ok():
rclpy.shutdown()
if __name__ == '__main__':
main()

27
requirements.txt Normal file
View File

@@ -0,0 +1,27 @@
# =============================================================================
# Drone Landing Competition - Python Dependencies
# =============================================================================
#
# Install with: pip install -r requirements.txt
#
# Note: ROS 2 packages (rclpy, geometry_msgs, std_msgs) are installed via
# the ROS 2 distribution and are not available on PyPI. See README.md
# or use the setup scripts in setup/ folder.
# =============================================================================
# -----------------------------------------------------------------------------
# Core Simulation
# -----------------------------------------------------------------------------
pybullet>=3.2.5 # Physics simulation engine
# -----------------------------------------------------------------------------
# Build Tools (optional - for creating standalone executables)
# -----------------------------------------------------------------------------
pyinstaller>=6.0.0 # Executable bundler
# -----------------------------------------------------------------------------
# Development (optional)
# -----------------------------------------------------------------------------
# pytest>=7.0.0 # Testing framework
# black>=23.0.0 # Code formatter
# flake8>=6.0.0 # Linter

221
ros_bridge.py Normal file
View File

@@ -0,0 +1,221 @@
#!/usr/bin/env python3
"""
ROS 2 Bridge for Drone Simulation - Connects PyBullet simulator to ROS 2.
Usage: python ros_bridge.py --host <SIMULATOR_IP> --port <PORT>
"""
import argparse
import json
import socket
import threading
from typing import Optional
import rclpy
from rclpy.node import Node
from rclpy.executors import MultiThreadedExecutor
from geometry_msgs.msg import Twist
from std_msgs.msg import String
class ROS2SimulatorBridge(Node):
"""Bridges ROS 2 topics to UDP-based simulator."""
DEFAULT_SIMULATOR_HOST = '127.0.0.1'
DEFAULT_SIMULATOR_PORT = 5555
SOCKET_TIMEOUT = 0.1
RECEIVE_BUFFER_SIZE = 4096
CMD_VEL_TOPIC = '/cmd_vel'
TELEMETRY_TOPIC = '/drone/telemetry'
def __init__(self, simulator_host: str = None, simulator_port: int = None):
super().__init__('ros2_simulator_bridge')
self._simulator_host = simulator_host or self.DEFAULT_SIMULATOR_HOST
self._simulator_port = simulator_port or self.DEFAULT_SIMULATOR_PORT
self._telemetry_port = self._simulator_port + 1
self.get_logger().info('=' * 60)
self.get_logger().info('ROS 2 Simulator Bridge Starting...')
self.get_logger().info(f' Simulator Host: {self._simulator_host}')
self.get_logger().info(f' Simulator Port: {self._simulator_port}')
self.get_logger().info(f' Telemetry Port: {self._telemetry_port}')
self.get_logger().info('=' * 60)
self._socket: Optional[socket.socket] = None
self._socket_lock = threading.Lock()
self._init_socket()
self._cmd_vel_sub = self.create_subscription(
Twist, self.CMD_VEL_TOPIC, self._cmd_vel_callback, 10
)
self.get_logger().info(f' Subscribed to: {self.CMD_VEL_TOPIC}')
self._telemetry_pub = self.create_publisher(
String, self.TELEMETRY_TOPIC, 10
)
self.get_logger().info(f' Publishing to: {self.TELEMETRY_TOPIC}')
self._running = True
self._receiver_thread = threading.Thread(
target=self._telemetry_receiver_loop,
name='TelemetryReceiver',
daemon=True
)
self._receiver_thread.start()
self.get_logger().info(' Telemetry receiver thread started')
self.get_logger().info('ROS 2 Simulator Bridge Ready!')
def _init_socket(self) -> None:
try:
self._socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
self._socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
self._socket.settimeout(self.SOCKET_TIMEOUT)
self._socket.bind(('0.0.0.0', self._telemetry_port))
self.get_logger().info(
f' UDP socket initialized (listening on port {self._telemetry_port})'
)
except socket.error as e:
self.get_logger().error(f'Failed to initialize socket: {e}')
raise
def _cmd_vel_callback(self, msg: Twist) -> None:
command = {
'thrust': msg.linear.z,
'yaw': msg.angular.z,
'pitch': msg.linear.x,
'roll': msg.linear.y
}
self._send_command_to_simulator(command)
def _send_command_to_simulator(self, command: dict) -> bool:
try:
json_data = json.dumps(command).encode('utf-8')
with self._socket_lock:
if self._socket is None:
return False
self._socket.sendto(
json_data,
(self._simulator_host, self._simulator_port)
)
return True
except socket.error as e:
self.get_logger().warning(f'Failed to send command: {e}')
return False
except json.JSONEncodeError as e:
self.get_logger().error(f'JSON encoding error: {e}')
return False
def _telemetry_receiver_loop(self) -> None:
self.get_logger().info('Telemetry receiver loop started')
while self._running and rclpy.ok():
try:
with self._socket_lock:
if self._socket is None:
continue
sock = self._socket
data, addr = sock.recvfrom(self.RECEIVE_BUFFER_SIZE)
if data:
json_str = data.decode('utf-8')
try:
json.loads(json_str)
except json.JSONDecodeError as e:
self.get_logger().warning(
f'Received invalid JSON from simulator: {e}'
)
continue
msg = String()
msg.data = json_str
self._telemetry_pub.publish(msg)
except socket.timeout:
continue
except socket.error as e:
if self._running:
self.get_logger().warning(f'Socket receive error: {e}')
continue
except Exception as e:
self.get_logger().error(f'Unexpected error in receiver loop: {e}')
continue
def shutdown(self) -> None:
self.get_logger().info('Shutting down ROS 2 Simulator Bridge...')
self._running = False
if self._receiver_thread.is_alive():
self._receiver_thread.join(timeout=1.0)
with self._socket_lock:
if self._socket is not None:
try:
self._socket.close()
except socket.error:
pass
self._socket = None
self.get_logger().info('Bridge shutdown complete')
def parse_args():
parser = argparse.ArgumentParser(
description='ROS 2 Bridge for Drone Simulation',
formatter_class=argparse.RawDescriptionHelpFormatter,
epilog="""
Examples:
python ros_bridge.py
python ros_bridge.py --host 192.168.1.100
python ros_bridge.py --host 192.168.1.100 --port 5555
"""
)
parser.add_argument(
'--host', '-H', type=str, default='127.0.0.1',
help='IP address of the simulator (default: 127.0.0.1)'
)
parser.add_argument(
'--port', '-p', type=int, default=5555,
help='UDP port of the simulator (default: 5555)'
)
args, _ = parser.parse_known_args()
return args
def main(args=None):
cli_args = parse_args()
print(f"\n{'='*60}")
print(f" Connecting to simulator at {cli_args.host}:{cli_args.port}")
print(f"{'='*60}\n")
rclpy.init(args=args)
bridge_node = None
try:
bridge_node = ROS2SimulatorBridge(
simulator_host=cli_args.host,
simulator_port=cli_args.port
)
executor = MultiThreadedExecutor(num_threads=2)
executor.add_node(bridge_node)
try:
executor.spin()
finally:
executor.shutdown()
except KeyboardInterrupt:
print('\nKeyboard interrupt received, shutting down...')
except Exception as e:
print(f'Error: {e}')
finally:
if bridge_node is not None:
bridge_node.shutdown()
bridge_node.destroy_node()
if rclpy.ok():
rclpy.shutdown()
if __name__ == '__main__':
main()

246
rover_controller.py Normal file
View File

@@ -0,0 +1,246 @@
#!/usr/bin/env python3
"""
Rover Controller - Controls the moving landing pad.
Usage: python rover_controller.py --pattern circular --speed 0.5 --amplitude 2.0
"""
import argparse
import math
import random
from enum import Enum
from typing import Tuple
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist, Point
from std_msgs.msg import String
import json
class MovementPattern(Enum):
STATIONARY = "stationary"
LINEAR = "linear"
CIRCULAR = "circular"
RANDOM = "random"
SQUARE = "square"
class RoverController(Node):
"""Controls the rover movement."""
DEFAULT_PATTERN = MovementPattern.STATIONARY
DEFAULT_SPEED = 0.5
DEFAULT_AMPLITUDE = 2.0
CONTROL_RATE = 50.0
MAX_X = 5.0
MAX_Y = 5.0
def __init__(
self,
pattern: MovementPattern = DEFAULT_PATTERN,
speed: float = DEFAULT_SPEED,
amplitude: float = DEFAULT_AMPLITUDE
):
super().__init__('rover_controller')
self._pattern = pattern
self._speed = speed
self._amplitude = amplitude
self._time = 0.0
self._position = Point()
self._position.x = 0.0
self._position.y = 0.0
self._position.z = 0.15
self._target_x = 0.0
self._target_y = 0.0
self._random_timer = 0.0
self._square_segment = 0
self.get_logger().info('=' * 50)
self.get_logger().info('Rover Controller Starting...')
self.get_logger().info(f' Pattern: {pattern.value}')
self.get_logger().info(f' Speed: {speed} m/s')
self.get_logger().info(f' Amplitude: {amplitude} m')
self.get_logger().info('=' * 50)
self._cmd_vel_pub = self.create_publisher(Twist, '/rover/cmd_vel', 10)
self._position_pub = self.create_publisher(Point, '/rover/position', 10)
self._telemetry_pub = self.create_publisher(String, '/rover/telemetry', 10)
self.get_logger().info(' Publishing to: /rover/cmd_vel, /rover/position, /rover/telemetry')
control_period = 1.0 / self.CONTROL_RATE
self._control_timer = self.create_timer(control_period, self._control_loop)
self.get_logger().info('Rover Controller Ready!')
def _control_loop(self) -> None:
dt = 1.0 / self.CONTROL_RATE
self._time += dt
vel_x, vel_y = self._calculate_velocity()
self._position.x += vel_x * dt
self._position.y += vel_y * dt
self._position.x = max(-self.MAX_X, min(self.MAX_X, self._position.x))
self._position.y = max(-self.MAX_Y, min(self.MAX_Y, self._position.y))
cmd = Twist()
cmd.linear.x = vel_x
cmd.linear.y = vel_y
self._cmd_vel_pub.publish(cmd)
self._position_pub.publish(self._position)
telemetry = {
"position": {
"x": round(self._position.x, 4),
"y": round(self._position.y, 4),
"z": round(self._position.z, 4)
},
"velocity": {
"x": round(vel_x, 4),
"y": round(vel_y, 4),
"z": 0.0
},
"pattern": self._pattern.value,
"timestamp": round(self._time, 4)
}
telemetry_msg = String()
telemetry_msg.data = json.dumps(telemetry)
self._telemetry_pub.publish(telemetry_msg)
def _calculate_velocity(self) -> Tuple[float, float]:
if self._pattern == MovementPattern.STATIONARY:
return self._pattern_stationary()
elif self._pattern == MovementPattern.LINEAR:
return self._pattern_linear()
elif self._pattern == MovementPattern.CIRCULAR:
return self._pattern_circular()
elif self._pattern == MovementPattern.RANDOM:
return self._pattern_random()
elif self._pattern == MovementPattern.SQUARE:
return self._pattern_square()
return (0.0, 0.0)
def _pattern_stationary(self) -> Tuple[float, float]:
kp = 1.0
return (-kp * self._position.x, -kp * self._position.y)
def _pattern_linear(self) -> Tuple[float, float]:
omega = self._speed / self._amplitude
target_x = self._amplitude * math.sin(omega * self._time)
kp = 2.0
vel_x = kp * (target_x - self._position.x)
vel_x = max(-self._speed, min(self._speed, vel_x))
return (vel_x, 0.0)
def _pattern_circular(self) -> Tuple[float, float]:
omega = self._speed / self._amplitude
target_x = self._amplitude * math.cos(omega * self._time)
target_y = self._amplitude * math.sin(omega * self._time)
kp = 2.0
vel_x = kp * (target_x - self._position.x)
vel_y = kp * (target_y - self._position.y)
speed = math.sqrt(vel_x**2 + vel_y**2)
if speed > self._speed:
vel_x = vel_x / speed * self._speed
vel_y = vel_y / speed * self._speed
return (vel_x, vel_y)
def _pattern_random(self) -> Tuple[float, float]:
dt = 1.0 / self.CONTROL_RATE
self._random_timer += dt
if self._random_timer >= 3.0:
self._random_timer = 0.0
self._target_x = random.uniform(-self._amplitude, self._amplitude)
self._target_y = random.uniform(-self._amplitude, self._amplitude)
kp = 1.0
vel_x = kp * (self._target_x - self._position.x)
vel_y = kp * (self._target_y - self._position.y)
speed = math.sqrt(vel_x**2 + vel_y**2)
if speed > self._speed:
vel_x = vel_x / speed * self._speed
vel_y = vel_y / speed * self._speed
return (vel_x, vel_y)
def _pattern_square(self) -> Tuple[float, float]:
corners = [
(self._amplitude, self._amplitude),
(-self._amplitude, self._amplitude),
(-self._amplitude, -self._amplitude),
(self._amplitude, -self._amplitude),
]
target = corners[self._square_segment % 4]
dx = target[0] - self._position.x
dy = target[1] - self._position.y
dist = math.sqrt(dx**2 + dy**2)
if dist < 0.1:
self._square_segment += 1
target = corners[self._square_segment % 4]
dx = target[0] - self._position.x
dy = target[1] - self._position.y
dist = math.sqrt(dx**2 + dy**2)
if dist > 0.01:
return (self._speed * dx / dist, self._speed * dy / dist)
return (0.0, 0.0)
def parse_args():
parser = argparse.ArgumentParser(
description='Rover Controller - Moving Landing Platform',
formatter_class=argparse.RawDescriptionHelpFormatter,
epilog="""
Movement Patterns:
stationary - Rover stays at origin (easiest)
linear - Back and forth along X axis
circular - Circular path around origin
random - Random movement within bounds
square - Square pattern around origin
"""
)
parser.add_argument(
'--pattern', '-p', type=str,
choices=[p.value for p in MovementPattern],
default='stationary', help='Movement pattern (default: stationary)'
)
parser.add_argument(
'--speed', '-s', type=float, default=0.5,
help='Movement speed in m/s (default: 0.5)'
)
parser.add_argument(
'--amplitude', '-a', type=float, default=2.0,
help='Movement amplitude in meters (default: 2.0)'
)
args, _ = parser.parse_known_args()
return args
def main(args=None):
cli_args = parse_args()
print(f"\n{'='*60}")
print(f" ROVER CONTROLLER - {cli_args.pattern.upper()}")
print(f"{'='*60}\n")
rclpy.init(args=args)
controller = None
try:
pattern = MovementPattern(cli_args.pattern)
controller = RoverController(
pattern=pattern,
speed=cli_args.speed,
amplitude=cli_args.amplitude
)
rclpy.spin(controller)
except KeyboardInterrupt:
print('\nShutting down...')
finally:
if controller:
controller.destroy_node()
if rclpy.ok():
rclpy.shutdown()
if __name__ == '__main__':
main()

189
setup/install_macos.sh Executable file
View File

@@ -0,0 +1,189 @@
#!/bin/bash
# =============================================================================
# Drone Simulation - macOS Installation Script
# =============================================================================
# Installs ROS 2 Humble via robostack (conda), PyBullet, and dependencies
# Uses a conda environment for all packages
#
# Usage:
# chmod +x install_macos.sh
# ./install_macos.sh
#
# Tested on: macOS Ventura, Sonoma (Apple Silicon & Intel)
# =============================================================================
set -e # Exit on error
echo "=============================================="
echo " Drone Simulation - macOS Installation"
echo "=============================================="
# Get the script directory and project root
SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
PROJECT_ROOT="$(dirname "$SCRIPT_DIR")"
ENV_NAME="drone_simulation"
echo "[INFO] Project root: $PROJECT_ROOT"
# Detect architecture
ARCH=$(uname -m)
echo "[INFO] Detected architecture: $ARCH"
# -----------------------------------------------------------------------------
# Step 1: Install Homebrew (if not present)
# -----------------------------------------------------------------------------
echo ""
echo "[STEP 1/5] Checking Homebrew..."
if ! command -v brew &> /dev/null; then
echo "[INFO] Installing Homebrew..."
/bin/bash -c "$(curl -fsSL https://raw.githubusercontent.com/Homebrew/install/HEAD/install.sh)"
# Add Homebrew to PATH for Apple Silicon
if [[ "$ARCH" == "arm64" ]]; then
echo 'eval "$(/opt/homebrew/bin/brew shellenv)"' >> ~/.zprofile
eval "$(/opt/homebrew/bin/brew shellenv)"
fi
else
echo "[INFO] Homebrew already installed"
fi
# Update Homebrew
brew update
# -----------------------------------------------------------------------------
# Step 2: Install Miniforge (conda)
# -----------------------------------------------------------------------------
echo ""
echo "[STEP 2/5] Installing Miniforge (conda)..."
if ! command -v conda &> /dev/null; then
echo "[INFO] Downloading Miniforge..."
if [[ "$ARCH" == "arm64" ]]; then
curl -L -O "https://github.com/conda-forge/miniforge/releases/latest/download/Miniforge3-MacOSX-arm64.sh"
bash Miniforge3-MacOSX-arm64.sh -b -p $HOME/miniforge3
rm Miniforge3-MacOSX-arm64.sh
else
curl -L -O "https://github.com/conda-forge/miniforge/releases/latest/download/Miniforge3-MacOSX-x86_64.sh"
bash Miniforge3-MacOSX-x86_64.sh -b -p $HOME/miniforge3
rm Miniforge3-MacOSX-x86_64.sh
fi
# Initialize conda
$HOME/miniforge3/bin/conda init zsh bash
# Source conda for this session
source $HOME/miniforge3/etc/profile.d/conda.sh
else
echo "[INFO] Conda already installed"
# Ensure conda is available in this session
source $(conda info --base)/etc/profile.d/conda.sh
fi
# -----------------------------------------------------------------------------
# Step 3: Create conda environment with ROS 2
# -----------------------------------------------------------------------------
echo ""
echo "[STEP 3/5] Creating conda environment with ROS 2..."
# Remove existing environment if present
conda deactivate 2>/dev/null || true
conda env remove -n $ENV_NAME 2>/dev/null || true
# Create new environment
conda create -n $ENV_NAME python=3.11 -y
# Activate environment
conda activate $ENV_NAME
# Add robostack channels
conda config --env --add channels conda-forge
conda config --env --add channels robostack-staging
echo "[INFO] Installing ROS 2 Humble via robostack (this may take a while)..."
conda install ros-humble-desktop ros-humble-geometry-msgs ros-humble-std-msgs -y
# -----------------------------------------------------------------------------
# Step 4: Install Python Dependencies
# -----------------------------------------------------------------------------
echo ""
echo "[STEP 4/5] Installing Python dependencies..."
pip install pybullet pyinstaller
# -----------------------------------------------------------------------------
# Step 5: Create Activation Script
# -----------------------------------------------------------------------------
echo ""
echo "[STEP 5/5] Creating activation script..."
ACTIVATE_SCRIPT="$PROJECT_ROOT/activate.sh"
cat > "$ACTIVATE_SCRIPT" << 'EOF'
#!/bin/bash
# =============================================================================
# Drone Competition - Environment Activation Script (macOS)
# =============================================================================
# This script activates the conda environment with ROS 2.
#
# Usage:
# source activate.sh
# =============================================================================
# Get the directory where this script is located
SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
# Initialize conda
if [ -f "$HOME/miniforge3/etc/profile.d/conda.sh" ]; then
source "$HOME/miniforge3/etc/profile.d/conda.sh"
elif [ -f "$(conda info --base)/etc/profile.d/conda.sh" ]; then
source "$(conda info --base)/etc/profile.d/conda.sh"
fi
# Activate conda environment
conda activate drone_competition
echo "[OK] Conda environment 'drone_competition' activated"
echo ""
echo "Environment ready! You can now run:"
echo " python simulation_host.py"
echo " python ros_bridge.py"
echo ""
EOF
chmod +x "$ACTIVATE_SCRIPT"
echo "[INFO] Created activation script: $ACTIVATE_SCRIPT"
# -----------------------------------------------------------------------------
# Verification
# -----------------------------------------------------------------------------
echo ""
echo "=============================================="
echo " Installation Complete!"
echo "=============================================="
echo ""
echo "Verifying installation..."
echo ""
echo -n " ROS 2: "
ros2 --version 2>/dev/null && echo "" || echo "FAILED"
echo -n " PyBullet: "
python3 -c "import pybullet; print('OK')" 2>/dev/null || echo "FAILED"
echo -n " rclpy: "
python3 -c "import rclpy; print('OK')" 2>/dev/null || echo "FAILED"
echo -n " PyInstaller: "
python3 -c "import PyInstaller; print('OK')" 2>/dev/null || echo "FAILED"
echo ""
echo "=============================================="
echo " IMPORTANT: Activate the environment first!"
echo "=============================================="
echo ""
echo "Before running any scripts, activate the environment:"
echo " source $ACTIVATE_SCRIPT"
echo ""
echo "Then run the simulation:"
echo " python simulation_host.py"
echo ""

215
setup/install_ubuntu.sh Executable file
View File

@@ -0,0 +1,215 @@
#!/bin/bash
# =============================================================================
# Drone Simulation - Ubuntu/Debian Installation Script
# =============================================================================
# Installs ROS 2 Humble/Jazzy, PyBullet, and all required dependencies
# Uses a Python virtual environment for pip packages (PEP 668 compliant)
#
# Usage:
# chmod +x install_ubuntu.sh
# ./install_ubuntu.sh
#
# Tested on: Ubuntu 22.04 LTS, Ubuntu 24.04 LTS
# =============================================================================
set -e # Exit on error
echo "=============================================="
echo " Drone Simulation - Ubuntu Installation"
echo "=============================================="
# Get the script directory and project root
SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
PROJECT_ROOT="$(dirname "$SCRIPT_DIR")"
VENV_DIR="$PROJECT_ROOT/venv"
echo "[INFO] Project root: $PROJECT_ROOT"
echo "[INFO] Virtual environment: $VENV_DIR"
# Detect Ubuntu version
UBUNTU_VERSION=$(lsb_release -rs)
echo "[INFO] Detected Ubuntu version: $UBUNTU_VERSION"
# Determine ROS 2 distribution based on Ubuntu version
if [[ "$UBUNTU_VERSION" == "22.04" ]]; then
ROS_DISTRO="humble"
elif [[ "$UBUNTU_VERSION" == "24.04" ]]; then
ROS_DISTRO="jazzy"
else
echo "[WARN] Ubuntu $UBUNTU_VERSION may not be officially supported."
echo " Attempting to install ROS 2 Humble..."
ROS_DISTRO="humble"
fi
echo "[INFO] Will install ROS 2 $ROS_DISTRO"
# -----------------------------------------------------------------------------
# Step 1: Set Locale
# -----------------------------------------------------------------------------
echo ""
echo "[STEP 1/7] Setting locale..."
sudo apt update && sudo apt install -y locales
sudo locale-gen en_US en_US.UTF-8
sudo update-locale LC_ALL=en_US.UTF-8 LANG=en_US.UTF-8
export LANG=en_US.UTF-8
# -----------------------------------------------------------------------------
# Step 2: Add ROS 2 Repository
# -----------------------------------------------------------------------------
echo ""
echo "[STEP 2/7] Adding ROS 2 apt repository..."
sudo apt install -y software-properties-common
sudo add-apt-repository -y universe
sudo apt update && sudo apt install -y curl
sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key \
-o /usr/share/keyrings/ros-archive-keyring.gpg
echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" \
| sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null
# -----------------------------------------------------------------------------
# Step 3: Install ROS 2
# -----------------------------------------------------------------------------
echo ""
echo "[STEP 3/8] Installing ROS 2 $ROS_DISTRO..."
sudo apt update
sudo apt install -y ros-${ROS_DISTRO}-desktop
# Install development tools
sudo apt install -y python3-colcon-common-extensions python3-rosdep
# Install Gazebo and ROS-Gazebo bridge
echo "[INFO] Installing Gazebo and ros_gz_bridge..."
sudo apt install -y ros-${ROS_DISTRO}-ros-gz ros-${ROS_DISTRO}-ros-gz-bridge
# -----------------------------------------------------------------------------
# Step 4: Initialize rosdep
# -----------------------------------------------------------------------------
echo ""
echo "[STEP 4/8] Initializing rosdep..."
if [ ! -f /etc/ros/rosdep/sources.list.d/20-default.list ]; then
sudo rosdep init
fi
rosdep update
# -----------------------------------------------------------------------------
# Step 5: Create Python Virtual Environment
# -----------------------------------------------------------------------------
echo ""
echo "[STEP 5/8] Creating Python virtual environment..."
# Install venv package if not present
sudo apt install -y python3-venv python3-full
# Create virtual environment with access to system site-packages
# This allows access to ROS 2 packages installed via apt
python3 -m venv "$VENV_DIR" --system-site-packages
echo "[INFO] Virtual environment created at: $VENV_DIR"
# -----------------------------------------------------------------------------
# Step 6: Install Python Dependencies in venv
# -----------------------------------------------------------------------------
echo ""
echo "[STEP 6/8] Installing Python dependencies in virtual environment..."
# Activate venv and install packages
source "$VENV_DIR/bin/activate"
# Upgrade pip
pip install --upgrade pip
# Install from requirements.txt if it exists
if [ -f "$PROJECT_ROOT/requirements.txt" ]; then
pip install -r "$PROJECT_ROOT/requirements.txt"
else
pip install pybullet pyinstaller
fi
echo "[INFO] Python packages installed in venv"
# -----------------------------------------------------------------------------
# Step 7: Create Activation Script
# -----------------------------------------------------------------------------
echo ""
echo "[STEP 7/8] Creating activation script..."
ACTIVATE_SCRIPT="$PROJECT_ROOT/activate.sh"
cat > "$ACTIVATE_SCRIPT" << EOF
#!/bin/bash
# =============================================================================
# Drone Competition - Environment Activation Script
# =============================================================================
# This script activates both ROS 2 and the Python virtual environment.
#
# Usage:
# source activate.sh
# =============================================================================
# Get the directory where this script is located
SCRIPT_DIR="\$(cd "\$(dirname "\${BASH_SOURCE[0]}")" && pwd)"
# Source ROS 2
source /opt/ros/${ROS_DISTRO}/setup.bash
echo "[OK] ROS 2 ${ROS_DISTRO} sourced"
# Activate Python virtual environment
source "\$SCRIPT_DIR/venv/bin/activate"
echo "[OK] Python venv activated"
echo ""
echo "Environment ready! You can now run:"
echo " python simulation_host.py # PyBullet"
echo " python gazebo_bridge.py # Gazebo"
echo " python ros_bridge.py"
echo ""
EOF
chmod +x "$ACTIVATE_SCRIPT"
echo "[INFO] Created activation script: $ACTIVATE_SCRIPT"
# -----------------------------------------------------------------------------
# Verification
# -----------------------------------------------------------------------------
echo ""
echo "=============================================="
echo " Installation Complete!"
echo "=============================================="
echo ""
echo "Verifying installation..."
echo ""
# Ensure we're in venv
source "$VENV_DIR/bin/activate"
source /opt/ros/${ROS_DISTRO}/setup.bash
echo -n " ROS 2: "
ros2 --version 2>/dev/null && echo "" || echo "FAILED"
echo -n " PyBullet: "
python3 -c "import pybullet; print('OK')" 2>/dev/null || echo "FAILED"
echo -n " rclpy: "
python3 -c "import rclpy; print('OK')" 2>/dev/null || echo "FAILED"
echo -n " geometry_msgs: "
python3 -c "from geometry_msgs.msg import Twist; print('OK')" 2>/dev/null || echo "FAILED"
echo -n " std_msgs: "
python3 -c "from std_msgs.msg import String; print('OK')" 2>/dev/null || echo "FAILED"
echo -n " PyInstaller: "
python3 -c "import PyInstaller; print('OK')" 2>/dev/null || echo "FAILED"
echo ""
echo "=============================================="
echo " IMPORTANT: Activate the environment first!"
echo "=============================================="
echo ""
echo "Before running any scripts, activate the environment:"
echo " source $ACTIVATE_SCRIPT"
echo ""
echo "Then run the simulation:"
echo " python simulation_host.py"
echo ""

232
setup/install_windows.ps1 Normal file
View File

@@ -0,0 +1,232 @@
# =============================================================================
# Drone Simulation - Windows Installation Script (PowerShell)
# =============================================================================
# Installs ROS 2 Humble, PyBullet, and all required dependencies
# Uses a Python virtual environment for pip packages
#
# Usage:
# 1. Open PowerShell as Administrator
# 2. Run: Set-ExecutionPolicy RemoteSigned -Scope CurrentUser
# 3. Run: .\install_windows.ps1
#
# Tested on: Windows 10/11
# =============================================================================
Write-Host "==============================================" -ForegroundColor Cyan
Write-Host " Drone Simulation - Windows Installation" -ForegroundColor Cyan
Write-Host "==============================================" -ForegroundColor Cyan
Write-Host ""
# Get script and project paths
$ScriptDir = Split-Path -Parent $MyInvocation.MyCommand.Path
$ProjectRoot = Split-Path -Parent $ScriptDir
$VenvDir = Join-Path $ProjectRoot "venv"
Write-Host "[INFO] Project root: $ProjectRoot" -ForegroundColor Gray
Write-Host "[INFO] Virtual environment: $VenvDir" -ForegroundColor Gray
# Check for Administrator privileges
$isAdmin = ([Security.Principal.WindowsPrincipal] [Security.Principal.WindowsIdentity]::GetCurrent()).IsInRole([Security.Principal.WindowsBuiltInRole]::Administrator)
if (-not $isAdmin) {
Write-Host "[WARN] Not running as Administrator. Some installations may fail." -ForegroundColor Yellow
Write-Host " Consider running PowerShell as Administrator." -ForegroundColor Yellow
Write-Host ""
}
# -----------------------------------------------------------------------------
# Step 1: Install Chocolatey (Package Manager)
# -----------------------------------------------------------------------------
Write-Host "[STEP 1/7] Checking Chocolatey..." -ForegroundColor Green
if (-not (Get-Command choco -ErrorAction SilentlyContinue)) {
Write-Host "[INFO] Installing Chocolatey..." -ForegroundColor Yellow
Set-ExecutionPolicy Bypass -Scope Process -Force
[System.Net.ServicePointManager]::SecurityProtocol = [System.Net.ServicePointManager]::SecurityProtocol -bor 3072
Invoke-Expression ((New-Object System.Net.WebClient).DownloadString('https://community.chocolatey.org/install.ps1'))
# Refresh environment
$env:Path = [System.Environment]::GetEnvironmentVariable("Path","Machine") + ";" + [System.Environment]::GetEnvironmentVariable("Path","User")
} else {
Write-Host "[INFO] Chocolatey already installed" -ForegroundColor Green
}
# -----------------------------------------------------------------------------
# Step 2: Install Python
# -----------------------------------------------------------------------------
Write-Host ""
Write-Host "[STEP 2/7] Installing Python..." -ForegroundColor Green
if (-not (Get-Command python -ErrorAction SilentlyContinue)) {
choco install python311 -y
# Refresh environment
$env:Path = [System.Environment]::GetEnvironmentVariable("Path","Machine") + ";" + [System.Environment]::GetEnvironmentVariable("Path","User")
} else {
Write-Host "[INFO] Python already installed" -ForegroundColor Green
}
# -----------------------------------------------------------------------------
# Step 3: Install Visual C++ Build Tools
# -----------------------------------------------------------------------------
Write-Host ""
Write-Host "[STEP 3/7] Installing Visual C++ Build Tools..." -ForegroundColor Green
choco install visualstudio2022-workload-vctools -y
# -----------------------------------------------------------------------------
# Step 4: Download and Install ROS 2
# -----------------------------------------------------------------------------
Write-Host ""
Write-Host "[STEP 4/7] Installing ROS 2 Humble..." -ForegroundColor Green
$ros2Path = "C:\dev\ros2_humble"
if (-not (Test-Path $ros2Path)) {
Write-Host "[INFO] Downloading ROS 2 Humble..." -ForegroundColor Yellow
# Create installation directory
New-Item -ItemType Directory -Force -Path "C:\dev" | Out-Null
# Download ROS 2 binary
$ros2Url = "https://github.com/ros2/ros2/releases/download/release-humble-20240523/ros2-humble-20240523-windows-release-amd64.zip"
$ros2Zip = "$env:TEMP\ros2-humble.zip"
Write-Host "[INFO] This may take a while (1-2 GB download)..." -ForegroundColor Yellow
Invoke-WebRequest -Uri $ros2Url -OutFile $ros2Zip
Write-Host "[INFO] Extracting ROS 2..." -ForegroundColor Yellow
Expand-Archive -Path $ros2Zip -DestinationPath "C:\dev" -Force
Remove-Item $ros2Zip
Write-Host "[INFO] ROS 2 installed to $ros2Path" -ForegroundColor Green
} else {
Write-Host "[INFO] ROS 2 already installed at $ros2Path" -ForegroundColor Green
}
# -----------------------------------------------------------------------------
# Step 5: Create Python Virtual Environment
# -----------------------------------------------------------------------------
Write-Host ""
Write-Host "[STEP 5/7] Creating Python virtual environment..." -ForegroundColor Green
# Remove existing venv if present
if (Test-Path $VenvDir) {
Remove-Item -Recurse -Force $VenvDir
}
# Create virtual environment
python -m venv $VenvDir
Write-Host "[INFO] Virtual environment created at: $VenvDir" -ForegroundColor Green
# -----------------------------------------------------------------------------
# Step 6: Install Python Dependencies in venv
# -----------------------------------------------------------------------------
Write-Host ""
Write-Host "[STEP 6/7] Installing Python dependencies in virtual environment..." -ForegroundColor Green
# Activate venv and install packages
& "$VenvDir\Scripts\Activate.ps1"
python -m pip install --upgrade pip
$requirementsFile = Join-Path $ProjectRoot "requirements.txt"
if (Test-Path $requirementsFile) {
pip install -r $requirementsFile
} else {
pip install pybullet pyinstaller
}
Write-Host "[INFO] Python packages installed in venv" -ForegroundColor Green
# -----------------------------------------------------------------------------
# Step 7: Create Activation Script
# -----------------------------------------------------------------------------
Write-Host ""
Write-Host "[STEP 7/7] Creating activation scripts..." -ForegroundColor Green
# Create batch file for cmd.exe
$activateBat = Join-Path $ProjectRoot "activate.bat"
@"
@echo off
REM =============================================================================
REM Drone Competition - Environment Activation Script (Windows CMD)
REM =============================================================================
REM Usage: activate.bat
REM =============================================================================
echo Activating ROS 2 Humble...
call C:\dev\ros2_humble\local_setup.bat
echo Activating Python virtual environment...
call "%~dp0venv\Scripts\activate.bat"
echo.
echo [OK] Environment ready! You can now run:
echo python simulation_host.py
echo python ros_bridge.py
echo.
"@ | Out-File -FilePath $activateBat -Encoding ASCII
# Create PowerShell script
$activatePs1 = Join-Path $ProjectRoot "activate.ps1"
@"
# =============================================================================
# Drone Competition - Environment Activation Script (Windows PowerShell)
# =============================================================================
# Usage: . .\activate.ps1
# =============================================================================
`$ScriptDir = Split-Path -Parent `$MyInvocation.MyCommand.Path
Write-Host "Activating ROS 2 Humble..." -ForegroundColor Yellow
& "C:\dev\ros2_humble\local_setup.ps1"
Write-Host "Activating Python virtual environment..." -ForegroundColor Yellow
& "`$ScriptDir\venv\Scripts\Activate.ps1"
Write-Host ""
Write-Host "[OK] Environment ready! You can now run:" -ForegroundColor Green
Write-Host " python simulation_host.py"
Write-Host " python ros_bridge.py"
Write-Host ""
"@ | Out-File -FilePath $activatePs1 -Encoding UTF8
Write-Host "[INFO] Created activation scripts:" -ForegroundColor Green
Write-Host " $activateBat (for CMD)" -ForegroundColor Gray
Write-Host " $activatePs1 (for PowerShell)" -ForegroundColor Gray
# -----------------------------------------------------------------------------
# Verification
# -----------------------------------------------------------------------------
Write-Host ""
Write-Host "==============================================" -ForegroundColor Cyan
Write-Host " Installation Complete!" -ForegroundColor Cyan
Write-Host "==============================================" -ForegroundColor Cyan
Write-Host ""
Write-Host "Verifying installation..." -ForegroundColor Yellow
Write-Host ""
try {
python -c "import pybullet; print(' PyBullet: OK')"
} catch {
Write-Host " PyBullet: FAILED" -ForegroundColor Red
}
try {
python -c "import PyInstaller; print(' PyInstaller: OK')"
} catch {
Write-Host " PyInstaller: FAILED" -ForegroundColor Red
}
Write-Host ""
Write-Host "==============================================" -ForegroundColor Cyan
Write-Host " IMPORTANT: Activate the environment first!" -ForegroundColor Cyan
Write-Host "==============================================" -ForegroundColor Cyan
Write-Host ""
Write-Host "Before running any scripts, activate the environment:" -ForegroundColor Yellow
Write-Host " CMD: $activateBat" -ForegroundColor White
Write-Host " PowerShell: . $activatePs1" -ForegroundColor White
Write-Host ""
Write-Host "Then run the simulation:" -ForegroundColor Yellow
Write-Host " python simulation_host.py" -ForegroundColor White
Write-Host ""

442
simulation_host.py Normal file
View File

@@ -0,0 +1,442 @@
#!/usr/bin/env python3
"""
Drone Simulation Host - PyBullet physics simulation (GPS-Denied).
Includes downward camera for drone vision.
Communicates via UDP on port 5555 for commands, 5556 for telemetry.
"""
import base64
import json
import math
import socket
import time
from typing import Dict, Any, Optional, Tuple
import numpy as np
import pybullet as p
import pybullet_data
class DroneSimulator:
"""PyBullet-based drone simulation with camera."""
UDP_HOST = '127.0.0.1'
UDP_PORT = 5555
UDP_BUFFER_SIZE = 65535
SOCKET_TIMEOUT = 0.001
TELEMETRY_PORT = 5556
PHYSICS_TIMESTEP = 1.0 / 240.0
GRAVITY = -9.81
TELEMETRY_INTERVAL = 10
DRONE_MASS = 1.0
DRONE_SIZE = (0.3, 0.3, 0.1)
DRONE_START_POS = (0.0, 0.0, 5.0)
THRUST_SCALE = 15.0
PITCH_TORQUE_SCALE = 2.0
ROLL_TORQUE_SCALE = 2.0
YAW_TORQUE_SCALE = 1.0
HOVER_THRUST = DRONE_MASS * abs(GRAVITY)
ROVER_SIZE = (1.0, 1.0, 0.3)
ROVER_POS = (0.0, 0.0, 0.15)
# Camera parameters
CAMERA_WIDTH = 320
CAMERA_HEIGHT = 240
CAMERA_FOV = 60.0
CAMERA_NEAR = 0.1
CAMERA_FAR = 20.0
CAMERA_INTERVAL = 5 # Capture every N telemetry frames
def __init__(self):
print("=" * 60)
print("Drone Simulation Host (GPS-Denied + Camera)")
print("=" * 60)
self._running = True
self._step_count = 0
self._camera_frame_count = 0
self._last_command: Dict[str, float] = {
'thrust': 0.0, 'pitch': 0.0, 'roll': 0.0, 'yaw': 0.0
}
self._rover_pos = list(self.ROVER_POS)
self._last_camera_image = None
self._init_physics()
self._init_objects()
self._init_network()
print("Simulation Ready! (GPS-Denied Mode)")
print(" Sensors: IMU, Altimeter, Camera")
print(f" Camera: {self.CAMERA_WIDTH}x{self.CAMERA_HEIGHT} @ {self.CAMERA_FOV}° FOV")
print(f" Listening on UDP port {self.UDP_PORT}")
print("=" * 60)
def _init_physics(self) -> None:
print("Initializing PyBullet physics engine...")
self._physics_client = p.connect(p.GUI)
p.setGravity(0, 0, self.GRAVITY)
p.setTimeStep(self.PHYSICS_TIMESTEP)
p.resetDebugVisualizerCamera(
cameraDistance=8.0,
cameraYaw=45,
cameraPitch=-30,
cameraTargetPosition=[0, 0, 1]
)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
print(" Physics engine initialized (240 Hz)")
def _init_objects(self) -> None:
print("Creating simulation objects...")
self._ground_id = p.loadURDF("plane.urdf")
print(" Ground plane loaded")
rover_collision = p.createCollisionShape(
p.GEOM_BOX,
halfExtents=[s/2 for s in self.ROVER_SIZE]
)
rover_visual = p.createVisualShape(
p.GEOM_BOX,
halfExtents=[s/2 for s in self.ROVER_SIZE],
rgbaColor=[0.2, 0.6, 0.2, 1.0]
)
self._rover_id = p.createMultiBody(
baseMass=0,
baseCollisionShapeIndex=rover_collision,
baseVisualShapeIndex=rover_visual,
basePosition=self.ROVER_POS
)
print(f" Rover created at position {self.ROVER_POS}")
self._create_landing_pad_marker()
drone_collision = p.createCollisionShape(
p.GEOM_BOX,
halfExtents=[s/2 for s in self.DRONE_SIZE]
)
drone_visual = p.createVisualShape(
p.GEOM_BOX,
halfExtents=[s/2 for s in self.DRONE_SIZE],
rgbaColor=[0.8, 0.2, 0.2, 1.0]
)
self._drone_id = p.createMultiBody(
baseMass=self.DRONE_MASS,
baseCollisionShapeIndex=drone_collision,
baseVisualShapeIndex=drone_visual,
basePosition=self.DRONE_START_POS
)
p.changeDynamics(
self._drone_id, -1,
linearDamping=0.1,
angularDamping=0.5
)
print(f" Drone created at position {self.DRONE_START_POS}")
def _create_landing_pad_marker(self) -> None:
marker_height = self.ROVER_POS[2] + self.ROVER_SIZE[2] / 2 + 0.01
h_size = 0.3
line_color = [1, 1, 1]
line_width = 3
p.addUserDebugLine(
[-h_size, 0, marker_height],
[h_size, 0, marker_height],
lineColorRGB=line_color,
lineWidth=line_width
)
p.addUserDebugLine(
[-h_size, -h_size, marker_height],
[-h_size, h_size, marker_height],
lineColorRGB=line_color,
lineWidth=line_width
)
p.addUserDebugLine(
[h_size, -h_size, marker_height],
[h_size, h_size, marker_height],
lineColorRGB=line_color,
lineWidth=line_width
)
def _init_network(self) -> None:
print("Initializing network...")
self._socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
self._socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
self._socket.settimeout(self.SOCKET_TIMEOUT)
self._socket.bind((self.UDP_HOST, self.UDP_PORT))
self._controller_addr: Optional[Tuple[str, int]] = None
print(f" UDP socket bound to {self.UDP_HOST}:{self.UDP_PORT}")
def run(self) -> None:
print("\nStarting simulation loop...")
print("Press Ctrl+C to exit\n")
try:
while self._running:
loop_start = time.time()
if not p.isConnected():
print("GUI window closed, exiting...")
break
self._receive_commands()
self._apply_drone_controls()
p.stepSimulation()
self._step_count += 1
if self._step_count % self.TELEMETRY_INTERVAL == 0:
self._send_telemetry()
elapsed = time.time() - loop_start
sleep_time = self.PHYSICS_TIMESTEP - elapsed
if sleep_time > 0:
time.sleep(sleep_time)
except KeyboardInterrupt:
print("\nKeyboard interrupt received")
finally:
self.shutdown()
def _receive_commands(self) -> None:
try:
data, addr = self._socket.recvfrom(self.UDP_BUFFER_SIZE)
self._controller_addr = (addr[0], self.TELEMETRY_PORT)
try:
command = json.loads(data.decode('utf-8'))
self._last_command = {
'thrust': float(command.get('thrust', 0.0)),
'pitch': float(command.get('pitch', 0.0)),
'roll': float(command.get('roll', 0.0)),
'yaw': float(command.get('yaw', 0.0))
}
except (json.JSONDecodeError, ValueError) as e:
print(f"Invalid command received: {e}")
except socket.timeout:
pass
except socket.error as e:
print(f"Socket error: {e}")
def _apply_drone_controls(self) -> None:
pos, orn = p.getBasePositionAndOrientation(self._drone_id)
rot_matrix = p.getMatrixFromQuaternion(orn)
local_up = [rot_matrix[2], rot_matrix[5], rot_matrix[8]]
thrust_command = self._last_command['thrust']
total_thrust = self.HOVER_THRUST + (thrust_command * self.THRUST_SCALE)
total_thrust = max(0, total_thrust)
thrust_force = [
local_up[0] * total_thrust,
local_up[1] * total_thrust,
local_up[2] * total_thrust
]
p.applyExternalForce(
self._drone_id, -1,
forceObj=thrust_force,
posObj=pos,
flags=p.WORLD_FRAME
)
pitch_torque = self._last_command['pitch'] * self.PITCH_TORQUE_SCALE
roll_torque = self._last_command['roll'] * self.ROLL_TORQUE_SCALE
yaw_torque = self._last_command['yaw'] * self.YAW_TORQUE_SCALE
p.applyExternalTorque(
self._drone_id, -1,
torqueObj=[pitch_torque, roll_torque, yaw_torque],
flags=p.LINK_FRAME
)
def _capture_camera_image(self) -> Optional[str]:
"""Capture image from drone's downward-facing camera. Returns base64 encoded JPEG."""
pos, orn = p.getBasePositionAndOrientation(self._drone_id)
euler = p.getEulerFromQuaternion(orn)
target_pos = [pos[0], pos[1], 0]
view_matrix = p.computeViewMatrix(
cameraEyePosition=pos,
cameraTargetPosition=target_pos,
cameraUpVector=[math.cos(euler[2]), math.sin(euler[2]), 0]
)
aspect = self.CAMERA_WIDTH / self.CAMERA_HEIGHT
projection_matrix = p.computeProjectionMatrixFOV(
fov=self.CAMERA_FOV,
aspect=aspect,
nearVal=self.CAMERA_NEAR,
farVal=self.CAMERA_FAR
)
_, _, rgb, _, _ = p.getCameraImage(
width=self.CAMERA_WIDTH,
height=self.CAMERA_HEIGHT,
viewMatrix=view_matrix,
projectionMatrix=projection_matrix,
renderer=p.ER_BULLET_HARDWARE_OPENGL
)
image = np.array(rgb, dtype=np.uint8).reshape(
self.CAMERA_HEIGHT, self.CAMERA_WIDTH, 4
)[:, :, :3]
# Encode as base64 for transmission
try:
from PIL import Image
import io
pil_img = Image.fromarray(image)
buffer = io.BytesIO()
pil_img.save(buffer, format='JPEG', quality=70)
return base64.b64encode(buffer.getvalue()).decode('utf-8')
except ImportError:
# Return raw RGB values as base64 if PIL not available
return base64.b64encode(image.tobytes()).decode('utf-8')
def _get_landing_pad_detection(self) -> Optional[Dict[str, float]]:
"""Simulate downward camera detecting landing pad."""
drone_pos, drone_orn = p.getBasePositionAndOrientation(self._drone_id)
rover_pos, _ = p.getBasePositionAndOrientation(self._rover_id)
dx = rover_pos[0] - drone_pos[0]
dy = rover_pos[1] - drone_pos[1]
dz = rover_pos[2] - drone_pos[2]
horizontal_dist = math.sqrt(dx**2 + dy**2)
vertical_dist = -dz
if vertical_dist <= 0 or vertical_dist > 10.0:
return None
fov_rad = math.radians(self.CAMERA_FOV / 2)
max_horizontal = vertical_dist * math.tan(fov_rad)
if horizontal_dist > max_horizontal:
return None
rot_matrix = p.getMatrixFromQuaternion(drone_orn)
body_x = [rot_matrix[0], rot_matrix[3], rot_matrix[6]]
body_y = [rot_matrix[1], rot_matrix[4], rot_matrix[7]]
relative_x = dx * body_x[0] + dy * body_x[1] + dz * body_x[2]
relative_y = dx * body_y[0] + dy * body_y[1] + dz * body_y[2]
angle = math.atan2(horizontal_dist, vertical_dist)
confidence = max(0.0, 1.0 - (angle / fov_rad))
confidence *= max(0.0, 1.0 - (vertical_dist / 10.0))
return {
"relative_x": round(relative_x, 4),
"relative_y": round(relative_y, 4),
"distance": round(vertical_dist, 4),
"confidence": round(confidence, 4)
}
def _send_telemetry(self) -> None:
if self._controller_addr is None:
return
pos, orn = p.getBasePositionAndOrientation(self._drone_id)
vel, ang_vel = p.getBaseVelocity(self._drone_id)
euler = p.getEulerFromQuaternion(orn)
landed_on_rover = self._check_landing()
pad_detection = self._get_landing_pad_detection()
# Capture camera image periodically
self._camera_frame_count += 1
camera_image = None
if self._camera_frame_count >= self.CAMERA_INTERVAL:
self._camera_frame_count = 0
camera_image = self._capture_camera_image()
telemetry = {
"imu": {
"orientation": {
"roll": round(euler[0], 4),
"pitch": round(euler[1], 4),
"yaw": round(euler[2], 4)
},
"angular_velocity": {
"x": round(ang_vel[0], 4),
"y": round(ang_vel[1], 4),
"z": round(ang_vel[2], 4)
},
"linear_acceleration": {
"x": 0.0,
"y": 0.0,
"z": round(-self.GRAVITY, 4)
}
},
"altimeter": {
"altitude": round(pos[2], 4),
"vertical_velocity": round(vel[2], 4)
},
"velocity": {
"x": round(vel[0], 4),
"y": round(vel[1], 4),
"z": round(vel[2], 4)
},
"landing_pad": pad_detection,
"camera": {
"width": self.CAMERA_WIDTH,
"height": self.CAMERA_HEIGHT,
"fov": self.CAMERA_FOV,
"image": camera_image # Base64 encoded JPEG (or None)
},
"landed": landed_on_rover,
"timestamp": round(self._step_count * self.PHYSICS_TIMESTEP, 4)
}
try:
json_data = json.dumps(telemetry).encode('utf-8')
self._socket.sendto(json_data, self._controller_addr)
except socket.error as e:
print(f"Failed to send telemetry: {e}")
def _check_landing(self) -> bool:
contact_points = p.getContactPoints(
bodyA=self._drone_id,
bodyB=self._rover_id
)
return len(contact_points) > 0
def shutdown(self) -> None:
print("\nShutting down simulation...")
self._running = False
try:
self._socket.close()
print(" Socket closed")
except socket.error:
pass
if p.isConnected():
p.disconnect()
print(" PyBullet disconnected")
print("Shutdown complete")
def main():
print("\n" + "=" * 60)
print(" DRONE LANDING SIMULATION (GPS-DENIED + CAMERA)")
print("=" * 60 + "\n")
try:
simulator = DroneSimulator()
simulator.run()
except Exception as e:
print(f"Fatal error: {e}")
raise
if __name__ == '__main__':
main()