Initial Attempt
This commit is contained in:
4
.gitignore
vendored
Normal file
4
.gitignore
vendored
Normal file
@@ -0,0 +1,4 @@
|
||||
|
||||
|
||||
venv/
|
||||
__pycache__/
|
||||
96
README.md
Normal file
96
README.md
Normal 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
26
activate.sh
Executable 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
93
build_exe.py
Normal 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
109
controllers.py
Normal 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
119
docs/architecture.md
Normal 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
187
docs/drone_guide.md
Normal 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
158
docs/gazebo.md
Normal 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
267
docs/installation.md
Normal 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
184
docs/protocol.md
Normal 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
140
docs/pybullet.md
Normal 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
100
docs/rover_controller.md
Normal 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
214
drone_controller.py
Normal 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
84
gazebo/launch/drone_landing.launch.py
Normal 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
17
gazebo/models/drone/model.config
Normal 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
400
gazebo/models/drone/model.sdf
Normal 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
138
gazebo/worlds/drone_landing.sdf
Normal 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
271
gazebo_bridge.py
Normal 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
27
requirements.txt
Normal 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
221
ros_bridge.py
Normal 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
246
rover_controller.py
Normal 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
189
setup/install_macos.sh
Executable 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
215
setup/install_ubuntu.sh
Executable 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
232
setup/install_windows.ps1
Normal 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
442
simulation_host.py
Normal 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()
|
||||
Reference in New Issue
Block a user