commit f489bfbad9e391fd0462e873723abeb614417e85 Author: default Date: Wed Dec 31 23:50:26 2025 +0000 Initial Attempt diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..ee55e71 --- /dev/null +++ b/.gitignore @@ -0,0 +1,4 @@ + + +venv/ +__pycache__/ diff --git a/README.md b/README.md new file mode 100644 index 0000000..37928cf --- /dev/null +++ b/README.md @@ -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` \ No newline at end of file diff --git a/activate.sh b/activate.sh new file mode 100755 index 0000000..c96bacb --- /dev/null +++ b/activate.sh @@ -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 "" diff --git a/build_exe.py b/build_exe.py new file mode 100644 index 0000000..1dddc27 --- /dev/null +++ b/build_exe.py @@ -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() diff --git a/controllers.py b/controllers.py new file mode 100644 index 0000000..64e0ad3 --- /dev/null +++ b/controllers.py @@ -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() diff --git a/docs/architecture.md b/docs/architecture.md new file mode 100644 index 0000000..49a8083 --- /dev/null +++ b/docs/architecture.md @@ -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) | diff --git a/docs/drone_guide.md b/docs/drone_guide.md new file mode 100644 index 0000000..af8e9e0 --- /dev/null +++ b/docs/drone_guide.md @@ -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 +``` diff --git a/docs/gazebo.md b/docs/gazebo.md new file mode 100644 index 0000000..7692194 --- /dev/null +++ b/docs/gazebo.md @@ -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 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 +``` diff --git a/docs/installation.md b/docs/installation.md new file mode 100644 index 0000000..f60a090 --- /dev/null +++ b/docs/installation.md @@ -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 +``` diff --git a/docs/protocol.md b/docs/protocol.md new file mode 100644 index 0000000..e2fb433 --- /dev/null +++ b/docs/protocol.md @@ -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": "" + }, + "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 +} +``` diff --git a/docs/pybullet.md b/docs/pybullet.md new file mode 100644 index 0000000..8873c0f --- /dev/null +++ b/docs/pybullet.md @@ -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 +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 +``` diff --git a/docs/rover_controller.md b/docs/rover_controller.md new file mode 100644 index 0000000..914cff4 --- /dev/null +++ b/docs/rover_controller.md @@ -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. diff --git a/drone_controller.py b/drone_controller.py new file mode 100644 index 0000000..ae6a338 --- /dev/null +++ b/drone_controller.py @@ -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() diff --git a/gazebo/launch/drone_landing.launch.py b/gazebo/launch/drone_landing.launch.py new file mode 100644 index 0000000..94550aa --- /dev/null +++ b/gazebo/launch/drone_landing.launch.py @@ -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 drone_landing.launch.py") + print("") + print("Or use the standalone script:") + print(" python3 gazebo_bridge.py") diff --git a/gazebo/models/drone/model.config b/gazebo/models/drone/model.config new file mode 100644 index 0000000..ec545e5 --- /dev/null +++ b/gazebo/models/drone/model.config @@ -0,0 +1,17 @@ + + + competition_drone + 1.0 + model.sdf + + + Competition Framework + competition@example.com + + + + A simple quadrotor drone for the drone landing competition. + Features velocity control via /drone/cmd_vel topic and + publishes odometry for state feedback. + + diff --git a/gazebo/models/drone/model.sdf b/gazebo/models/drone/model.sdf new file mode 100644 index 0000000..c72f2bc --- /dev/null +++ b/gazebo/models/drone/model.sdf @@ -0,0 +1,400 @@ + + + + + 0 0 5 0 0 0 + + + + + 1.0 + + 0.029125 + 0 + 0 + 0.029125 + 0 + 0.055225 + + + + + + + 0.3 0.3 0.1 + + + + + + + + 0.3 0.3 0.1 + + + + 0.6 0.1 0.1 1 + 0.8 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.12 0.12 0 0 0 0.785 + + + 0.2 0.02 0.02 + + + + 0.2 0.2 0.2 1 + 0.3 0.3 0.3 1 + + + + + + 0.12 -0.12 0 0 0 -0.785 + + + 0.2 0.02 0.02 + + + + 0.2 0.2 0.2 1 + 0.3 0.3 0.3 1 + + + + + + -0.12 0.12 0 0 0 -0.785 + + + 0.2 0.02 0.02 + + + + 0.2 0.2 0.2 1 + 0.3 0.3 0.3 1 + + + + + + -0.12 -0.12 0 0 0 0.785 + + + 0.2 0.02 0.02 + + + + 0.2 0.2 0.2 1 + 0.3 0.3 0.3 1 + + + + + + true + 100 + + + 00.01 + 00.01 + 00.01 + + + 00.1 + 00.1 + 00.1 + + + + + + + 0 0 -0.05 0 1.5708 0 + true + 30 + + 1.047 + + 320 + 240 + R8G8B8 + + + 0.1 + 20 + + + /drone/camera + + + + + + 0.17 0.17 0.05 0 0 0 + + 0.01 + + 0.0000100 + 0.000010 + 0.00002 + + + + + + 0.08 + 0.01 + + + + 0.1 0.1 0.1 1 + 0.2 0.2 0.2 1 + + + + + base_link + rotor_fl + + 0 0 1 + -1e161e16 + + + + + + 0.17 -0.17 0.05 0 0 0 + + 0.01 + + 0.0000100 + 0.000010 + 0.00002 + + + + + + 0.08 + 0.01 + + + + 0.1 0.1 0.1 1 + 0.2 0.2 0.2 1 + + + + + base_link + rotor_fr + + 0 0 1 + -1e161e16 + + + + + + -0.17 0.17 0.05 0 0 0 + + 0.01 + + 0.0000100 + 0.000010 + 0.00002 + + + + + + 0.08 + 0.01 + + + + 0.1 0.1 0.1 1 + 0.2 0.2 0.2 1 + + + + + base_link + rotor_bl + + 0 0 1 + -1e161e16 + + + + + + -0.17 -0.17 0.05 0 0 0 + + 0.01 + + 0.0000100 + 0.000010 + 0.00002 + + + + + + 0.08 + 0.01 + + + + 0.1 0.1 0.1 1 + 0.2 0.2 0.2 1 + + + + + base_link + rotor_br + + 0 0 1 + -1e161e16 + + + + + + drone + rotor_fl_joint + rotor_fl + ccw + 0.0125 + 0.025 + 1000 + 8.54858e-06 + 0.016 + command/motor_speed + 0 + 0.000806428 + 1e-06 + 10 + + + + drone + rotor_fr_joint + rotor_fr + cw + 0.0125 + 0.025 + 1000 + 8.54858e-06 + 0.016 + command/motor_speed + 1 + 0.000806428 + 1e-06 + 10 + + + + drone + rotor_bl_joint + rotor_bl + cw + 0.0125 + 0.025 + 1000 + 8.54858e-06 + 0.016 + command/motor_speed + 2 + 0.000806428 + 1e-06 + 10 + + + + drone + rotor_br_joint + rotor_br + ccw + 0.0125 + 0.025 + 1000 + 8.54858e-06 + 0.016 + command/motor_speed + 3 + 0.000806428 + 1e-06 + 10 + + + + + drone + cmd_vel + enable + base_link + 2.7 2.7 2.7 + 2 3 0.15 + 0.4 0.52 0.18 + 2 2 2 + + + + rotor_fl_joint + 8.54858e-06 + 0.016 + 1 + + + rotor_fr_joint + 8.54858e-06 + 0.016 + -1 + + + rotor_bl_joint + 8.54858e-06 + 0.016 + -1 + + + rotor_br_joint + 8.54858e-06 + 0.016 + 1 + + + + + + + odom + base_link + 50 + + + + + true + true + 50 + + + + diff --git a/gazebo/worlds/drone_landing.sdf b/gazebo/worlds/drone_landing.sdf new file mode 100644 index 0000000..3b55d36 --- /dev/null +++ b/gazebo/worlds/drone_landing.sdf @@ -0,0 +1,138 @@ + + + + + + + + 0.001 + 1.0 + + + + + + + + ogre2 + + + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.3 0.3 0.3 1 + 0.7 0.7 0.7 1 + 0.01 0.01 0.01 1 + + + + + + + + true + 0 0 0.15 0 0 0 + + + + + 1.0 1.0 0.3 + + + + + + + 1.0 1.0 0.3 + + + + 0.1 0.4 0.1 1 + 0.2 0.6 0.2 1 + 0.1 0.1 0.1 1 + + + + + + 0 0 0.151 0 0 0 + + + 0.6 0.1 0.001 + + + + 1 1 1 1 + 1 1 1 1 + + + + -0.25 0 0.151 0 0 0 + + + 0.1 0.6 0.001 + + + + 1 1 1 1 + 1 1 1 1 + + + + 0.25 0 0.151 0 0 0 + + + 0.1 0.6 0.001 + + + + 1 1 1 1 + 1 1 1 1 + + + + + + + diff --git a/gazebo_bridge.py b/gazebo_bridge.py new file mode 100644 index 0000000..231a6a0 --- /dev/null +++ b/gazebo_bridge.py @@ -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() diff --git a/requirements.txt b/requirements.txt new file mode 100644 index 0000000..0c3593d --- /dev/null +++ b/requirements.txt @@ -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 diff --git a/ros_bridge.py b/ros_bridge.py new file mode 100644 index 0000000..3bdaa22 --- /dev/null +++ b/ros_bridge.py @@ -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 --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() diff --git a/rover_controller.py b/rover_controller.py new file mode 100644 index 0000000..fc0cc4b --- /dev/null +++ b/rover_controller.py @@ -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() diff --git a/setup/install_macos.sh b/setup/install_macos.sh new file mode 100755 index 0000000..219f004 --- /dev/null +++ b/setup/install_macos.sh @@ -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 "" diff --git a/setup/install_ubuntu.sh b/setup/install_ubuntu.sh new file mode 100755 index 0000000..cce4162 --- /dev/null +++ b/setup/install_ubuntu.sh @@ -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 "" diff --git a/setup/install_windows.ps1 b/setup/install_windows.ps1 new file mode 100644 index 0000000..372f0b1 --- /dev/null +++ b/setup/install_windows.ps1 @@ -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 "" diff --git a/simulation_host.py b/simulation_host.py new file mode 100644 index 0000000..2e5fe4b --- /dev/null +++ b/simulation_host.py @@ -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()