Scripts and simulation packaging update
This commit is contained in:
86
README.md
86
README.md
@@ -4,17 +4,28 @@ A GPS-denied drone landing simulation using relative sensors (IMU, altimeter, ca
|
||||
|
||||
## Quick Start
|
||||
|
||||
```bash
|
||||
# Install (Ubuntu)
|
||||
./setup/install_ubuntu.sh
|
||||
source activate.sh
|
||||
### Windows (Standalone - No ROS 2 Required)
|
||||
|
||||
# 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
|
||||
```powershell
|
||||
. .\activate.ps1
|
||||
python standalone_simulation.py
|
||||
|
||||
# With moving rover
|
||||
python standalone_simulation.py --pattern circular --speed 0.3
|
||||
```
|
||||
|
||||
### Linux (Full ROS 2 Setup)
|
||||
|
||||
```bash
|
||||
source activate.sh
|
||||
|
||||
# Terminal 1: Simulator
|
||||
python simulation_host.py
|
||||
|
||||
# Terminal 2: ROS bridge
|
||||
python ros_bridge.py
|
||||
|
||||
# Terminal 3: Controllers
|
||||
python controllers.py --pattern circular --speed 0.3
|
||||
```
|
||||
|
||||
@@ -22,22 +33,17 @@ python controllers.py --pattern circular --speed 0.3
|
||||
|
||||
```
|
||||
┌─────────────────────────────────────────────────────────────────────────┐
|
||||
│ ┌──────────────────┐ ┌──────────────────────────┐ │
|
||||
│ │ simulation_host │◄── UDP:5555 ──────►│ ros_bridge.py │ │
|
||||
│ │ (PyBullet) │ └────────────┬─────────────┘ │
|
||||
│ └──────────────────┘ │ │
|
||||
│ OR │ │
|
||||
│ ┌──────────────────┐ ┌────────────┴─────────────┐ │
|
||||
│ │ Gazebo │◄── ROS Topics ────►│ gazebo_bridge.py │ │
|
||||
│ └──────────────────┘ └────────────┬─────────────┘ │
|
||||
│ │ │
|
||||
│ ┌────────────▼─────────────┐ │
|
||||
│ STANDALONE MODE (Windows) FULL MODE (Linux + ROS 2) │
|
||||
│ ───────────────────────── ────────────────────────── │
|
||||
│ │
|
||||
│ ┌──────────────────────┐ ┌───────────────┐ ┌───────────────┐ │
|
||||
│ │standalone_simulation │ │simulation_host│◄──►│ ros_bridge │ │
|
||||
│ │ (All-in-one) │ └───────────────┘ └───────┬───────┘ │
|
||||
│ └──────────────────────┘ │ │
|
||||
│ ┌─────────▼─────────┐ │
|
||||
│ │ controllers.py │ │
|
||||
│ │ ┌─────────────────────┐ │ │
|
||||
│ │ │ DroneController │ │ │
|
||||
│ │ │ RoverController │ │ │
|
||||
│ │ └─────────────────────┘ │ │
|
||||
│ └──────────────────────────┘ │
|
||||
│ │ (Drone + Rover) │ │
|
||||
│ └───────────────────┘ │
|
||||
└─────────────────────────────────────────────────────────────────────────┘
|
||||
```
|
||||
|
||||
@@ -45,20 +51,25 @@ python controllers.py --pattern circular --speed 0.3
|
||||
|
||||
| File | Description |
|
||||
|------|-------------|
|
||||
| `standalone_simulation.py` | **Windows: All-in-one simulation (no ROS 2)** |
|
||||
| `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** |
|
||||
| `controllers.py` | Runs drone + rover controllers |
|
||||
| `drone_controller.py` | Drone landing logic (edit this) |
|
||||
| `rover_controller.py` | Moving landing pad |
|
||||
|
||||
## Controller Options
|
||||
|
||||
```bash
|
||||
python controllers.py --help
|
||||
# Standalone (Windows)
|
||||
python standalone_simulation.py --pattern circular --speed 0.3
|
||||
|
||||
# Full mode (Linux)
|
||||
python controllers.py --pattern circular --speed 0.3
|
||||
|
||||
Options:
|
||||
--pattern, -p Rover pattern: stationary, linear, circular, random, square
|
||||
--pattern, -p Rover pattern: stationary, linear, circular, square
|
||||
--speed, -s Rover speed in m/s (default: 0.5)
|
||||
--amplitude, -a Rover amplitude in meters (default: 2.0)
|
||||
```
|
||||
@@ -72,8 +83,8 @@ The drone has no GPS. Available sensors:
|
||||
| **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) |
|
||||
| **Camera** | 320x240 downward-facing image |
|
||||
| **Landing Pad** | Relative position when visible |
|
||||
|
||||
## Documentation
|
||||
|
||||
@@ -85,12 +96,19 @@ The drone has no GPS. Available sensors:
|
||||
| [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 |
|
||||
| [Gazebo](docs/gazebo.md) | Gazebo-specific setup (Linux only) |
|
||||
|
||||
## Platform Support
|
||||
|
||||
| Platform | Standalone | Full (ROS 2) | Gazebo |
|
||||
|----------|------------|--------------|--------|
|
||||
| Windows | ✅ | ⚠️ Complex | ❌ |
|
||||
| Linux | ✅ | ✅ | ✅ |
|
||||
| macOS | ✅ | ⚠️ Limited | ❌ |
|
||||
|
||||
## 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`
|
||||
1. **Windows**: Run `python standalone_simulation.py`
|
||||
2. **Linux**: Read [docs/drone_guide.md](docs/drone_guide.md)
|
||||
3. Edit `drone_controller.py` to implement your algorithm
|
||||
4. Test with different rover patterns
|
||||
@@ -21,6 +21,7 @@ echo "[OK] Python venv activated"
|
||||
|
||||
echo ""
|
||||
echo "Environment ready! You can now run:"
|
||||
echo " python simulation_host.py"
|
||||
echo " python simulation_host.py # PyBullet"
|
||||
echo " python gazebo_bridge.py # Gazebo"
|
||||
echo " python ros_bridge.py"
|
||||
echo ""
|
||||
|
||||
@@ -1,8 +1,32 @@
|
||||
# Architecture Overview
|
||||
|
||||
GPS-denied drone landing simulation with camera vision.
|
||||
GPS-denied drone landing simulation with multiple operation modes.
|
||||
|
||||
## System Diagram
|
||||
## Operation Modes
|
||||
|
||||
### Standalone Mode (Windows/Simple)
|
||||
|
||||
All-in-one simulation - no ROS 2 or external dependencies:
|
||||
|
||||
```
|
||||
┌────────────────────────────────────────┐
|
||||
│ standalone_simulation.py │
|
||||
│ ┌──────────────────────────────────┐ │
|
||||
│ │ PyBullet Physics Engine │ │
|
||||
│ │ ┌────────┐ ┌────────────────┐ │ │
|
||||
│ │ │ Drone │ │ Landing Pad │ │ │
|
||||
│ │ └────────┘ └────────────────┘ │ │
|
||||
│ ├──────────────────────────────────┤ │
|
||||
│ │ Built-in Controller │ │
|
||||
│ │ • Landing algorithm │ │
|
||||
│ │ • Rover movement patterns │ │
|
||||
│ └──────────────────────────────────┘ │
|
||||
└────────────────────────────────────────┘
|
||||
```
|
||||
|
||||
### Full Mode (Linux + ROS 2)
|
||||
|
||||
Modular architecture for development and testing:
|
||||
|
||||
```
|
||||
┌─────────────────────────────────────────────────────────────────────────┐
|
||||
@@ -16,7 +40,7 @@ GPS-denied drone landing simulation with camera vision.
|
||||
│ OR │ │
|
||||
│ ┌──────────────────┐ ┌────────────┴─────────────┐ │
|
||||
│ │ Gazebo │◄── ROS Topics ────►│ gazebo_bridge.py │ │
|
||||
│ │ (Ignition) │ │ (Gazebo ↔ ROS Bridge) │ │
|
||||
│ │ (Linux only) │ │ (Gazebo ↔ ROS Bridge) │ │
|
||||
│ └──────────────────┘ └────────────┬─────────────┘ │
|
||||
│ │ │
|
||||
│ ┌────────────▼─────────────┐ │
|
||||
@@ -31,37 +55,31 @@ GPS-denied drone landing simulation with camera vision.
|
||||
|
||||
## Components
|
||||
|
||||
### Simulators
|
||||
### Standalone
|
||||
|
||||
| Component | Description |
|
||||
|-----------|-------------|
|
||||
| **PyBullet** (`simulation_host.py`) | Standalone physics, UDP networking, camera rendering |
|
||||
| **Gazebo** | Full robotics simulator, native ROS 2 integration, camera sensor |
|
||||
| **standalone_simulation.py** | Complete simulation with built-in controller |
|
||||
|
||||
### Bridges
|
||||
### Full Mode
|
||||
|
||||
| Component | Description |
|
||||
|-----------|-------------|
|
||||
| **PyBullet** (`simulation_host.py`) | Physics engine, UDP networking, camera |
|
||||
| **Gazebo** | Full robotics simulator (Linux only) |
|
||||
| **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 |
|
||||
| **gazebo_bridge.py** | Connects Gazebo ↔ ROS 2 |
|
||||
| **controllers.py** | Runs drone + rover controllers |
|
||||
| **drone_controller.py** | GPS-denied landing logic |
|
||||
| **rover_controller.py** | Moving landing pad patterns |
|
||||
|
||||
## ROS Topics
|
||||
## ROS Topics (Full Mode)
|
||||
|
||||
| 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
|
||||
|
||||
@@ -71,49 +89,28 @@ Simulator Bridge DroneController
|
||||
│ Render Camera │ │
|
||||
│ Compute Physics │ │
|
||||
│──────────────────────►│ │
|
||||
│ │ │
|
||||
│ │ GPS-Denied Sensors: │
|
||||
│ │ - IMU │
|
||||
│ │ - Altimeter │
|
||||
│ │ - Velocity │
|
||||
│ │ - Camera Image (JPEG) │
|
||||
│ │ - Camera Image │
|
||||
│ │ - Landing Pad Detection │
|
||||
│ │─────────────────────────►│
|
||||
│ │ │
|
||||
│ │ /cmd_vel │
|
||||
│◄──────────────────────│◄─────────────────────────│
|
||||
```
|
||||
|
||||
## Camera System
|
||||
## Platform Support
|
||||
|
||||
Both simulators provide a downward-facing camera:
|
||||
| Mode | Windows | Linux | macOS |
|
||||
|------|---------|-------|-------|
|
||||
| Standalone | ✅ | ✅ | ✅ |
|
||||
| Full (ROS 2) | ⚠️ | ✅ | ⚠️ |
|
||||
| Gazebo | ❌ | ✅ | ❌ |
|
||||
|
||||
| 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
|
||||
## UDP Protocol (Full Mode)
|
||||
|
||||
| Port | Direction | Content |
|
||||
|------|-----------|---------|
|
||||
| 5555 | Bridge → Simulator | Command JSON |
|
||||
| 5556 | Simulator → Bridge | Telemetry JSON (includes camera image) |
|
||||
| 5556 | Simulator → Bridge | Telemetry JSON |
|
||||
|
||||
@@ -228,10 +228,18 @@ chmod +x activate.sh
|
||||
|
||||
### Windows
|
||||
|
||||
- PyBullet works in GUI mode
|
||||
- Gazebo not officially supported
|
||||
- ROS 2 requires Windows-specific binaries
|
||||
- Consider WSL2 for full Linux experience
|
||||
- **PyBullet works fully** - GUI mode with camera
|
||||
- **Gazebo NOT supported** - Linux only
|
||||
- **ROS 2 optional** - Only needed for ros_bridge.py
|
||||
- Use `python simulation_host.py` directly
|
||||
|
||||
**On Windows, the recommended workflow is:**
|
||||
```powershell
|
||||
. .\activate.ps1
|
||||
python simulation_host.py
|
||||
```
|
||||
|
||||
The PyBullet simulation runs standalone with full GUI - no ROS 2 or Gazebo needed.
|
||||
|
||||
---
|
||||
|
||||
|
||||
@@ -2,7 +2,37 @@
|
||||
|
||||
Running the GPS-denied drone simulation with PyBullet.
|
||||
|
||||
## Quick Start
|
||||
## Windows (Standalone Mode)
|
||||
|
||||
No ROS 2 required! Run the all-in-one simulation:
|
||||
|
||||
```powershell
|
||||
. .\activate.ps1
|
||||
python standalone_simulation.py
|
||||
```
|
||||
|
||||
### Options
|
||||
|
||||
```powershell
|
||||
# Stationary landing pad
|
||||
python standalone_simulation.py
|
||||
|
||||
# Moving rover patterns
|
||||
python standalone_simulation.py --pattern circular --speed 0.3
|
||||
python standalone_simulation.py --pattern linear --speed 0.5
|
||||
python standalone_simulation.py --pattern square --speed 0.4
|
||||
|
||||
Options:
|
||||
--pattern, -p stationary, linear, circular, square
|
||||
--speed, -s Rover speed in m/s (default: 0.5)
|
||||
--amplitude, -a Movement amplitude in meters (default: 2.0)
|
||||
```
|
||||
|
||||
The simulation includes a built-in landing controller. Watch the drone automatically land on the rover!
|
||||
|
||||
---
|
||||
|
||||
## Linux (Full ROS 2 Mode)
|
||||
|
||||
**Terminal 1 - Simulator:**
|
||||
```bash
|
||||
@@ -22,7 +52,7 @@ source activate.sh
|
||||
python controllers.py --pattern circular --speed 0.3
|
||||
```
|
||||
|
||||
## Remote Setup
|
||||
### Remote Setup
|
||||
|
||||
Run simulator on one machine, controllers on another.
|
||||
|
||||
@@ -38,6 +68,8 @@ python ros_bridge.py --host <MACHINE_1_IP>
|
||||
python controllers.py
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## Simulation Parameters
|
||||
|
||||
| Parameter | Value |
|
||||
@@ -49,30 +81,23 @@ python controllers.py
|
||||
|
||||
## GPS-Denied Sensors
|
||||
|
||||
The simulator provides:
|
||||
|
||||
| Sensor | Description |
|
||||
|--------|-------------|
|
||||
| IMU | Orientation (roll, pitch, yaw), angular velocity |
|
||||
| IMU | Orientation, angular velocity |
|
||||
| Altimeter | Barometric altitude, vertical velocity |
|
||||
| Velocity | Optical flow estimate (x, y, z) |
|
||||
| Velocity | Optical flow estimate |
|
||||
| Camera | 320x240 downward JPEG image |
|
||||
| Landing Pad | Vision-based relative position (60° FOV, 10m range) |
|
||||
| Landing Pad | Vision-based relative position |
|
||||
|
||||
## 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 |
|
||||
@@ -81,23 +106,6 @@ The image is included in telemetry as `camera.image`.
|
||||
| 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:
|
||||
@@ -107,8 +115,6 @@ source activate.sh
|
||||
python build_exe.py
|
||||
```
|
||||
|
||||
Output: `dist/simulation_host` (or `.exe` on Windows)
|
||||
|
||||
## Troubleshooting
|
||||
|
||||
### "Cannot connect to X server"
|
||||
@@ -120,18 +126,12 @@ PyBullet requires a display:
|
||||
|
||||
### Drone flies erratically
|
||||
|
||||
Reduce control gains:
|
||||
Reduce control gains in `drone_controller.py`:
|
||||
```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:
|
||||
|
||||
@@ -1,27 +1,4 @@
|
||||
# =============================================================================
|
||||
# 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
|
||||
pybullet>=3.2.5
|
||||
numpy>=1.20.0
|
||||
pillow>=9.0.0
|
||||
pyinstaller>=6.0.0
|
||||
|
||||
@@ -2,35 +2,28 @@
|
||||
# =============================================================================
|
||||
# Drone Simulation - macOS Installation Script
|
||||
# =============================================================================
|
||||
# Installs ROS 2 Humble via robostack (conda), PyBullet, and dependencies
|
||||
# Uses a conda environment for all packages
|
||||
# Installs PyBullet and Python dependencies (Gazebo not supported on macOS)
|
||||
#
|
||||
# Usage:
|
||||
# chmod +x install_macos.sh
|
||||
# ./install_macos.sh
|
||||
#
|
||||
# Tested on: macOS Ventura, Sonoma (Apple Silicon & Intel)
|
||||
# Usage: ./install_macos.sh
|
||||
# =============================================================================
|
||||
|
||||
set -e # Exit on error
|
||||
set -e
|
||||
|
||||
echo "=============================================="
|
||||
echo " Drone Simulation - macOS Installation"
|
||||
echo "=============================================="
|
||||
echo ""
|
||||
|
||||
# Get the script directory and project root
|
||||
# Get script directory and project root
|
||||
SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
|
||||
PROJECT_ROOT="$(dirname "$SCRIPT_DIR")"
|
||||
ENV_NAME="drone_simulation"
|
||||
VENV_DIR="$PROJECT_ROOT/venv"
|
||||
|
||||
echo "[INFO] Project root: $PROJECT_ROOT"
|
||||
|
||||
# Detect architecture
|
||||
ARCH=$(uname -m)
|
||||
echo "[INFO] Detected architecture: $ARCH"
|
||||
echo "[INFO] Virtual environment: $VENV_DIR"
|
||||
|
||||
# -----------------------------------------------------------------------------
|
||||
# Step 1: Install Homebrew (if not present)
|
||||
# Step 1: Check Homebrew
|
||||
# -----------------------------------------------------------------------------
|
||||
echo ""
|
||||
echo "[STEP 1/5] Checking Homebrew..."
|
||||
@@ -38,70 +31,34 @@ 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)
|
||||
# Step 2: Install Python
|
||||
# -----------------------------------------------------------------------------
|
||||
echo ""
|
||||
echo "[STEP 2/5] Installing Miniforge (conda)..."
|
||||
echo "[STEP 2/5] Installing Python..."
|
||||
|
||||
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
|
||||
if ! command -v python3 &> /dev/null; then
|
||||
brew install python@3.11
|
||||
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
|
||||
echo "[INFO] Python already installed: $(python3 --version)"
|
||||
fi
|
||||
|
||||
# -----------------------------------------------------------------------------
|
||||
# Step 3: Create conda environment with ROS 2
|
||||
# Step 3: Create Virtual Environment
|
||||
# -----------------------------------------------------------------------------
|
||||
echo ""
|
||||
echo "[STEP 3/5] Creating conda environment with ROS 2..."
|
||||
echo "[STEP 3/5] Creating Python virtual environment..."
|
||||
|
||||
# Remove existing environment if present
|
||||
conda deactivate 2>/dev/null || true
|
||||
conda env remove -n $ENV_NAME 2>/dev/null || true
|
||||
if [ -d "$VENV_DIR" ]; then
|
||||
rm -rf "$VENV_DIR"
|
||||
fi
|
||||
|
||||
# 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
|
||||
python3 -m venv "$VENV_DIR"
|
||||
echo "[INFO] Virtual environment created at: $VENV_DIR"
|
||||
|
||||
# -----------------------------------------------------------------------------
|
||||
# Step 4: Install Python Dependencies
|
||||
@@ -109,7 +66,18 @@ conda install ros-humble-desktop ros-humble-geometry-msgs ros-humble-std-msgs -y
|
||||
echo ""
|
||||
echo "[STEP 4/5] Installing Python dependencies..."
|
||||
|
||||
pip install pybullet pyinstaller
|
||||
source "$VENV_DIR/bin/activate"
|
||||
pip install --upgrade pip
|
||||
|
||||
if [ -f "$PROJECT_ROOT/requirements.txt" ]; then
|
||||
echo "[INFO] Installing from requirements.txt..."
|
||||
pip install -r "$PROJECT_ROOT/requirements.txt"
|
||||
else
|
||||
echo "[INFO] Installing packages manually..."
|
||||
pip install pybullet numpy pillow pyinstaller
|
||||
fi
|
||||
|
||||
echo "[INFO] Python packages installed"
|
||||
|
||||
# -----------------------------------------------------------------------------
|
||||
# Step 5: Create Activation Script
|
||||
@@ -117,73 +85,54 @@ pip install pybullet pyinstaller
|
||||
echo ""
|
||||
echo "[STEP 5/5] Creating activation script..."
|
||||
|
||||
ACTIVATE_SCRIPT="$PROJECT_ROOT/activate.sh"
|
||||
cat > "$ACTIVATE_SCRIPT" << 'EOF'
|
||||
cat > "$PROJECT_ROOT/activate.sh" << 'EOF'
|
||||
#!/bin/bash
|
||||
# =============================================================================
|
||||
# Drone Competition - Environment Activation Script (macOS)
|
||||
# =============================================================================
|
||||
# This script activates the conda environment with ROS 2.
|
||||
#
|
||||
# Usage:
|
||||
# source activate.sh
|
||||
# =============================================================================
|
||||
# Drone Simulation - Environment Activation (macOS)
|
||||
|
||||
# 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"
|
||||
# Activate Python venv
|
||||
if [ -f "$SCRIPT_DIR/venv/bin/activate" ]; then
|
||||
source "$SCRIPT_DIR/venv/bin/activate"
|
||||
echo "[OK] Python venv activated"
|
||||
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 "Environment ready!"
|
||||
echo ""
|
||||
echo "Run: python standalone_simulation.py"
|
||||
echo ""
|
||||
echo "Note: ROS 2 and Gazebo are not supported on macOS."
|
||||
echo " Use standalone_simulation.py for the complete experience."
|
||||
echo ""
|
||||
EOF
|
||||
|
||||
chmod +x "$ACTIVATE_SCRIPT"
|
||||
echo "[INFO] Created activation script: $ACTIVATE_SCRIPT"
|
||||
chmod +x "$PROJECT_ROOT/activate.sh"
|
||||
echo "[INFO] Created: $PROJECT_ROOT/activate.sh"
|
||||
|
||||
# -----------------------------------------------------------------------------
|
||||
# Verification
|
||||
# -----------------------------------------------------------------------------
|
||||
echo ""
|
||||
echo "Verifying installation..."
|
||||
|
||||
source "$PROJECT_ROOT/activate.sh"
|
||||
|
||||
echo ""
|
||||
echo "Checking Python packages:"
|
||||
python3 -c "import pybullet; print(' PyBullet: OK')" || echo " PyBullet: FAILED"
|
||||
python3 -c "import numpy; print(' NumPy: OK')" || echo " NumPy: FAILED"
|
||||
python3 -c "from PIL import Image; print(' Pillow: OK')" || echo " Pillow: FAILED"
|
||||
|
||||
echo ""
|
||||
echo "=============================================="
|
||||
echo " Installation Complete!"
|
||||
echo "=============================================="
|
||||
echo ""
|
||||
echo "Verifying installation..."
|
||||
echo "Quick start:"
|
||||
echo " source activate.sh"
|
||||
echo " python standalone_simulation.py"
|
||||
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 "With moving rover:"
|
||||
echo " python standalone_simulation.py --pattern circular --speed 0.3"
|
||||
echo ""
|
||||
|
||||
@@ -2,23 +2,19 @@
|
||||
# =============================================================================
|
||||
# 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)
|
||||
# Installs ROS 2, Gazebo, PyBullet, and all required dependencies
|
||||
#
|
||||
# Usage:
|
||||
# chmod +x install_ubuntu.sh
|
||||
# ./install_ubuntu.sh
|
||||
#
|
||||
# Tested on: Ubuntu 22.04 LTS, Ubuntu 24.04 LTS
|
||||
# Usage: ./install_ubuntu.sh
|
||||
# =============================================================================
|
||||
|
||||
set -e # Exit on error
|
||||
set -e
|
||||
|
||||
echo "=============================================="
|
||||
echo " Drone Simulation - Ubuntu Installation"
|
||||
echo "=============================================="
|
||||
echo ""
|
||||
|
||||
# Get the script directory and project root
|
||||
# Get script directory and project root
|
||||
SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
|
||||
PROJECT_ROOT="$(dirname "$SCRIPT_DIR")"
|
||||
VENV_DIR="$PROJECT_ROOT/venv"
|
||||
@@ -27,71 +23,87 @@ 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"
|
||||
if [ -f /etc/os-release ]; then
|
||||
. /etc/os-release
|
||||
UBUNTU_VERSION=$VERSION_ID
|
||||
echo "[INFO] Detected: $NAME $VERSION_ID"
|
||||
else
|
||||
echo "[WARN] Ubuntu $UBUNTU_VERSION may not be officially supported."
|
||||
echo " Attempting to install ROS 2 Humble..."
|
||||
echo "[WARN] Could not detect Ubuntu version, assuming 22.04"
|
||||
UBUNTU_VERSION="22.04"
|
||||
fi
|
||||
|
||||
# -----------------------------------------------------------------------------
|
||||
# Step 1: System Dependencies
|
||||
# -----------------------------------------------------------------------------
|
||||
echo ""
|
||||
echo "[STEP 1/8] Installing system dependencies..."
|
||||
|
||||
sudo apt-get update
|
||||
sudo apt-get install -y \
|
||||
curl \
|
||||
gnupg \
|
||||
lsb-release \
|
||||
software-properties-common \
|
||||
python3 \
|
||||
python3-pip \
|
||||
python3-venv \
|
||||
git
|
||||
|
||||
echo "[INFO] System dependencies installed"
|
||||
|
||||
# -----------------------------------------------------------------------------
|
||||
# Step 2: ROS 2 Repository Setup
|
||||
# -----------------------------------------------------------------------------
|
||||
echo ""
|
||||
echo "[STEP 2/8] Setting up ROS 2 repository..."
|
||||
|
||||
# Add ROS 2 GPG key
|
||||
sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg
|
||||
|
||||
# Determine ROS 2 distro based on Ubuntu version
|
||||
if [ "$UBUNTU_VERSION" = "24.04" ]; then
|
||||
ROS_DISTRO="jazzy"
|
||||
elif [ "$UBUNTU_VERSION" = "22.04" ]; then
|
||||
ROS_DISTRO="humble"
|
||||
else
|
||||
echo "[WARN] Ubuntu $UBUNTU_VERSION not officially supported, trying humble"
|
||||
ROS_DISTRO="humble"
|
||||
fi
|
||||
|
||||
echo "[INFO] Will install ROS 2 $ROS_DISTRO"
|
||||
echo "[INFO] Using 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
|
||||
# Add repository
|
||||
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
|
||||
sudo apt-get update
|
||||
sudo apt-get install -y ros-${ROS_DISTRO}-ros-base ros-${ROS_DISTRO}-geometry-msgs ros-${ROS_DISTRO}-std-msgs ros-${ROS_DISTRO}-nav-msgs ros-${ROS_DISTRO}-sensor-msgs
|
||||
|
||||
# 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
|
||||
echo "[INFO] ROS 2 $ROS_DISTRO installed"
|
||||
|
||||
# -----------------------------------------------------------------------------
|
||||
# Step 4: Initialize rosdep
|
||||
# Step 4: Install Gazebo (optional)
|
||||
# -----------------------------------------------------------------------------
|
||||
echo ""
|
||||
echo "[STEP 4/8] Initializing rosdep..."
|
||||
if [ ! -f /etc/ros/rosdep/sources.list.d/20-default.list ]; then
|
||||
sudo rosdep init
|
||||
echo "[STEP 4/8] Installing Gazebo..."
|
||||
|
||||
if [ "$ROS_DISTRO" = "jazzy" ]; then
|
||||
GZ_VERSION="harmonic"
|
||||
else
|
||||
GZ_VERSION="fortress"
|
||||
fi
|
||||
rosdep update
|
||||
|
||||
sudo apt-get install -y ros-${ROS_DISTRO}-ros-gz || {
|
||||
echo "[WARN] Could not install ros-gz, Gazebo simulation will not be available"
|
||||
echo "[INFO] PyBullet simulation will still work"
|
||||
}
|
||||
|
||||
echo "[INFO] Gazebo installation complete"
|
||||
|
||||
# -----------------------------------------------------------------------------
|
||||
# Step 5: Create Python Virtual Environment
|
||||
@@ -99,35 +111,34 @@ rosdep update
|
||||
echo ""
|
||||
echo "[STEP 5/8] Creating Python virtual environment..."
|
||||
|
||||
# Install venv package if not present
|
||||
sudo apt install -y python3-venv python3-full
|
||||
# Remove existing venv if present
|
||||
if [ -d "$VENV_DIR" ]; then
|
||||
rm -rf "$VENV_DIR"
|
||||
fi
|
||||
|
||||
# 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
|
||||
# Create virtual environment
|
||||
python3 -m venv "$VENV_DIR"
|
||||
|
||||
echo "[INFO] Virtual environment created at: $VENV_DIR"
|
||||
|
||||
# -----------------------------------------------------------------------------
|
||||
# Step 6: Install Python Dependencies in venv
|
||||
# Step 6: Install Python Dependencies
|
||||
# -----------------------------------------------------------------------------
|
||||
echo ""
|
||||
echo "[STEP 6/8] Installing Python dependencies in virtual environment..."
|
||||
echo "[STEP 6/8] Installing Python dependencies..."
|
||||
|
||||
# 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
|
||||
echo "[INFO] Installing from requirements.txt..."
|
||||
pip install -r "$PROJECT_ROOT/requirements.txt"
|
||||
else
|
||||
pip install pybullet pyinstaller
|
||||
echo "[INFO] Installing packages manually..."
|
||||
pip install pybullet numpy pillow pyinstaller
|
||||
fi
|
||||
|
||||
echo "[INFO] Python packages installed in venv"
|
||||
echo "[INFO] Python packages installed"
|
||||
|
||||
# -----------------------------------------------------------------------------
|
||||
# Step 7: Create Activation Script
|
||||
@@ -135,81 +146,67 @@ echo "[INFO] Python packages installed in venv"
|
||||
echo ""
|
||||
echo "[STEP 7/8] Creating activation script..."
|
||||
|
||||
ACTIVATE_SCRIPT="$PROJECT_ROOT/activate.sh"
|
||||
cat > "$ACTIVATE_SCRIPT" << EOF
|
||||
cat > "$PROJECT_ROOT/activate.sh" << 'EOF'
|
||||
#!/bin/bash
|
||||
# =============================================================================
|
||||
# Drone Competition - Environment Activation Script
|
||||
# =============================================================================
|
||||
# This script activates both ROS 2 and the Python virtual environment.
|
||||
#
|
||||
# Usage:
|
||||
# source activate.sh
|
||||
# =============================================================================
|
||||
# Drone Simulation - Environment Activation
|
||||
|
||||
# Get the directory where this script is located
|
||||
SCRIPT_DIR="\$(cd "\$(dirname "\${BASH_SOURCE[0]}")" && pwd)"
|
||||
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"
|
||||
if [ -f "/opt/ros/jazzy/setup.bash" ]; then
|
||||
source /opt/ros/jazzy/setup.bash
|
||||
echo "[OK] ROS 2 jazzy sourced"
|
||||
elif [ -f "/opt/ros/humble/setup.bash" ]; then
|
||||
source /opt/ros/humble/setup.bash
|
||||
echo "[OK] ROS 2 humble sourced"
|
||||
else
|
||||
echo "[WARN] ROS 2 not found - standalone_simulation.py will work"
|
||||
fi
|
||||
|
||||
# Activate Python virtual environment
|
||||
source "\$SCRIPT_DIR/venv/bin/activate"
|
||||
# Activate Python venv
|
||||
if [ -f "$SCRIPT_DIR/venv/bin/activate" ]; then
|
||||
source "$SCRIPT_DIR/venv/bin/activate"
|
||||
echo "[OK] Python venv activated"
|
||||
fi
|
||||
|
||||
# Set Gazebo model path
|
||||
export GZ_SIM_RESOURCE_PATH="$SCRIPT_DIR/gazebo/models:$GZ_SIM_RESOURCE_PATH"
|
||||
|
||||
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 "Environment ready! Run one of:"
|
||||
echo " python standalone_simulation.py (No ROS 2 required)"
|
||||
echo " python simulation_host.py (With ROS 2)"
|
||||
echo ""
|
||||
EOF
|
||||
|
||||
chmod +x "$ACTIVATE_SCRIPT"
|
||||
echo "[INFO] Created activation script: $ACTIVATE_SCRIPT"
|
||||
chmod +x "$PROJECT_ROOT/activate.sh"
|
||||
echo "[INFO] Created: $PROJECT_ROOT/activate.sh"
|
||||
|
||||
# -----------------------------------------------------------------------------
|
||||
# Verification
|
||||
# Step 8: Verification
|
||||
# -----------------------------------------------------------------------------
|
||||
echo ""
|
||||
echo "[STEP 8/8] Verifying installation..."
|
||||
|
||||
source "$PROJECT_ROOT/activate.sh"
|
||||
|
||||
echo ""
|
||||
echo "Checking Python packages:"
|
||||
python3 -c "import pybullet; print(' PyBullet: OK')" || echo " PyBullet: FAILED"
|
||||
python3 -c "import numpy; print(' NumPy: OK')" || echo " NumPy: FAILED"
|
||||
python3 -c "from PIL import Image; print(' Pillow: OK')" || echo " Pillow: FAILED"
|
||||
|
||||
echo ""
|
||||
echo "=============================================="
|
||||
echo " Installation Complete!"
|
||||
echo "=============================================="
|
||||
echo ""
|
||||
echo "Verifying installation..."
|
||||
echo "Quick start:"
|
||||
echo " source activate.sh"
|
||||
echo " python standalone_simulation.py"
|
||||
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 "Or with ROS 2:"
|
||||
echo " python simulation_host.py # Terminal 1"
|
||||
echo " python ros_bridge.py # Terminal 2"
|
||||
echo " python controllers.py # Terminal 3"
|
||||
echo ""
|
||||
|
||||
@@ -1,15 +1,13 @@
|
||||
# =============================================================================
|
||||
# Drone Simulation - Windows Installation Script (PowerShell)
|
||||
# =============================================================================
|
||||
# Installs ROS 2 Humble, PyBullet, and all required dependencies
|
||||
# Uses a Python virtual environment for pip packages
|
||||
# Installs PyBullet and Python dependencies
|
||||
# ROS 2 is optional (complex setup, not required for standalone mode)
|
||||
#
|
||||
# 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
|
||||
@@ -29,14 +27,12 @@ Write-Host "[INFO] Virtual environment: $VenvDir" -ForegroundColor Gray
|
||||
$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 ""
|
||||
}
|
||||
|
||||
# Function to refresh environment PATH
|
||||
function Refresh-Path {
|
||||
$env:Path = [System.Environment]::GetEnvironmentVariable("Path","Machine") + ";" + [System.Environment]::GetEnvironmentVariable("Path","User")
|
||||
# Also add common Chocolatey paths
|
||||
$chocoPath = "$env:ProgramData\chocolatey\bin"
|
||||
if (Test-Path $chocoPath) {
|
||||
$env:Path = "$chocoPath;$env:Path"
|
||||
@@ -46,14 +42,14 @@ function Refresh-Path {
|
||||
# -----------------------------------------------------------------------------
|
||||
# Step 1: Install Chocolatey (Package Manager)
|
||||
# -----------------------------------------------------------------------------
|
||||
Write-Host "[STEP 1/7] Checking Chocolatey..." -ForegroundColor Green
|
||||
Write-Host "[STEP 1/5] Checking Chocolatey..." -ForegroundColor Green
|
||||
|
||||
$chocoInstalled = $false
|
||||
try {
|
||||
$chocoVersion = choco --version 2>$null
|
||||
if ($chocoVersion) {
|
||||
$chocoInstalled = $true
|
||||
Write-Host "[INFO] Chocolatey already installed (version $chocoVersion)" -ForegroundColor Green
|
||||
Write-Host "[INFO] Chocolatey already installed" -ForegroundColor Green
|
||||
}
|
||||
} catch {
|
||||
$chocoInstalled = $false
|
||||
@@ -67,23 +63,10 @@ if (-not $chocoInstalled) {
|
||||
[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 to find choco
|
||||
Refresh-Path
|
||||
|
||||
# Verify installation
|
||||
$chocoPath = "$env:ProgramData\chocolatey\bin\choco.exe"
|
||||
if (Test-Path $chocoPath) {
|
||||
Write-Host "[INFO] Chocolatey installed successfully" -ForegroundColor Green
|
||||
} else {
|
||||
Write-Host "[ERROR] Chocolatey installation failed. Please install manually:" -ForegroundColor Red
|
||||
Write-Host " https://chocolatey.org/install" -ForegroundColor Yellow
|
||||
Write-Host "[INFO] After installing Chocolatey, close and reopen PowerShell, then run this script again." -ForegroundColor Yellow
|
||||
exit 1
|
||||
}
|
||||
Write-Host "[INFO] Chocolatey installed" -ForegroundColor Green
|
||||
} catch {
|
||||
Write-Host "[ERROR] Failed to install Chocolatey: $_" -ForegroundColor Red
|
||||
Write-Host "[INFO] Please install Chocolatey manually: https://chocolatey.org/install" -ForegroundColor Yellow
|
||||
exit 1
|
||||
Write-Host "[WARN] Chocolatey installation failed, continuing..." -ForegroundColor Yellow
|
||||
}
|
||||
}
|
||||
|
||||
@@ -91,7 +74,7 @@ if (-not $chocoInstalled) {
|
||||
# Step 2: Install Python
|
||||
# -----------------------------------------------------------------------------
|
||||
Write-Host ""
|
||||
Write-Host "[STEP 2/7] Installing Python..." -ForegroundColor Green
|
||||
Write-Host "[STEP 2/5] Checking Python..." -ForegroundColor Green
|
||||
|
||||
$pythonInstalled = $false
|
||||
try {
|
||||
@@ -107,241 +90,137 @@ try {
|
||||
if (-not $pythonInstalled) {
|
||||
Write-Host "[INFO] Installing Python 3.11..." -ForegroundColor Yellow
|
||||
|
||||
# Use full path to choco if needed
|
||||
$chocoExe = "$env:ProgramData\chocolatey\bin\choco.exe"
|
||||
if (Test-Path $chocoExe) {
|
||||
& $chocoExe install python311 -y
|
||||
} else {
|
||||
choco install python311 -y
|
||||
Write-Host "[ERROR] Please install Python 3.11 manually from https://python.org" -ForegroundColor Red
|
||||
exit 1
|
||||
}
|
||||
|
||||
Refresh-Path
|
||||
|
||||
# Verify
|
||||
try {
|
||||
$pythonVersion = python --version
|
||||
Write-Host "[INFO] Python installed ($pythonVersion)" -ForegroundColor Green
|
||||
} catch {
|
||||
Write-Host "[ERROR] Python installation failed" -ForegroundColor Red
|
||||
Write-Host "[INFO] Please install Python 3.11 manually from https://python.org" -ForegroundColor Yellow
|
||||
exit 1
|
||||
}
|
||||
}
|
||||
|
||||
# -----------------------------------------------------------------------------
|
||||
# Step 3: Install Visual C++ Build Tools (optional but recommended)
|
||||
# Step 3: Create Python Virtual Environment
|
||||
# -----------------------------------------------------------------------------
|
||||
Write-Host ""
|
||||
Write-Host "[STEP 3/7] Checking Visual C++ Build Tools..." -ForegroundColor Green
|
||||
Write-Host "[STEP 3/5] Creating Python virtual environment..." -ForegroundColor Green
|
||||
|
||||
# Check if cl.exe exists (Visual C++ compiler)
|
||||
$vsInstalled = $false
|
||||
$vsPaths = @(
|
||||
"C:\Program Files\Microsoft Visual Studio\2022\BuildTools\VC\Tools\MSVC",
|
||||
"C:\Program Files\Microsoft Visual Studio\2022\Community\VC\Tools\MSVC",
|
||||
"C:\Program Files (x86)\Microsoft Visual Studio\2019\BuildTools\VC\Tools\MSVC"
|
||||
)
|
||||
|
||||
foreach ($path in $vsPaths) {
|
||||
if (Test-Path $path) {
|
||||
$vsInstalled = $true
|
||||
Write-Host "[INFO] Visual C++ Build Tools found" -ForegroundColor Green
|
||||
break
|
||||
}
|
||||
}
|
||||
|
||||
if (-not $vsInstalled) {
|
||||
Write-Host "[INFO] Visual C++ Build Tools not found" -ForegroundColor Yellow
|
||||
Write-Host "[INFO] Attempting to install (this may take 10-20 minutes)..." -ForegroundColor Yellow
|
||||
|
||||
try {
|
||||
$chocoExe = "$env:ProgramData\chocolatey\bin\choco.exe"
|
||||
if (Test-Path $chocoExe) {
|
||||
& $chocoExe install visualstudio2022-workload-vctools -y
|
||||
} else {
|
||||
choco install visualstudio2022-workload-vctools -y
|
||||
}
|
||||
Write-Host "[INFO] Visual C++ Build Tools installed" -ForegroundColor Green
|
||||
} catch {
|
||||
Write-Host "[WARN] Could not install Visual C++ Build Tools automatically" -ForegroundColor Yellow
|
||||
Write-Host "[INFO] PyBullet may fail to install. If it does, install Visual Studio Build Tools manually:" -ForegroundColor Yellow
|
||||
Write-Host " https://visualstudio.microsoft.com/visual-cpp-build-tools/" -ForegroundColor Yellow
|
||||
}
|
||||
}
|
||||
|
||||
# -----------------------------------------------------------------------------
|
||||
# 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
|
||||
|
||||
try {
|
||||
# Use BITS for more reliable download
|
||||
Start-BitsTransfer -Source $ros2Url -Destination $ros2Zip -DisplayName "Downloading ROS 2"
|
||||
} catch {
|
||||
# Fallback to Invoke-WebRequest
|
||||
Write-Host "[INFO] Using alternative download method..." -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
|
||||
Write-Host "[INFO] Virtual environment created" -ForegroundColor Green
|
||||
|
||||
# -----------------------------------------------------------------------------
|
||||
# Step 6: Install Python Dependencies in venv
|
||||
# Step 4: Install Python Dependencies
|
||||
# -----------------------------------------------------------------------------
|
||||
Write-Host ""
|
||||
Write-Host "[STEP 6/7] Installing Python dependencies in virtual environment..." -ForegroundColor Green
|
||||
Write-Host "[STEP 4/5] Installing Python dependencies..." -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) {
|
||||
Write-Host "[INFO] Installing from requirements.txt..." -ForegroundColor Gray
|
||||
pip install -r $requirementsFile
|
||||
} else {
|
||||
pip install pybullet pyinstaller pillow
|
||||
Write-Host "[INFO] Installing packages manually..." -ForegroundColor Gray
|
||||
pip install pybullet
|
||||
pip install numpy
|
||||
pip install pillow
|
||||
pip install pyinstaller
|
||||
}
|
||||
|
||||
Write-Host "[INFO] Python packages installed in venv" -ForegroundColor Green
|
||||
Write-Host "[INFO] Python packages installed" -ForegroundColor Green
|
||||
|
||||
# -----------------------------------------------------------------------------
|
||||
# Step 7: Create Activation Script
|
||||
# Step 5: Create Activation Scripts
|
||||
# -----------------------------------------------------------------------------
|
||||
Write-Host ""
|
||||
Write-Host "[STEP 7/7] Creating activation scripts..." -ForegroundColor Green
|
||||
Write-Host "[STEP 5/5] Creating activation scripts..." -ForegroundColor Green
|
||||
|
||||
# Create batch file for cmd.exe
|
||||
$activateBat = Join-Path $ProjectRoot "activate.bat"
|
||||
@"
|
||||
@echo off
|
||||
REM =============================================================================
|
||||
REM Drone Simulation - Environment Activation Script (Windows CMD)
|
||||
REM =============================================================================
|
||||
REM Usage: activate.bat
|
||||
REM =============================================================================
|
||||
REM Drone Simulation - Environment Activation (Windows CMD)
|
||||
|
||||
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 python controllers.py
|
||||
echo [OK] Environment ready!
|
||||
echo.
|
||||
echo Run: python standalone_simulation.py
|
||||
echo.
|
||||
"@ | Out-File -FilePath $activateBat -Encoding ASCII
|
||||
|
||||
# Create PowerShell script
|
||||
$activatePs1 = Join-Path $ProjectRoot "activate.ps1"
|
||||
@"
|
||||
# =============================================================================
|
||||
# Drone Simulation - Environment Activation Script (Windows PowerShell)
|
||||
# =============================================================================
|
||||
# Drone Simulation - Environment Activation (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
|
||||
# Activate Python virtual environment
|
||||
& "`$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 " python controllers.py"
|
||||
Write-Host "[OK] Environment ready!" -ForegroundColor Green
|
||||
Write-Host ""
|
||||
Write-Host "Run: python standalone_simulation.py" -ForegroundColor White
|
||||
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
|
||||
Write-Host "[INFO] Created activation scripts" -ForegroundColor Green
|
||||
|
||||
# -----------------------------------------------------------------------------
|
||||
# Verification
|
||||
# -----------------------------------------------------------------------------
|
||||
Write-Host ""
|
||||
Write-Host "==============================================" -ForegroundColor Cyan
|
||||
Write-Host " Installation Complete!" -ForegroundColor Cyan
|
||||
Write-Host " Verifying Installation..." -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 {
|
||||
$pybulletOk = python -c "import pybullet; print(' PyBullet: OK')" 2>&1
|
||||
if ($LASTEXITCODE -eq 0) {
|
||||
Write-Host $pybulletOk -ForegroundColor Green
|
||||
} else {
|
||||
Write-Host " PyBullet: FAILED" -ForegroundColor Red
|
||||
}
|
||||
|
||||
try {
|
||||
python -c "import PyInstaller; print(' PyInstaller: OK')"
|
||||
} catch {
|
||||
Write-Host " PyInstaller: FAILED" -ForegroundColor Red
|
||||
$numpyOk = python -c "import numpy; print(' NumPy: OK')" 2>&1
|
||||
if ($LASTEXITCODE -eq 0) {
|
||||
Write-Host $numpyOk -ForegroundColor Green
|
||||
} else {
|
||||
Write-Host " NumPy: FAILED" -ForegroundColor Red
|
||||
}
|
||||
|
||||
try {
|
||||
python -c "import PIL; print(' Pillow: OK')"
|
||||
} catch {
|
||||
Write-Host " Pillow: FAILED" -ForegroundColor Red
|
||||
$pillowOk = python -c "from PIL import Image; print(' Pillow: OK')" 2>&1
|
||||
if ($LASTEXITCODE -eq 0) {
|
||||
Write-Host $pillowOk -ForegroundColor Green
|
||||
} else {
|
||||
Write-Host " Pillow: FAILED (install with: pip install pillow)" -ForegroundColor Yellow
|
||||
}
|
||||
|
||||
Write-Host ""
|
||||
Write-Host "==============================================" -ForegroundColor Cyan
|
||||
Write-Host " IMPORTANT: Activate the environment first!" -ForegroundColor Cyan
|
||||
Write-Host " Installation Complete!" -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 "Quick start:" -ForegroundColor Yellow
|
||||
Write-Host " . .\activate.ps1" -ForegroundColor White
|
||||
Write-Host " python standalone_simulation.py" -ForegroundColor White
|
||||
Write-Host ""
|
||||
Write-Host "Then run the simulation:" -ForegroundColor Yellow
|
||||
Write-Host " python simulation_host.py" -ForegroundColor White
|
||||
Write-Host " python ros_bridge.py" -ForegroundColor White
|
||||
Write-Host " python controllers.py" -ForegroundColor White
|
||||
Write-Host "With moving rover:" -ForegroundColor Yellow
|
||||
Write-Host " python standalone_simulation.py --pattern circular --speed 0.3" -ForegroundColor White
|
||||
Write-Host ""
|
||||
Write-Host "Note: ROS 2 and Gazebo are not supported on Windows." -ForegroundColor Gray
|
||||
Write-Host " standalone_simulation.py provides the complete experience." -ForegroundColor Gray
|
||||
Write-Host ""
|
||||
|
||||
354
standalone_simulation.py
Normal file
354
standalone_simulation.py
Normal file
@@ -0,0 +1,354 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
Standalone Drone Simulation - Runs without ROS 2.
|
||||
Includes built-in controller for landing demonstration.
|
||||
|
||||
Usage: python standalone_simulation.py [--pattern PATTERN] [--speed SPEED]
|
||||
"""
|
||||
|
||||
import argparse
|
||||
import base64
|
||||
import json
|
||||
import math
|
||||
import time
|
||||
from typing import Dict, Any, Optional, Tuple
|
||||
|
||||
import numpy as np
|
||||
import pybullet as p
|
||||
import pybullet_data
|
||||
|
||||
|
||||
class StandaloneSimulation:
|
||||
"""Complete simulation with built-in controller - no ROS 2 needed."""
|
||||
|
||||
PHYSICS_TIMESTEP = 1.0 / 240.0
|
||||
GRAVITY = -9.81
|
||||
CONTROL_INTERVAL = 5 # Apply control every N physics steps
|
||||
|
||||
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_START_POS = [0.0, 0.0, 0.15]
|
||||
|
||||
CAMERA_FOV = 60.0
|
||||
|
||||
def __init__(self, rover_pattern='stationary', rover_speed=0.5, rover_amplitude=2.0):
|
||||
print("=" * 60)
|
||||
print("Standalone Drone Simulation (No ROS 2 Required)")
|
||||
print("=" * 60)
|
||||
|
||||
self._running = True
|
||||
self._step_count = 0
|
||||
self._time = 0.0
|
||||
|
||||
# Rover movement settings
|
||||
self._rover_pattern = rover_pattern
|
||||
self._rover_speed = rover_speed
|
||||
self._rover_amplitude = rover_amplitude
|
||||
self._rover_pos = list(self.ROVER_START_POS)
|
||||
|
||||
# Control command
|
||||
self._command = {'thrust': 0.0, 'pitch': 0.0, 'roll': 0.0, 'yaw': 0.0}
|
||||
|
||||
self._init_physics()
|
||||
self._init_objects()
|
||||
|
||||
print(f" Rover Pattern: {rover_pattern}")
|
||||
print(f" Rover Speed: {rover_speed} m/s")
|
||||
print(" Press Ctrl+C to exit")
|
||||
print("=" * 60)
|
||||
|
||||
def _init_physics(self) -> None:
|
||||
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())
|
||||
|
||||
def _init_objects(self) -> None:
|
||||
self._ground_id = p.loadURDF("plane.urdf")
|
||||
|
||||
# Create rover (landing pad)
|
||||
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_START_POS
|
||||
)
|
||||
|
||||
# Create landing pad marker
|
||||
self._create_landing_marker()
|
||||
|
||||
# Create drone
|
||||
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
|
||||
)
|
||||
|
||||
def _create_landing_marker(self) -> None:
|
||||
marker_height = self.ROVER_START_POS[2] + self.ROVER_SIZE[2] / 2 + 0.01
|
||||
h_size = 0.3
|
||||
line_color = [1, 1, 1]
|
||||
|
||||
p.addUserDebugLine([-h_size, 0, marker_height], [h_size, 0, marker_height],
|
||||
lineColorRGB=line_color, lineWidth=3)
|
||||
p.addUserDebugLine([-h_size, -h_size, marker_height], [-h_size, h_size, marker_height],
|
||||
lineColorRGB=line_color, lineWidth=3)
|
||||
p.addUserDebugLine([h_size, -h_size, marker_height], [h_size, h_size, marker_height],
|
||||
lineColorRGB=line_color, lineWidth=3)
|
||||
|
||||
def run(self) -> None:
|
||||
print("\nSimulation running...")
|
||||
|
||||
try:
|
||||
while self._running:
|
||||
loop_start = time.time()
|
||||
|
||||
if not p.isConnected():
|
||||
break
|
||||
|
||||
# Update rover position
|
||||
self._update_rover()
|
||||
|
||||
# Run controller
|
||||
if self._step_count % self.CONTROL_INTERVAL == 0:
|
||||
self._run_controller()
|
||||
|
||||
# Apply physics
|
||||
self._apply_controls()
|
||||
p.stepSimulation()
|
||||
self._step_count += 1
|
||||
self._time += self.PHYSICS_TIMESTEP
|
||||
|
||||
# Check landing
|
||||
if self._check_landing():
|
||||
print("\n*** LANDING SUCCESSFUL! ***\n")
|
||||
time.sleep(2)
|
||||
break
|
||||
|
||||
# Timing
|
||||
elapsed = time.time() - loop_start
|
||||
sleep_time = self.PHYSICS_TIMESTEP - elapsed
|
||||
if sleep_time > 0:
|
||||
time.sleep(sleep_time)
|
||||
|
||||
except KeyboardInterrupt:
|
||||
print("\nStopping simulation...")
|
||||
finally:
|
||||
if p.isConnected():
|
||||
p.disconnect()
|
||||
|
||||
def _update_rover(self) -> None:
|
||||
"""Move the rover based on pattern."""
|
||||
dt = self.PHYSICS_TIMESTEP
|
||||
|
||||
if self._rover_pattern == 'stationary':
|
||||
return
|
||||
|
||||
elif self._rover_pattern == 'linear':
|
||||
omega = self._rover_speed / self._rover_amplitude
|
||||
target_x = self._rover_amplitude * math.sin(omega * self._time)
|
||||
self._rover_pos[0] += 2.0 * (target_x - self._rover_pos[0]) * dt
|
||||
|
||||
elif self._rover_pattern == 'circular':
|
||||
omega = self._rover_speed / self._rover_amplitude
|
||||
target_x = self._rover_amplitude * math.cos(omega * self._time)
|
||||
target_y = self._rover_amplitude * math.sin(omega * self._time)
|
||||
self._rover_pos[0] += 2.0 * (target_x - self._rover_pos[0]) * dt
|
||||
self._rover_pos[1] += 2.0 * (target_y - self._rover_pos[1]) * dt
|
||||
|
||||
elif self._rover_pattern == 'square':
|
||||
segment = int(self._time / 3) % 4
|
||||
corners = [(1, 1), (-1, 1), (-1, -1), (1, -1)]
|
||||
target = corners[segment]
|
||||
target_x = target[0] * self._rover_amplitude
|
||||
target_y = target[1] * self._rover_amplitude
|
||||
dx = target_x - self._rover_pos[0]
|
||||
dy = target_y - self._rover_pos[1]
|
||||
dist = math.sqrt(dx**2 + dy**2)
|
||||
if dist > 0.01:
|
||||
self._rover_pos[0] += self._rover_speed * dx / dist * dt
|
||||
self._rover_pos[1] += self._rover_speed * dy / dist * dt
|
||||
|
||||
# Update rover position in simulation
|
||||
p.resetBasePositionAndOrientation(
|
||||
self._rover_id,
|
||||
[self._rover_pos[0], self._rover_pos[1], self._rover_pos[2]],
|
||||
[0, 0, 0, 1]
|
||||
)
|
||||
|
||||
def _get_telemetry(self) -> Dict[str, Any]:
|
||||
"""Get current drone state."""
|
||||
pos, orn = p.getBasePositionAndOrientation(self._drone_id)
|
||||
vel, ang_vel = p.getBaseVelocity(self._drone_id)
|
||||
euler = p.getEulerFromQuaternion(orn)
|
||||
|
||||
# Landing pad detection
|
||||
dx = self._rover_pos[0] - pos[0]
|
||||
dy = self._rover_pos[1] - pos[1]
|
||||
dz = self._rover_pos[2] - pos[2]
|
||||
horizontal_dist = math.sqrt(dx**2 + dy**2)
|
||||
vertical_dist = -dz
|
||||
|
||||
landing_pad = None
|
||||
if vertical_dist > 0 and vertical_dist < 10.0:
|
||||
fov_rad = math.radians(self.CAMERA_FOV / 2)
|
||||
max_horizontal = vertical_dist * math.tan(fov_rad)
|
||||
if horizontal_dist < max_horizontal:
|
||||
landing_pad = {
|
||||
"relative_x": dx,
|
||||
"relative_y": dy,
|
||||
"distance": vertical_dist,
|
||||
"confidence": 1.0 - (horizontal_dist / max_horizontal)
|
||||
}
|
||||
|
||||
return {
|
||||
"altimeter": {"altitude": pos[2], "vertical_velocity": vel[2]},
|
||||
"velocity": {"x": vel[0], "y": vel[1], "z": vel[2]},
|
||||
"imu": {
|
||||
"orientation": {"roll": euler[0], "pitch": euler[1], "yaw": euler[2]},
|
||||
"angular_velocity": {"x": ang_vel[0], "y": ang_vel[1], "z": ang_vel[2]}
|
||||
},
|
||||
"landing_pad": landing_pad,
|
||||
"rover_position": {"x": self._rover_pos[0], "y": self._rover_pos[1]}
|
||||
}
|
||||
|
||||
def _run_controller(self) -> None:
|
||||
"""Simple landing controller."""
|
||||
telemetry = self._get_telemetry()
|
||||
|
||||
altimeter = telemetry['altimeter']
|
||||
altitude = altimeter['altitude']
|
||||
vertical_vel = altimeter['vertical_velocity']
|
||||
|
||||
velocity = telemetry['velocity']
|
||||
vel_x = velocity['x']
|
||||
vel_y = velocity['y']
|
||||
|
||||
landing_pad = telemetry['landing_pad']
|
||||
|
||||
# Altitude control
|
||||
target_alt = 0.0
|
||||
Kp_z, Kd_z = 0.5, 0.3
|
||||
thrust = Kp_z * (target_alt - altitude) - Kd_z * vertical_vel
|
||||
|
||||
# Horizontal control
|
||||
Kp_xy, Kd_xy = 0.3, 0.2
|
||||
|
||||
if landing_pad is not None:
|
||||
target_x = landing_pad['relative_x']
|
||||
target_y = landing_pad['relative_y']
|
||||
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
|
||||
|
||||
# Clamp
|
||||
thrust = max(-1.0, min(1.0, thrust))
|
||||
pitch = max(-0.5, min(0.5, pitch))
|
||||
roll = max(-0.5, min(0.5, roll))
|
||||
|
||||
self._command = {'thrust': thrust, 'pitch': pitch, 'roll': roll, 'yaw': 0.0}
|
||||
|
||||
def _apply_controls(self) -> None:
|
||||
"""Apply control commands to drone."""
|
||||
pos, orn = p.getBasePositionAndOrientation(self._drone_id)
|
||||
rot_matrix = p.getMatrixFromQuaternion(orn)
|
||||
local_up = [rot_matrix[2], rot_matrix[5], rot_matrix[8]]
|
||||
|
||||
total_thrust = self.HOVER_THRUST + (self._command['thrust'] * 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
|
||||
)
|
||||
|
||||
p.applyExternalTorque(
|
||||
self._drone_id, -1,
|
||||
torqueObj=[
|
||||
self._command['pitch'] * self.PITCH_TORQUE_SCALE,
|
||||
self._command['roll'] * self.ROLL_TORQUE_SCALE,
|
||||
self._command['yaw'] * self.YAW_TORQUE_SCALE
|
||||
],
|
||||
flags=p.LINK_FRAME
|
||||
)
|
||||
|
||||
def _check_landing(self) -> bool:
|
||||
"""Check if drone landed on rover."""
|
||||
contacts = p.getContactPoints(bodyA=self._drone_id, bodyB=self._rover_id)
|
||||
if len(contacts) > 0:
|
||||
vel, _ = p.getBaseVelocity(self._drone_id)
|
||||
speed = math.sqrt(vel[0]**2 + vel[1]**2 + vel[2]**2)
|
||||
return speed < 0.5
|
||||
return False
|
||||
|
||||
|
||||
def main():
|
||||
parser = argparse.ArgumentParser(description='Standalone Drone Simulation')
|
||||
parser.add_argument('--pattern', '-p', type=str, default='stationary',
|
||||
choices=['stationary', 'linear', 'circular', 'square'],
|
||||
help='Rover movement pattern')
|
||||
parser.add_argument('--speed', '-s', type=float, default=0.5,
|
||||
help='Rover speed in m/s')
|
||||
parser.add_argument('--amplitude', '-a', type=float, default=2.0,
|
||||
help='Rover movement amplitude in meters')
|
||||
args = parser.parse_args()
|
||||
|
||||
sim = StandaloneSimulation(
|
||||
rover_pattern=args.pattern,
|
||||
rover_speed=args.speed,
|
||||
rover_amplitude=args.amplitude
|
||||
)
|
||||
sim.run()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
Reference in New Issue
Block a user