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

Controller Update

Browse Source
This commit is contained in:
default
2026-02-09 05:51:51 +00:00
parent cd9ae9a4f6
commit 1a616472f0
16 changed files with 1545 additions and 669 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
.gitignore vendored Normal file
View File

@@ -0,0 +1,3 @@
__pycache__
.venv

143
README.md
View File

@@ -1,30 +1,30 @@
# UAV-UGV Simulation
GPS-Denied Navigation with Geofencing
**GPS-Denied Navigation with Vision-Based Localization**
A simulation environment for UAV and UGV development using GPS-denied navigation with vision-based localization, while maintaining GPS-based geofencing for safety.
A ROS 2 simulation environment for UAV and UGV development using GPS-denied navigation with vision-based localization, while maintaining GPS-based geofencing for safety.
## GPS-Denied Navigation
## Features
**Navigation**: All vehicles navigate using relative positioning only:
- Visual odometry from cameras
- Optical flow sensors
- IMU integration
- Local coordinate frames
**GPS Usage**: GPS is ONLY used for geofencing (safety boundaries), NOT for navigation or position control.
- **GPS-Denied Navigation**: Visual odometry, optical flow, and EKF sensor fusion
- **GPS-Based Geofencing**: Safety boundaries using GPS (navigation remains GPS-free)
- **Multi-Vehicle Support**: Coordinated UAV and UGV operations
- **ArduPilot SITL**: Full flight controller simulation
- **Gazebo Harmonic**: Modern physics simulation
- **ROS 2 Integration**: Full ROS 2 Humble/Jazzy support
## Requirements
- Ubuntu 22.04 or 24.04 (WSL2 supported)
- 16GB RAM recommended
- 50GB disk space
- NVIDIA GPU recommended (software rendering available)
## Installation
```bash
git clone https://git.sirblob.co/SirBlob/simulation.git
cd simulation
git clone <repository-url> ~/sim/uav_ugv_simulation
cd ~/sim/uav_ugv_simulation
bash setup.sh
```
@@ -32,72 +32,101 @@ Installation takes 20-40 minutes (builds ArduPilot from source).
## Quick Start
### Autonomous Mode (Recommended)
The UAV automatically arms, takes off, and executes a mission:
```bash
cd ~/simulation
cd ~/sim/uav_ugv_simulation
source activate_venv.sh
# Hover mission
bash scripts/run_autonomous.sh --mission hover
# Square pattern
bash scripts/run_autonomous.sh --mission square
# Circle pattern
bash scripts/run_autonomous.sh --mission circle
# WSL/Software rendering
bash scripts/run_autonomous.sh --software-render --mission hover
```
### Manual Mode (MAVProxy)
```bash
bash scripts/run_simulation.sh
# Wait for EKF initialization (~15 seconds), then:
# mode guided
# arm throttle force
# takeoff 5
```
For WSL with graphics issues:
```bash
bash scripts/run_simulation.sh --software-render
```
## Controlling the UAV
Once the simulation is running, use MAVProxy commands in the console:
```
mode guided # Switch to GUIDED mode
arm throttle # Arm the drone
takeoff 5 # Takeoff to 5 meters
guided 10 5 -10 # Fly to position (North, East, Down)
rtl # Return to launch
land # Land
```
Or via ROS 2:
### ROS 2 Mode
```bash
# Arm
ros2 service call /mavros/cmd/arming mavros_msgs/srv/CommandBool "{value: true}"
# Takeoff
ros2 service call /mavros/cmd/takeoff mavros_msgs/srv/CommandTOL "{altitude: 5}"
# Fly to position (local coordinates)
ros2 topic pub /mavros/setpoint_position/local geometry_msgs/PoseStamped \
"{header: {frame_id: 'map'}, pose: {position: {x: 10, y: 5, z: 5}}}"
source /opt/ros/humble/setup.bash
ros2 launch uav_ugv_simulation full_simulation.launch.py
```
## Simulation Options
## Mission Types
```bash
# Default
bash scripts/run_simulation.sh
| Mission | Description |
|---------|-------------|
| `hover` | Take off, hover for 30 seconds, land |
| `square` | Fly a 5m square pattern |
| `circle` | Fly a circular pattern |
# Custom world
bash scripts/run_simulation.sh --world iris_runway
## GPS-Denied Navigation
# Rover instead of copter
bash scripts/run_simulation.sh --vehicle Rover
**Navigation** uses only relative positioning:
- Visual odometry from cameras
- Optical flow sensors
- IMU integration
- Local coordinate frames (NED)
# Software rendering (WSL)
bash scripts/run_simulation.sh --software-render
**GPS is ONLY used for geofencing** (safety boundaries), NOT for navigation or position control.
## Project Structure
```
uav_ugv_simulation/
├── setup.sh # Installation script
├── scripts/
│ ├── run_autonomous.sh # Autonomous simulation
│ ├── run_simulation.sh # Manual simulation
│ └── kill_simulation.sh # Cleanup
├── src/
│ ├── control/ # UAV/UGV controllers
│ ├── vision/ # Visual odometry, optical flow
│ ├── localization/ # EKF sensor fusion
│ ├── navigation/ # Path planning
│ └── safety/ # Geofencing, failsafe
├── launch/ # ROS 2 launch files
├── config/ # Configuration files
├── models/ # Gazebo models
└── worlds/ # Gazebo worlds
```
## Uninstall
## Commands
```bash
bash scripts/uninstall.sh # Remove ArduPilot and plugin
bash scripts/uninstall.sh --all # Remove everything
```
| Command | Description |
|---------|-------------|
| `bash scripts/run_autonomous.sh` | Run autonomous simulation |
| `bash scripts/run_simulation.sh` | Run manual simulation |
| `bash scripts/kill_simulation.sh` | Kill all processes |
| `bash scripts/uninstall.sh` | Uninstall ArduPilot |
## Documentation
- [Setup Guide](docs/setup_guide.md)
- [WSL Setup Guide](docs/wsl_setup_guide.md)
- [Usage Guide](docs/usage.md)
- [Architecture](docs/architecture.md)
- [GPS-Denied Navigation](docs/gps_denied_navigation.md)
- [WSL Setup](docs/wsl_setup_guide.md)
- [Troubleshooting](docs/troubleshooting.md)
## License
MIT License

4
config/ardupilot_gps_denied.parm
View File

@@ -88,8 +88,8 @@ FLTMODE6 9 # Land
# ====================
# Arming Checks
# ====================
# Relax arming checks for GPS-denied operation
ARMING_CHECK 14 # Skip GPS check (bit 2 = 4)
# Disable arming checks for simulation testing
ARMING_CHECK 0 # Disable all checks (for sim only)
# ====================
# Logging

233
docs/architecture.md
View File

@@ -1,120 +1,177 @@
# Architecture
System architecture for GPS-denied UAV/UGV navigation.
# System Architecture
## Overview
The system uses vision-based navigation with GPS reserved only for geofencing.
```
Vision Sensors (Cameras)
|
v
Visual Odometry & Optical Flow
|
v
Position Estimator (EKF)
|
v
ArduPilot Flight Controller
|
v
Motor Control
┌─────────────────────────────────────────────────────────────────┐
│ Simulation │
├───────────────────┬─────────────────────┬───────────────────────┤
│ Gazebo Harmonic │ ArduPilot SITL │ ROS 2 Nodes │
│ (Physics/3D) │ (Flight Control) │ (Perception/Nav) │
└───────────────────┴─────────────────────┴───────────────────────┘
│ │ │
└────────────────────┼──────────────────────┘
│
┌─────────┴─────────┐
│ MAVLink/MAVROS │
└───────────────────┘
```
## Components
### Perception
### 1. Gazebo Harmonic
- **Forward Camera**: Visual odometry, feature tracking
- **Downward Camera**: Optical flow, ground plane detection
- **IMU**: Angular velocity, acceleration
**Role:** 3D simulation, physics, sensors
### Localization
- **World simulation**: Ground plane, lighting, physics
- **UAV model**: Iris quadcopter with cameras
- **UGV model**: Differential drive rover
- **Sensors**: Cameras, IMU, rangefinder
- **Visual Odometry**: Estimates motion from camera images
- **Optical Flow**: Velocity estimation from downward camera
- **EKF Fusion**: Combines all sensor inputs into position estimate
### 2. ArduPilot SITL
### Navigation
**Role:** Flight controller simulation
- **Waypoint Navigation**: Relative coordinates (meters from origin)
- **Path Planning**: Collision-free paths using local map
- **Position Hold**: Maintain position using vision
- **EKF3**: State estimation using external vision
- **Flight modes**: GUIDED, LOITER, RTL, LAND
- **Motor mixing**: Quadcopter dynamics
- **Failsafe**: Battery, geofence, communication
### Control
**Key Parameters (GPS-Denied):**
```
GPS_TYPE 0 # GPS disabled
EK3_SRC1_POSXY 6 # External nav for position
EK3_SRC1_VELXY 6 # External nav for velocity
VISO_TYPE 1 # MAVLink vision input
ARMING_CHECK 0 # Disabled for simulation
```
- **ArduPilot**: Flight controller firmware
- **MAVROS**: ROS interface to ArduPilot
- **Velocity/Position Control**: Low-level motor commands
### 3. ROS 2 Nodes
### Safety
#### Vision Pipeline
- **Geofencing**: GPS-based boundary checking (safety only)
- **Altitude Limits**: Maximum flight ceiling
- **Failsafe**: RTL on signal loss or boundary breach
```
Camera → Visual Odometry → Position Estimator → Controller
↓
Optical Flow ─────────────┘
```
| Node | Function |
|------|----------|
| `visual_odom_node` | ORB feature tracking, pose estimation |
| `optical_flow_node` | Lucas-Kanade velocity estimation |
| `position_estimator` | Weighted average sensor fusion |
| `ekf_fusion_node` | Extended Kalman Filter fusion |
#### Control Pipeline
```
Mission Planner → UAV Controller → MAVROS → ArduPilot
→ UGV Controller → cmd_vel
```
| Node | Function |
|------|----------|
| `uav_controller` | GUIDED mode control, auto-arm |
| `ugv_controller` | Differential drive control |
| `mission_planner` | Multi-vehicle coordination |
#### Safety Pipeline
```
GPS → Geofence Monitor → Failsafe Handler → Emergency Action
```
| Node | Function |
|------|----------|
| `geofence_node` | GPS-based boundary monitoring |
| `failsafe_handler` | Vision loss, battery, emergency |
## Data Flow
### Position Estimation
```
1. Camera captures frames (30 Hz)
2. ORB detects features
3. Essential matrix computed
4. Relative motion estimated
5. Position integrated
6. Published to /uav/visual_odometry/pose
```
### Control Loop
```
1. Target received (/uav/setpoint_position)
2. Current position from VO or MAVROS
3. Error computed
4. Velocity command generated
5. Sent via MAVROS to ArduPilot
6. ArduPilot executes motor commands
```
### Geofencing (GPS Only)
```
1. GPS fix received
2. Check against polygon boundary
3. Check altitude limits
4. If breach: trigger RTL/LAND
5. Navigation continues using VO (not GPS)
```
## Coordinate Frames
| Frame | Description |
|-------|-------------|
| body | Attached to vehicle, X forward, Y right, Z down |
| odom | Origin at takeoff, accumulates drift |
| map | Fixed world frame (may be corrected) |
| `odom` | Local origin (takeoff point) |
| `base_link` | Vehicle body frame |
| `map` | World frame (aligned with odom) |
All navigation commands use local coordinates relative to takeoff point.
**NED Convention:**
- X = North (forward)
- Y = East (right)
- Z = Down (negative altitude)
## Data Flow
## File Structure
```
Camera Images
|
v
Feature Detection (ORB/SIFT)
|
v
Feature Matching (frame to frame)
|
v
Motion Estimation (Essential Matrix)
|
v
EKF Update (fuse with IMU)
|
v
Position Estimate (x, y, z, roll, pitch, yaw)
|
v
ArduPilot (external position input)
|
v
PID Control -> Motor PWM
src/
├── vision/
│ ├── visual_odometry.py # Feature tracking VO
│ ├── optical_flow.py # LK optical flow
│ └── camera_processor.py # Image processing
├── localization/
│ ├── position_estimator.py # Weighted fusion
│ └── ekf_fusion.py # EKF fusion
├── navigation/
│ ├── local_planner.py # Path planning
│ └── waypoint_follower.py # Waypoint tracking
├── control/
│ ├── uav_controller.py # UAV flight control
│ ├── ugv_controller.py # UGV drive control
│ └── mission_planner.py # Coordination
└── safety/
├── geofence_monitor.py # GPS boundaries
└── failsafe_handler.py # Emergency handling
```
## ArduPilot Integration
## Configuration
ArduPilot receives external position via MAVLink:
### ArduPilot EKF Sources
1. Visual odometry → `VISION_POSITION_ESTIMATE`
2. EKF uses external source (EK3_SRC1_POSXY=6)
3. GPS disabled for navigation (GPS_TYPE=0 or EK3_SRC1_POSXY!=1)
```yaml
EK3_SRC1_POSXY: 6 # External Nav
EK3_SRC1_POSZ: 1 # Barometer
EK3_SRC1_VELXY: 6 # External Nav
EK3_SRC1_YAW: 6 # External Nav
```
## Geofencing
### Sensor Weights
GPS is only used for safety boundaries:
1. GPS position → lat/lon
2. Check against polygon/circle boundary
3. If outside: trigger RTL or LAND
Navigation continues using vision regardless of GPS status.
## ROS 2 Topics
| Topic | Type | Description |
|-------|------|-------------|
| /uav/camera/forward/image_raw | sensor_msgs/Image | Forward camera |
| /uav/camera/downward/image_raw | sensor_msgs/Image | Downward camera |
| /mavros/local_position/pose | geometry_msgs/PoseStamped | Current position |
| /mavros/setpoint_position/local | geometry_msgs/PoseStamped | Target position |
| /mavros/imu/data | sensor_msgs/Imu | IMU data |
```yaml
vo_weight: 0.6 # Visual odometry
optical_flow: 0.3 # Optical flow
imu: 0.1 # IMU integration
```

267
docs/gps_denied_navigation.md
View File

@@ -1,128 +1,211 @@
# GPS-Denied Navigation
How the system navigates without GPS.
## Overview
## Principle
This system enables UAV/UGV navigation **without GPS** by using:
All navigation uses relative positioning from visual sensors. GPS is only used for geofencing (safety boundaries).
1. **Visual Odometry** - Camera-based pose estimation
2. **Optical Flow** - Velocity estimation from downward camera
3. **IMU Integration** - Short-term dead reckoning
4. **EKF Fusion** - Combine all sensors
| Function | GPS Used? |
|----------|-----------|
| Position estimation | No - visual odometry |
| Waypoint navigation | No - local coordinates |
| Velocity control | No - optical flow |
| Geofencing | Yes - safety only |
**GPS is ONLY used for geofencing (safety boundaries).**
## Position Estimation
## How It Works
### Visual Odometry
1. Detect features in camera image (ORB, SIFT)
2. Match features between consecutive frames
3. Estimate camera motion from feature displacement
4. Accumulate motion into position estimate
```
Frame N-1 Frame N
│ │
▼ ▼
┌────────┐ ┌────────┐
│Features│──│Features│ → Match features
└────────┘ └────────┘
│ │
└─────┬─────┘
▼
Essential Matrix → Rotation + Translation
```
**Algorithm:**
1. Detect ORB/SIFT features in current frame
2. Match with previous frame features
3. Compute Essential Matrix (RANSAC)
4. Recover rotation and translation
5. Integrate to get absolute pose
### Optical Flow
1. Capture ground images from downward camera
2. Measure pixel displacement between frames
3. Convert to velocity using altitude
4. Integrate for position
```
Downward Camera
│
▼
┌───────┐
│ Image │ → Lucas-Kanade flow
└───────┘
│
▼
Pixel velocity × Altitude / Focal length = Ground velocity
```
**Works best at:**
- Altitudes 0.5m - 10m
- Textured ground surfaces
- Stable lighting
### Sensor Fusion
Extended Kalman Filter combines:
- Visual odometry (position)
- Optical flow (velocity)
- IMU (acceleration, rotation)
- Barometer (altitude)
```
Visual Odometry ─┬─→ Weighted Average ─→ Position Estimate
Optical Flow ────┤
IMU ─────────────┘
```
Output: Full 6-DOF pose estimate
**Weights (configurable):**
- Visual Odometry: 60%
- Optical Flow: 30%
- IMU: 10%
## ArduPilot Configuration
Key parameters for GPS-denied operation:
### EKF3 External Navigation
```
# EKF Source Configuration
EK3_SRC1_POSXY = 6 # External Nav for position
EK3_SRC1_VELXY = 6 # External Nav for velocity
EK3_SRC1_POSZ = 1 # Barometer for altitude
# GPS Type - Disabled
GPS_TYPE 0
GPS_TYPE2 0
# Disable GPS for navigation
GPS_TYPE = 0 # No GPS (or keep for geofence)
# EKF3 Source Configuration
AHRS_EKF_TYPE 3 # Use EKF3
EK3_ENABLE 1
EK2_ENABLE 0
# Enable external navigation
VISO_TYPE = 1 # Enable visual odometry input
# Position from External Nav
EK3_SRC1_POSXY 6 # External Nav
EK3_SRC1_POSZ 1 # Barometer
EK3_SRC1_VELXY 6 # External Nav
EK3_SRC1_VELZ 0 # None
EK3_SRC1_YAW 6 # External Nav
# Arming checks
ARMING_CHECK = 0 # Disable pre-arm checks (for testing)
# Vision Position Input
VISO_TYPE 1 # MAVLink
VISO_POS_X 0.1 # Camera offset
VISO_DELAY_MS 50 # Processing delay
```
See `config/ardupilot_gps_denied.parm` for complete parameters.
### Arming Checks
## Sending Position to ArduPilot
```
# For simulation, disable all
ARMING_CHECK 0
Visual odometry sends position via MAVLink:
# For real flight, keep safety checks
# ARMING_CHECK 14 # Skip GPS only
```
## Coordinate Frames
### Local NED Frame
```
North (X+)
↑
│
│
West ←─────┼─────→ East (Y+)
│
│
↓
(Down is Z+)
```
**All navigation uses LOCAL coordinates:**
- Takeoff point is origin (0, 0, 0)
- No global GPS coordinates
- Relative waypoints only
### Example Waypoints
```python
# VISION_POSITION_ESTIMATE message
msg = mavutil.mavlink.MAVLink_vision_position_estimate_message(
usec=timestamp_us,
x=position_x, # meters, NED frame
y=position_y,
z=position_z,
roll=roll, # radians
pitch=pitch,
yaw=yaw
)
```
## Drift Mitigation
Visual odometry accumulates drift over time. Strategies:
1. **Loop Closure**: Recognize previously visited locations
2. **Landmark Matching**: Use known visual markers
3. **Multi-Sensor Fusion**: Weight sensors by confidence
4. **Periodic Reset**: Return to known position
## Geofencing
GPS is only used for safety boundaries:
```yaml
geofence:
enabled: true
use_gps: true
fence_type: polygon
action: RTL
max_altitude: 50
```
If drone crosses boundary, triggers return-to-launch.
## Coordinate System
All waypoints use local NED coordinates:
- X: North (meters from origin)
- Y: East (meters from origin)
- Z: Down (negative for altitude)
Example mission:
```python
# Square pattern (5m sides)
waypoints = [
{"x": 0, "y": 0, "z": -5}, # Takeoff to 5m
{"x": 10, "y": 0, "z": -5}, # 10m north
{"x": 10, "y": 10, "z": -5}, # 10m east
{"x": 0, "y": 0, "z": -5}, # Return
{"x": 0, "y": 0, "z": 0}, # Land
(5, 0, -5), # North 5m, altitude 5m
(5, 5, -5), # North-East corner
(0, 5, -5), # East
(0, 0, -5), # Back to start
]
```
## Limitations
- Drift accumulates over distance/time
- Requires visual features (fails in featureless environments)
- Requires sufficient lighting
- Performance degrades with fast motion or blur
### Visual Odometry
- **Scale drift**: Position error grows over time
- **Texture needed**: Poor in featureless environments
- **Lighting**: Affected by shadows, brightness changes
- **Motion blur**: High-speed motion degrades accuracy
### Optical Flow
- **Altitude dependent**: Accuracy varies with height
- **Ground texture**: Needs visible ground features
- **Tilt sensitivity**: Assumes mostly horizontal flight
### Mitigation Strategies
1. **Loop closure**: Return to known positions
2. **Landmark detection**: ArUco markers for correction
3. **Multi-sensor fusion**: Combine VO + OF + IMU
4. **Flight patterns**: Minimize cumulative drift
## Geofencing (GPS Only)
GPS is used **only** for safety:
```python
# Geofence uses GPS coordinates
fence_points = [
(47.397742, 8.545594), # Corner 1 (lat, lon)
(47.398242, 8.545594), # Corner 2
(47.398242, 8.546094), # Corner 3
(47.397742, 8.546094), # Corner 4
]
# But navigation uses local coordinates
current_position = (10.5, 3.2, -5.0) # NED, meters
```
**Breach Actions:**
- `RTL` - Return to local origin
- `LAND` - Land immediately
- `HOLD` - Hold position
## Testing GPS-Denied Mode
### 1. Verify EKF Source
In MAVProxy:
```
param show EK3_SRC*
```
Should show:
```
EK3_SRC1_POSXY 6.0
EK3_SRC1_VELXY 6.0
```
### 2. Check Vision Input
```bash
ros2 topic echo /uav/visual_odometry/pose
```
Should show updating position.
### 3. Monitor EKF Status
```bash
ros2 topic echo /uav/mavros/state
```
Should show `connected: true` and `mode: GUIDED`.

132
docs/setup_guide.md
View File

@@ -1,87 +1,106 @@
# Setup Guide
Complete installation for Ubuntu 22.04/24.04 and WSL2.
## Prerequisites
## One-Command Installation
- Ubuntu 22.04 (Humble) or 24.04 (Jazzy)
- 16GB RAM minimum
- 50GB free disk space
- Internet connection
## Automatic Installation
The `setup.sh` script installs everything automatically:
```bash
git clone https://git.sirblob.co/SirBlob/simulation.git
cd simulation
cd ~/sim/uav_ugv_simulation
bash setup.sh
```
The script installs:
- ROS 2 (Humble or Jazzy)
- Gazebo Harmonic
- ArduPilot SITL
- ardupilot_gazebo plugin
- Python dependencies
### What Gets Installed
Installation takes 20-40 minutes.
1. **ROS 2** (Humble or Jazzy based on Ubuntu version)
2. **Gazebo Harmonic** (modern simulation)
3. **ArduPilot SITL** (flight controller)
4. **ardupilot_gazebo plugin** (ArduPilot-Gazebo bridge)
5. **Python dependencies** (pymavlink, opencv, scipy, etc.)
6. **MAVROS** (ROS 2 - MAVLink bridge)
### Installation Time
- First install: 20-40 minutes
- ArduPilot build: ~15 minutes
- Gazebo plugin build: ~5 minutes
## Manual Installation
If you prefer to install components separately:
### 1. ROS 2
### Step 1: Install ROS 2
```bash
# Add ROS 2 repository
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 $(lsb_release -cs) main" | \
sudo tee /etc/apt/sources.list.d/ros2.list
# Ubuntu 22.04
sudo apt install software-properties-common
sudo add-apt-repository universe
sudo apt update && sudo apt install curl -y
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 ros-humble-ros-base ros-humble-ros-gz
sudo apt install ros-humble-desktop
```
### 2. Gazebo Harmonic
### Step 2: Install Gazebo Harmonic
```bash
sudo wget https://packages.osrfoundation.org/gazebo.gpg \
-O /usr/share/keyrings/pkgs-osrf-archive-keyring.gpg
echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/pkgs-osrf-archive-keyring.gpg] \
http://packages.osrfoundation.org/gazebo/ubuntu-stable $(lsb_release -cs) main" | \
sudo tee /etc/apt/sources.list.d/gazebo-stable.list
sudo apt install -y wget
sudo wget https://packages.osrfoundation.org/gazebo.gpg -O /usr/share/keyrings/pkgs-osrf-archive-keyring.gpg
echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/pkgs-osrf-archive-keyring.gpg] http://packages.osrfoundation.org/gazebo/ubuntu-stable $(lsb_release -cs) main" | sudo tee /etc/apt/sources.list.d/gazebo-stable.list
sudo apt update
sudo apt install gz-harmonic libgz-cmake3-dev libgz-sim8-dev
sudo apt install gz-harmonic
```
### 3. ArduPilot SITL
### Step 3: Install ArduPilot
```bash
git clone --recurse-submodules https://github.com/ArduPilot/ardupilot.git ~/ardupilot
cd ~/ardupilot
cd ~
git clone --recurse-submodules https://github.com/ArduPilot/ardupilot.git
cd ardupilot
Tools/environment_install/install-prereqs-ubuntu.sh -y
. ~/.profile
./waf configure --board sitl
./waf copter
```
### 4. ardupilot_gazebo Plugin
### Step 4: Install ardupilot_gazebo Plugin
```bash
git clone https://github.com/ArduPilot/ardupilot_gazebo.git ~/ardupilot_gazebo
cd ~/ardupilot_gazebo
cd ~
git clone https://github.com/ArduPilot/ardupilot_gazebo.git
cd ardupilot_gazebo
mkdir build && cd build
cmake .. -DCMAKE_BUILD_TYPE=Release
make -j$(nproc)
cmake .. -DCMAKE_BUILD_TYPE=RelWithDebInfo
make -j4
```
### 5. Python Environment
### Step 5: Install Python Dependencies
```bash
cd ~/simulation
cd ~/sim/uav_ugv_simulation
python3 -m venv venv
source venv/bin/activate
pip install -r requirements.txt
```
## Environment Setup
After installation, source the environment:
```bash
source ~/sim/uav_ugv_simulation/activate_venv.sh
```
This sets up:
- Python virtual environment
- Gazebo resource paths
- ArduPilot paths
## Verify Installation
```bash
@@ -91,26 +110,23 @@ gz sim --version
# Check ArduPilot
sim_vehicle.py --help
# Check plugin
ls ~/ardupilot_gazebo/build/libArduPilotPlugin.so
```
## Running the Simulation
```bash
cd ~/simulation
source activate_venv.sh
bash scripts/run_simulation.sh
# Check Python deps
python3 -c "import pymavlink; print('pymavlink OK')"
python3 -c "import cv2; print('opencv OK')"
```
## Uninstall
```bash
bash scripts/uninstall.sh # ArduPilot and plugin only
bash scripts/uninstall.sh --all # Everything including project
# Remove ArduPilot and plugin only
bash scripts/uninstall.sh
# Remove everything including venv
bash scripts/uninstall.sh --all
```
To remove ROS 2 and Gazebo:
```bash
sudo apt remove ros-humble-* gz-harmonic
```
## Next Steps
1. Run a test simulation: `bash scripts/run_autonomous.sh --mission hover`
2. Read the [Usage Guide](usage.md)
3. Check [Troubleshooting](troubleshooting.md) if issues arise

379
docs/troubleshooting.md
View File

@@ -1,236 +1,253 @@
# Troubleshooting
# Troubleshooting Guide
Common issues and solutions.
## Common Issues
## Installation Issues
### Arming Failed
### ROS 2 packages not found
**Symptoms:**
- "PreArm: Gyros inconsistent"
- "PreArm: Need Position Estimate"
- "Arm: Throttle too high"
```
E: Unable to locate package ros-humble-ros-base
```
Check Ubuntu version matches ROS distro:
- Ubuntu 22.04 → ROS 2 Humble
- Ubuntu 24.04 → ROS 2 Jazzy
Re-run setup:
**Solution:**
```bash
bash setup.sh
```
# Wait for EKF initialization (15+ seconds)
# Look for these messages:
# EKF3 IMU0 initialised
# EKF3 IMU1 initialised
# AHRS: EKF3 active
### Gazebo cmake error (gz-cmake3 not found)
```
Could not find a package configuration file provided by "gz-cmake3"
```
Install Gazebo development packages:
```bash
sudo apt install libgz-cmake3-dev libgz-sim8-dev libgz-plugin2-dev
```
### ArduPilot build fails
```bash
cd ~/ardupilot
./waf clean
./waf configure --board sitl
./waf copter
```
### ardupilot_gazebo build fails
Ensure Gazebo dev packages are installed:
```bash
sudo apt install gz-harmonic libgz-cmake3-dev libgz-sim8-dev \
libgz-plugin2-dev libgz-common5-dev libgz-physics7-dev \
libgz-sensors8-dev rapidjson-dev
```
Rebuild:
```bash
cd ~/ardupilot_gazebo
rm -rf build
mkdir build && cd build
cmake .. -DCMAKE_BUILD_TYPE=Release
make -j$(nproc)
```
## Runtime Issues
### Gazebo won't start
Check Gazebo installation:
```bash
gz sim --version
```
Check plugin path:
```bash
echo $GZ_SIM_SYSTEM_PLUGIN_PATH
ls ~/ardupilot_gazebo/build/libArduPilotPlugin.so
```
### Black screen in Gazebo (WSL)
Use software rendering:
```bash
export LIBGL_ALWAYS_SOFTWARE=1
bash scripts/run_simulation.sh
```
Or use the flag:
```bash
bash scripts/run_simulation.sh --software-render
```
### Gazebo crashes with OpenGL error
```bash
export MESA_GL_VERSION_OVERRIDE=3.3
export MESA_GLSL_VERSION_OVERRIDE=330
export LIBGL_ALWAYS_SOFTWARE=1
```
### sim_vehicle.py not found
```bash
export PATH=$PATH:$HOME/ardupilot/Tools/autotest
```
Or source the activation script:
```bash
source activate_venv.sh
```
### MAVProxy not found
```bash
pip3 install --user mavproxy pymavlink
export PATH=$PATH:$HOME/.local/bin
```
### Drone doesn't respond to commands
1. Check ArduPilot is running:
```bash
ps aux | grep arducopter
```
2. Check connection:
```
# In MAVProxy console
status
```
3. Ensure GUIDED mode:
```
# Then force arm:
mode guided
arm throttle force
takeoff 5
```
4. Arm the drone:
```
arm throttle
### Gazebo Won't Start
**Symptoms:**
- "libGL error"
- Black screen
- Segmentation fault
**Solution (WSL/No GPU):**
```bash
# Use software rendering
bash scripts/run_autonomous.sh --software-render
# Or set manually:
export LIBGL_ALWAYS_SOFTWARE=1
export GALLIUM_DRIVER=llvmpipe
```
### Drone immediately disarms
### ArduPilot SITL Won't Connect
Usually means pre-arm checks failing:
```
# In MAVProxy console
arm check
**Symptoms:**
- "Waiting for heartbeat"
- Connection timeout
- "No MAVLink heartbeat"
**Solution:**
```bash
# Kill existing processes
bash scripts/kill_simulation.sh
# Check if port is in use
lsof -i :5760
# Restart simulation
bash scripts/run_autonomous.sh --mission hover
```
Common fixes:
```
# Disable GPS check for GPS-denied operation
param set ARMING_CHECK 0
### MAVROS Connection Failed
**Symptoms:**
- "FCU connection lost"
- MAVROS not publishing topics
**Solution:**
```bash
# Verify SITL is running
pgrep -a arducopter
# Check connection URL
# Should be tcp://127.0.0.1:5760
# Restart MAVROS
ros2 run mavros mavros_node --ros-args -p fcu_url:=tcp://127.0.0.1:5760
```
### Drone drifts or flips on takeoff
### Drone Drifts / Unstable
Check EKF is using vision/external nav:
**Symptoms:**
- Position drift after takeoff
- Oscillations
- Won't hold position
**Causes:**
1. Visual odometry not providing updates
2. EKF not using external nav
3. Poor camera data
**Solution:**
```bash
# Verify VO is publishing
ros2 topic hz /uav/visual_odometry/pose
# Check EKF source
# In MAVProxy:
param show EK3_SRC1_POSXY # Should be 6
# Verify camera is working
ros2 topic hz /uav/camera/forward/image_raw
```
# In MAVProxy console
param show EK3_SRC*
### Module Not Found
**Symptoms:**
- "ModuleNotFoundError: No module named 'pymavlink'"
- "ImportError: No module named 'cv2'"
**Solution:**
```bash
# Activate virtual environment
source activate_venv.sh
# Reinstall dependencies
pip install -r requirements.txt
```
## WSL-Specific Issues
### DISPLAY not set
### Display Not Available
For WSLg (Windows 11):
**Symptoms:**
- "cannot open display"
- GUI won't show
**Solution:**
```bash
# Install VcXsrv on Windows, then:
export DISPLAY=:0
# Or use WSLg (Windows 11)
# Should work automatically
```
For VcXsrv (Windows 10):
```bash
export DISPLAY=$(cat /etc/resolv.conf | grep nameserver | awk '{print $2}'):0
```
### Graphics Performance
### VcXsrv connection refused
1. Ensure XLaunch is running
2. Disable access control in XLaunch
3. Check Windows Firewall allows VcXsrv
### Slow graphics performance
**Symptoms:**
- Very slow rendering
- Low FPS in Gazebo
**Solution:**
```bash
# Use software rendering
bash scripts/run_simulation.sh --software-render
# Or set environment
export LIBGL_ALWAYS_SOFTWARE=1
export GALLIUM_DRIVER=llvmpipe
# Or reduce visual quality
# In Gazebo, disable shadows and effects
```
## Logs and Debugging
## Debugging Commands
### Gazebo verbose output
### Check Running Processes
```bash
gz sim -v4 ~/ardupilot_gazebo/worlds/iris_runway.sdf
# All simulation processes
pgrep -a "gz|ardupilot|mavros|ros2"
# ArduPilot SITL
pgrep -a arducopter
# Gazebo
pgrep -a "gz sim"
```
### ArduPilot logs
Logs are saved in:
```
~/ardupilot/logs/
```
### Check ROS topics
### View Logs
```bash
source activate_venv.sh
# ArduPilot logs
ls ~/.ardupilot/logs/
# ROS 2 logs
ros2 run rqt_console rqt_console
```
### Check Topics
```bash
# List all topics
ros2 topic list
ros2 topic echo /mavros/state
# Check topic rate
ros2 topic hz /uav/mavros/state
# View topic data
ros2 topic echo /uav/mavros/local_position/pose
```
## Reset Everything
### Check Services
```bash
# Stop all processes
# List services
ros2 service list
# Call arming service
ros2 service call /uav/mavros/cmd/arming mavros_msgs/srv/CommandBool "{value: true}"
```
## Reset Procedures
### Soft Reset
```bash
# Kill all processes
bash scripts/kill_simulation.sh
# Clean rebuild of ArduPilot
cd ~/ardupilot
./waf clean
./waf copter
# Wait 5 seconds
sleep 5
# Clean rebuild of plugin
cd ~/ardupilot_gazebo
rm -rf build
mkdir build && cd build
cmake ..
make -j$(nproc)
# Restart
bash scripts/run_autonomous.sh --mission hover
```
## Full Reinstall
### Full Reset
```bash
bash scripts/uninstall.sh
# Kill everything
bash scripts/kill_simulation.sh
pkill -9 -f python
pkill -9 -f ros2
# Clear ArduPilot eeprom
rm -rf ~/.ardupilot/eeprom.bin
# Restart
bash scripts/run_autonomous.sh --mission hover
```
### Reinstall
```bash
# Uninstall
bash scripts/uninstall.sh --all
# Reinstall
bash setup.sh
```
## Getting Help
1. Check the logs in the terminal
2. Verify all processes are running
3. Check ROS 2 topics are publishing
4. Ensure EKF is initialized before arming
5. Use `--software-render` on WSL/no GPU
## Related Documentation
- [Setup Guide](setup_guide.md)
- [Usage Guide](usage.md)
- [WSL Setup](wsl_setup_guide.md)

206
docs/usage.md
View File

@@ -1,136 +1,154 @@
# Usage Guide
How to run and control the simulation.
## Running the Simulation
## Starting the Simulation
### Option 1: Autonomous Mode (Recommended)
The simplest way to run - the UAV automatically arms, takes off, and flies:
```bash
cd ~/simulation
source activate_venv.sh
bash scripts/run_autonomous.sh --mission hover
```
**Mission types:**
- `hover` - Take off to 5m, hover 30 seconds, land
- `square` - Fly a 5m square pattern
- `circle` - Fly a circular pattern (5m radius)
**Options:**
```bash
# Software rendering (WSL/no GPU)
bash scripts/run_autonomous.sh --software-render --mission hover
# Custom altitude and duration
python3 src/autonomous_controller.py --altitude 10 --duration 60 --mission hover
```
### Option 2: Manual Mode (MAVProxy)
For interactive control via MAVProxy:
```bash
bash scripts/run_simulation.sh
```
This launches:
1. Gazebo with the drone model
2. ArduPilot SITL (flight controller)
3. MAVProxy console (for commands)
Wait for EKF initialization messages (~15 seconds):
```
EKF3 IMU0 initialised
EKF3 IMU1 initialised
AHRS: EKF3 active
```
## Simulation Options
Then type commands:
```
mode guided
arm throttle force
takeoff 5
```
### Option 3: ROS 2 Launch
For full ROS 2 integration with MAVROS:
```bash
# Default (iris_runway world)
bash scripts/run_simulation.sh
# Specific world
bash scripts/run_simulation.sh --world iris_runway
# Rover instead of copter
bash scripts/run_simulation.sh --vehicle Rover
# Software rendering (for WSL or no GPU)
bash scripts/run_simulation.sh --software-render
# Show available options
bash scripts/run_simulation.sh --help
source /opt/ros/humble/setup.bash
source activate_venv.sh
ros2 launch uav_ugv_simulation full_simulation.launch.py
```
## Controlling the UAV
## MAVProxy Commands
### MAVProxy Console
| Command | Description |
|---------|-------------|
| `mode guided` | Switch to GUIDED mode |
| `arm throttle force` | Force arm (bypasses checks) |
| `takeoff 5` | Take off to 5 meters |
| `guided 10 5 -10` | Go to position (N, E, Down) |
| `land` | Land at current position |
| `rtl` | Return to launch |
| `disarm` | Disarm motors |
The simulation opens a MAVProxy console. Commands:
## ROS 2 Topics
```
mode guided # Switch to GUIDED mode (required for commands)
arm throttle # Arm motors
takeoff 5 # Takeoff to 5 meters altitude
### UAV Topics
# Fly to position (North, East, Down in meters)
guided 10 0 -5 # 10m north, 0m east, 5m altitude
guided 10 10 -5 # 10m north, 10m east, 5m altitude
guided 0 0 -5 # Return to origin at 5m altitude
| Topic | Type | Description |
|-------|------|-------------|
| `/uav/mavros/state` | `mavros_msgs/State` | Armed/mode status |
| `/uav/mavros/local_position/pose` | `PoseStamped` | Current position |
| `/uav/visual_odometry/pose` | `PoseStamped` | VO position estimate |
| `/uav/setpoint_position` | `PoseStamped` | Target position |
| `/uav/controller/command` | `String` | Control commands |
rtl # Return to launch
land # Land at current position
disarm # Disarm motors (after landing)
```
### UGV Topics
### ROS 2 Interface
| Topic | Type | Description |
|-------|------|-------------|
| `/ugv/odom` | `Odometry` | Current odometry |
| `/ugv/goal_pose` | `PoseStamped` | Target position |
| `/ugv/cmd_vel` | `Twist` | Velocity command |
If MAVROS is running, control via ROS 2:
### Control via ROS 2
```bash
# Arm
ros2 service call /mavros/cmd/arming mavros_msgs/srv/CommandBool "{value: true}"
# Send command to UAV
ros2 topic pub /uav/controller/command std_msgs/String "data: 'takeoff'"
# Set GUIDED mode
ros2 service call /mavros/set_mode mavros_msgs/srv/SetMode "{custom_mode: 'GUIDED'}"
# Send waypoint
ros2 topic pub /uav/setpoint_position geometry_msgs/PoseStamped \
"{header: {frame_id: 'odom'}, pose: {position: {x: 10, y: 5, z: 5}}}"
# Takeoff
ros2 service call /mavros/cmd/takeoff mavros_msgs/srv/CommandTOL "{altitude: 5}"
# Fly to position (local frame, meters)
ros2 topic pub /mavros/setpoint_position/local geometry_msgs/PoseStamped \
"{header: {frame_id: 'map'}, pose: {position: {x: 10, y: 5, z: 5}}}"
# Land
ros2 service call /mavros/cmd/land mavros_msgs/srv/CommandTOL "{}"
# Send UGV goal
ros2 topic pub /ugv/goal_pose geometry_msgs/PoseStamped \
"{header: {frame_id: 'odom'}, pose: {position: {x: 5, y: 5, z: 0}}}"
```
### Monitoring
## Mission Planner
Run coordinated multi-vehicle missions:
```bash
# List topics
ros2 topic list
# View position
ros2 topic echo /mavros/local_position/pose
# View velocity
ros2 topic echo /mavros/local_position/velocity_local
# View IMU
ros2 topic echo /mavros/imu/data
ros2 run uav_ugv_simulation mission_planner
```
## Flight Modes
Send commands:
```bash
# Load demo mission
ros2 topic pub /mission/command std_msgs/String "data: 'load'"
| Mode | Description |
|------|-------------|
| STABILIZE | Manual control with attitude stabilization |
| ALT_HOLD | Maintain altitude, manual position |
| LOITER | Hold position and altitude |
| GUIDED | Accept position commands |
| AUTO | Follow pre-planned mission |
| RTL | Return to launch point |
| LAND | Controlled descent and landing |
# Start mission
ros2 topic pub /mission/command std_msgs/String "data: 'start'"
# Pause/Resume
ros2 topic pub /mission/command std_msgs/String "data: 'pause'"
ros2 topic pub /mission/command std_msgs/String "data: 'resume'"
# Abort
ros2 topic pub /mission/command std_msgs/String "data: 'abort'"
```
## Stopping the Simulation
Press `Ctrl+C` in the terminal running the simulation.
Or run:
```bash
# Kill all processes
bash scripts/kill_simulation.sh
# Or press Ctrl+C in the terminal running the simulation
```
## Camera Topics
## Configuration Files
The UAV has two cameras:
| File | Description |
|------|-------------|
| `config/uav_params.yaml` | UAV navigation/vision parameters |
| `config/ugv_params.yaml` | UGV motion parameters |
| `config/mavros_params.yaml` | MAVROS connection settings |
| `config/geofence_params.yaml` | Geofence boundaries |
| `config/ardupilot_gps_denied.parm` | ArduPilot EKF configuration |
```bash
# Forward camera (visual odometry)
ros2 topic echo /uav/camera/forward/image_raw
## Next Steps
# Downward camera (optical flow)
ros2 topic echo /uav/camera/downward/image_raw
```
## GPS-Denied Navigation
All position commands use local coordinates (meters from takeoff point):
- X: North
- Y: East
- Z: Up (or Down for NED frame)
GPS is only used for geofencing boundaries, not for navigation.
- [Architecture Overview](architecture.md)
- [GPS-Denied Navigation](gps_denied_navigation.md)
- [Troubleshooting](troubleshooting.md)

204
docs/wsl_setup_guide.md
View File

@@ -1,129 +1,187 @@
# WSL Setup Guide
Setup guide for Windows Subsystem for Linux (WSL2).
## Overview
## Prerequisites
This guide covers running the UAV-UGV simulation on Windows Subsystem for Linux (WSL2).
- Windows 10 (version 21H2+) or Windows 11
## Requirements
- Windows 10 (21H2+) or Windows 11
- WSL2 with Ubuntu 22.04
- 16GB RAM minimum
- Optional: NVIDIA GPU with WSL drivers
### Install WSL2
## WSL2 Installation
### 1. Enable WSL2
Open PowerShell as Administrator:
```powershell
wsl --install
```
### 2. Install Ubuntu
```powershell
wsl --install -d Ubuntu-22.04
```
Restart your computer, then open Ubuntu from the Start menu.
### 3. Set WSL2 as Default
## GUI Support
```powershell
wsl --set-default-version 2
```
### Windows 11 (WSLg)
## Graphics Setup
GUI works automatically. No additional setup needed.
### Option A: WSLg (Windows 11 - Recommended)
### Windows 10 (VcXsrv)
WSLg is built into Windows 11 and works automatically.
1. Download and install [VcXsrv](https://sourceforge.net/projects/vcxsrv/)
2. Run XLaunch with these settings:
- Multiple windows
- Start no client
- Disable access control (checked)
3. In WSL, set DISPLAY:
Verify:
```bash
export DISPLAY=$(cat /etc/resolv.conf | grep nameserver | awk '{print $2}'):0
echo $DISPLAY
# Should show something like :0
```
### Option B: VcXsrv (Windows 10)
1. Download VcXsrv: https://sourceforge.net/projects/vcxsrv/
2. Launch with "Disable access control" checked
3. In WSL:
```bash
export DISPLAY=$(grep nameserver /etc/resolv.conf | awk '{print $2}'):0
echo "export DISPLAY=$(grep nameserver /etc/resolv.conf | awk '{print $2}'):0" >> ~/.bashrc
```
## GPU Support
### NVIDIA GPU (Optional)
1. Install NVIDIA drivers for WSL: https://developer.nvidia.com/cuda/wsl
2. Verify in WSL:
```bash
nvidia-smi
```
If working, you can use hardware acceleration.
### Software Rendering (No GPU)
Most reliable for WSL:
```bash
export LIBGL_ALWAYS_SOFTWARE=1
export GALLIUM_DRIVER=llvmpipe
```
## Installation
Same as native Ubuntu:
```bash
git clone https://git.sirblob.co/SirBlob/simulation.git
cd simulation
cd ~/sim/uav_ugv_simulation
bash setup.sh
```
The setup script automatically:
- Detects WSL environment
- Installs GUI support packages
- Creates WSL environment file
- Configures DISPLAY variable
## Running Simulation
## Running the Simulation
**Always use software rendering on WSL:**
```bash
cd ~/simulation
source activate_venv.sh
bash scripts/run_simulation.sh
bash scripts/run_autonomous.sh --software-render --mission hover
```
If graphics are slow or Gazebo crashes:
```bash
bash scripts/run_simulation.sh --software-render
## Performance Tips
### 1. Allocate More Memory
Create/edit `%USERPROFILE%\.wslconfig`:
```ini
[wsl2]
memory=12GB
processors=4
swap=8GB
```
Restart WSL:
```powershell
wsl --shutdown
```
### 2. Use Fast Storage
Run simulation from Windows filesystem only if needed:
```bash
# Faster (Linux filesystem)
cd ~/sim/uav_ugv_simulation
# Slower (Windows filesystem)
cd /mnt/c/Projects/simulation
```
### 3. Disable Unnecessary Visuals
In software rendering mode, Gazebo is slower. Consider:
- Running headless: `--headless` flag
- Reducing physics rate
- Simpler world files
## Troubleshooting
### Black screen or no display
### "cannot open display"
Check DISPLAY variable:
```bash
echo $DISPLAY
# For WSLg
export DISPLAY=:0
# For VcXsrv
export DISPLAY=$(grep nameserver /etc/resolv.conf | awk '{print $2}'):0
```
For WSLg (Windows 11), should be `:0`
For VcXsrv (Windows 10), should be `<IP>:0`
### Graphics freeze/crash
Test with:
```bash
xcalc
```
### Gazebo crashes immediately
Use software rendering:
```bash
# Always use software rendering
export LIBGL_ALWAYS_SOFTWARE=1
bash scripts/run_simulation.sh
export GALLIUM_DRIVER=llvmpipe
bash scripts/run_autonomous.sh --software-render
```
### OpenGL errors
```bash
export MESA_GL_VERSION_OVERRIDE=3.3
export MESA_GLSL_VERSION_OVERRIDE=330
```
### VcXsrv connection refused
1. Check Windows Firewall allows VcXsrv
2. Ensure XLaunch is running
3. Disable access control in XLaunch settings
### Slow performance
- Close unnecessary Windows applications
- Allocate more RAM to WSL in `.wslconfig`:
```ini
[wsl2]
memory=8GB
processors=4
```
- Increase WSL memory (see Performance Tips)
- Use software rendering
- Close other applications
- Use software rendering flag
## Environment Variables
The `activate_venv.sh` script sets these automatically:
### Network issues
```bash
export DISPLAY=:0 # or IP:0 for VcXsrv
export LIBGL_ALWAYS_INDIRECT=0
export MESA_GL_VERSION_OVERRIDE=3.3
# If MAVLink connection fails
# Check Windows firewall allows WSL traffic
```
## Uninstall
## Quick Start Commands
```bash
bash scripts/uninstall.sh --all
# 1. Open Ubuntu terminal
# 2. Navigate to project
cd ~/sim/uav_ugv_simulation
# 3. Activate environment
source activate_venv.sh
# 4. Run with software rendering
bash scripts/run_autonomous.sh --software-render --mission hover
```
## Related
- [Setup Guide](setup_guide.md)
- [Troubleshooting](troubleshooting.md)

28
launch/full_simulation.launch.py
View File

@@ -31,17 +31,21 @@ def generate_launch_description():
'use_ground_truth', default_value='true', description='Use Gazebo ground truth'
)
# Gazebo Harmonic (gz sim) instead of Gazebo Classic
gazebo = ExecuteProcess(
cmd=['gazebo', '--verbose', LaunchConfiguration('world')],
cmd=[
'gz', 'sim', '-v4', '-r',
os.path.expanduser('~/ardupilot_gazebo/worlds/iris_runway.sdf')
],
output='screen',
additional_env={
'GAZEBO_MODEL_PATH': f"{pkg_share}/models:" + os.path.expanduser('~/ardupilot_gazebo/models'),
'GAZEBO_RESOURCE_PATH': f"{pkg_share}/worlds:" + os.path.expanduser('~/ardupilot_gazebo/worlds')
'GZ_SIM_RESOURCE_PATH': f"{pkg_share}/models:{os.path.expanduser('~/ardupilot_gazebo/models')}:{os.path.expanduser('~/ardupilot_gazebo/worlds')}",
'GZ_SIM_SYSTEM_PLUGIN_PATH': os.path.expanduser('~/ardupilot_gazebo/build')
}
)
ardupilot_uav = TimerAction(
period=3.0,
period=5.0, # Wait for Gazebo to initialize
actions=[
ExecuteProcess(
cmd=[
@@ -49,20 +53,20 @@ def generate_launch_description():
'-v', 'ArduCopter',
'-f', 'gazebo-iris',
'--model', 'JSON',
'--map', '--console',
'--no-mavproxy', # Don't start MAVProxy, MAVROS will connect
'-I0',
'--out', '127.0.0.1:14550',
'--out', '127.0.0.1:14551',
'--add-param-file', os.path.join(pkg_share, 'config', 'ardupilot_gps_denied.parm')
],
cwd=os.path.expanduser('~/ardupilot/ArduCopter'),
output='screen'
cwd=os.path.expanduser('~/ardupilot'),
output='screen',
additional_env={
'PATH': os.environ.get('PATH', '') + ':' + os.path.expanduser('~/ardupilot/Tools/autotest')
}
)
]
)
mavros_uav = TimerAction(
period=8.0,
period=15.0, # Wait for ArduPilot SITL to start
actions=[
Node(
package='mavros',
@@ -73,7 +77,7 @@ def generate_launch_description():
parameters=[
os.path.join(pkg_share, 'config', 'mavros_params.yaml'),
{
'fcu_url': 'udp://:14550@127.0.0.1:14555',
'fcu_url': 'tcp://127.0.0.1:5760', # SITL TCP port
'gcs_url': '',
'target_system_id': 1,
'target_component_id': 1,

30
launch/uav_only.launch.py
View File

@@ -14,36 +14,42 @@ def generate_launch_description():
world_arg = DeclareLaunchArgument(
'world',
default_value=os.path.join(pkg_share, 'worlds', 'empty_custom.world'),
description='Path to world file'
default_value='iris_runway.sdf',
description='World file name'
)
# Gazebo Harmonic
gazebo = ExecuteProcess(
cmd=['gazebo', '--verbose', LaunchConfiguration('world')],
cmd=[
'gz', 'sim', '-v4', '-r',
os.path.expanduser('~/ardupilot_gazebo/worlds/iris_runway.sdf')
],
output='screen',
additional_env={
'GAZEBO_MODEL_PATH': f"{pkg_share}/models:" + os.path.expanduser('~/ardupilot_gazebo/models'),
'GZ_SIM_RESOURCE_PATH': f"{pkg_share}/models:{os.path.expanduser('~/ardupilot_gazebo/models')}:{os.path.expanduser('~/ardupilot_gazebo/worlds')}",
'GZ_SIM_SYSTEM_PLUGIN_PATH': os.path.expanduser('~/ardupilot_gazebo/build')
}
)
ardupilot = TimerAction(
period=3.0,
period=5.0,
actions=[
ExecuteProcess(
cmd=[
'sim_vehicle.py', '-v', 'ArduCopter', '-f', 'gazebo-iris',
'--model', 'JSON', '--map', '--console', '-I0',
'--out', '127.0.0.1:14550',
'--add-param-file', os.path.join(pkg_share, 'config', 'ardupilot_gps_denied.parm')
'--model', 'JSON', '--no-mavproxy', '-I0',
],
cwd=os.path.expanduser('~/ardupilot/ArduCopter'),
output='screen'
cwd=os.path.expanduser('~/ardupilot'),
output='screen',
additional_env={
'PATH': os.environ.get('PATH', '') + ':' + os.path.expanduser('~/ardupilot/Tools/autotest')
}
)
]
)
mavros = TimerAction(
period=8.0,
period=15.0,
actions=[
Node(
package='mavros',
@@ -53,7 +59,7 @@ def generate_launch_description():
output='screen',
parameters=[
os.path.join(pkg_share, 'config', 'mavros_params.yaml'),
{'fcu_url': 'udp://:14550@127.0.0.1:14555'}
{'fcu_url': 'tcp://127.0.0.1:5760'}
]
)
]

4
scripts/kill_simulation.sh
View File

@@ -2,6 +2,8 @@
echo "Killing all simulation processes..."
pkill -9 -f "gz sim" 2>/dev/null || true
pkill -9 -f "ruby" 2>/dev/null || true # gz sim uses ruby
pkill -9 -f "gazebo" 2>/dev/null || true
pkill -9 -f "gzserver" 2>/dev/null || true
pkill -9 -f "gzclient" 2>/dev/null || true
@@ -10,6 +12,8 @@ pkill -9 -f "mavproxy" 2>/dev/null || true
pkill -9 -f "mavros" 2>/dev/null || true
pkill -9 -f "ArduCopter" 2>/dev/null || true
pkill -9 -f "ArduRover" 2>/dev/null || true
pkill -9 -f "arducopter" 2>/dev/null || true
pkill -9 -f "autonomous_controller" 2>/dev/null || true
pkill -9 -f "ros2" 2>/dev/null || true
sleep 1

122
scripts/run_autonomous.sh Executable file
View File

@@ -0,0 +1,122 @@
#!/bin/bash
# Autonomous UAV-UGV Simulation Runner
# Launches Gazebo + ArduPilot SITL + Autonomous Controller
set -e
SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
PROJECT_DIR="$(dirname "$SCRIPT_DIR")"
# Colors
RED='\033[0;31m'
GREEN='\033[0;32m'
YELLOW='\033[1;33m'
CYAN='\033[0;36m'
NC='\033[0m'
print_info() { echo -e "${CYAN}[INFO]${NC} $1"; }
print_success() { echo -e "${GREEN}[SUCCESS]${NC} $1"; }
print_error() { echo -e "${RED}[ERROR]${NC} $1"; }
# Parse arguments
SOFTWARE_RENDER=false
WORLD="iris_runway.sdf"
MISSION="hover" # hover, square, circle
while [[ $# -gt 0 ]]; do
case $1 in
--software-render) SOFTWARE_RENDER=true; shift ;;
--world) WORLD="$2"; shift 2 ;;
--mission) MISSION="$2"; shift 2 ;;
-h|--help)
echo "Usage: $0 [options]"
echo " --software-render Use software rendering (WSL/no GPU)"
echo " --world <file> World file to load (default: iris_runway.sdf)"
echo " --mission <type> Mission type: hover, square, circle (default: hover)"
exit 0
;;
*) shift ;;
esac
done
# Cleanup function
cleanup() {
print_info "Cleaning up..."
pkill -f "gz sim" 2>/dev/null || true
pkill -f "arducopter" 2>/dev/null || true
pkill -f "sim_vehicle.py" 2>/dev/null || true
pkill -f "autonomous_controller.py" 2>/dev/null || true
}
trap cleanup EXIT
# Setup environment
export PATH=$PATH:$HOME/ardupilot/Tools/autotest:$HOME/.local/bin
export GZ_SIM_RESOURCE_PATH=$HOME/ardupilot_gazebo/models:$HOME/ardupilot_gazebo/worlds
export GZ_SIM_SYSTEM_PLUGIN_PATH=$HOME/ardupilot_gazebo/build
if [ "$SOFTWARE_RENDER" = true ]; then
print_info "Using software rendering (Mesa)"
export LIBGL_ALWAYS_SOFTWARE=1
export GALLIUM_DRIVER=llvmpipe
fi
# Kill any existing processes
cleanup 2>/dev/null
print_info "==================================="
print_info " Autonomous UAV-UGV Simulation"
print_info "==================================="
print_info "World: $WORLD"
print_info "Mission: $MISSION"
echo ""
# Step 1: Start Gazebo
print_info "Starting Gazebo Harmonic..."
gz sim -v4 -r $HOME/ardupilot_gazebo/worlds/$WORLD &
GZ_PID=$!
sleep 5
# Check if Gazebo started
if ! kill -0 $GZ_PID 2>/dev/null; then
print_error "Gazebo failed to start"
exit 1
fi
print_success "Gazebo running (PID: $GZ_PID)"
# Step 2: Start ArduPilot SITL
print_info "Starting ArduPilot SITL..."
cd ~/ardupilot
sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --no-mavproxy -I0 &
SITL_PID=$!
sleep 10
# Check if SITL started
if ! kill -0 $SITL_PID 2>/dev/null; then
print_error "ArduPilot SITL failed to start"
exit 1
fi
print_success "ArduPilot SITL running (PID: $SITL_PID)"
# Step 3: Start the autonomous controller
print_info "Starting Autonomous Controller..."
print_info "Mission: $MISSION"
python3 "$PROJECT_DIR/src/autonomous_controller.py" --mission "$MISSION" &
CTRL_PID=$!
print_success "Autonomous Controller started (PID: $CTRL_PID)"
print_info ""
print_info "==================================="
print_info " Simulation Running"
print_info "==================================="
print_info "The UAV will automatically:"
print_info " 1. Wait for EKF initialization"
print_info " 2. Arm and enter GUIDED mode"
print_info " 3. Take off to 5 meters"
print_info " 4. Execute mission: $MISSION"
print_info ""
print_info "Press Ctrl+C to stop"
print_info "==================================="
# Wait for controller to finish or user interrupt
wait $CTRL_PID

36
scripts/setup_gazebo_nvidia.sh Executable file
View File

@@ -0,0 +1,36 @@
#!/bin/bash
# Setup script for NVIDIA GPU with Gazebo
set -e
echo "Setting up NVIDIA GPU for Gazebo..."
# Check if NVIDIA GPU is available
if ! command -v nvidia-smi &> /dev/null; then
echo "NVIDIA driver not found. Please install NVIDIA drivers first."
exit 1
fi
nvidia-smi
# Set environment variables for NVIDIA
export __NV_PRIME_RENDER_OFFLOAD=1
export __GLX_VENDOR_LIBRARY_NAME=nvidia
# For Gazebo Harmonic
export LIBGL_ALWAYS_SOFTWARE=0
# Create persistent config
cat >> ~/.bashrc << 'EOF'
# NVIDIA GPU for Gazebo
export __NV_PRIME_RENDER_OFFLOAD=1
export __GLX_VENDOR_LIBRARY_NAME=nvidia
export LIBGL_ALWAYS_SOFTWARE=0
EOF
echo ""
echo "NVIDIA setup complete!"
echo "Please restart your terminal or run: source ~/.bashrc"
echo ""
echo "Test with: glxinfo | grep 'OpenGL renderer'"

368
src/autonomous_controller.py Executable file
View File

@@ -0,0 +1,368 @@
#!/usr/bin/env python3
"""
Autonomous UAV Controller using pymavlink.
Connects directly to ArduPilot SITL and controls the drone autonomously.
No ROS/MAVROS required.
"""
import time
import math
import argparse
from pymavlink import mavutil
class AutonomousController:
"""Autonomous UAV controller using pymavlink."""
def __init__(self, connection_string='tcp:127.0.0.1:5760'):
"""Initialize connection to ArduPilot."""
print(f"[CTRL] Connecting to {connection_string}...")
self.mav = mavutil.mavlink_connection(connection_string)
# Wait for heartbeat
print("[CTRL] Waiting for heartbeat...")
self.mav.wait_heartbeat()
print(f"[CTRL] Connected! System {self.mav.target_system} Component {self.mav.target_component}")
self.home_position = None
def wait_for_ekf(self, timeout=60):
"""Wait for EKF to initialize."""
print("[CTRL] Waiting for EKF initialization...")
start_time = time.time()
while time.time() - start_time < timeout:
# Request EKF status
msg = self.mav.recv_match(type='EKF_STATUS_REPORT', blocking=True, timeout=1)
if msg:
# Check if EKF is healthy (flags indicate position and velocity estimates are good)
if msg.flags & 0x01: # EKF_ATTITUDE
print(f"[CTRL] EKF Status: flags={msg.flags:#x}")
if msg.flags >= 0x1FF: # All basic flags set
print("[CTRL] EKF initialized successfully!")
return True
# Also check for GPS status messages to see progress
msg = self.mav.recv_match(type='STATUSTEXT', blocking=False)
if msg:
text = msg.text if isinstance(msg.text, str) else msg.text.decode('utf-8', errors='ignore')
print(f"[SITL] {text}")
if 'EKF3 IMU' in text and 'initialised' in text:
time.sleep(2) # Give it a moment after EKF init
return True
print("[CTRL] EKF initialization timeout!")
return False
def set_mode(self, mode):
"""Set flight mode."""
mode_mapping = self.mav.mode_mapping()
if mode not in mode_mapping:
print(f"[CTRL] Unknown mode: {mode}")
return False
mode_id = mode_mapping[mode]
self.mav.mav.set_mode_send(
self.mav.target_system,
mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED,
mode_id
)
# Wait for ACK
for _ in range(10):
msg = self.mav.recv_match(type='HEARTBEAT', blocking=True, timeout=1)
if msg and msg.custom_mode == mode_id:
print(f"[CTRL] Mode set to {mode}")
return True
print(f"[CTRL] Failed to set mode {mode}")
return False
def arm(self, force=True):
"""Arm the vehicle."""
print("[CTRL] Arming...")
# Disable arming checks if force
if force:
self.mav.mav.param_set_send(
self.mav.target_system,
self.mav.target_component,
b'ARMING_CHECK',
0,
mavutil.mavlink.MAV_PARAM_TYPE_INT32
)
time.sleep(0.5)
self.mav.mav.command_long_send(
self.mav.target_system,
self.mav.target_component,
mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
0, # confirmation
1, # arm
21196 if force else 0, # force arm magic number
0, 0, 0, 0, 0
)
# Wait for arm
for _ in range(30):
msg = self.mav.recv_match(type='HEARTBEAT', blocking=True, timeout=1)
if msg and msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED:
print("[CTRL] Armed successfully!")
return True
# Check for failure messages
ack = self.mav.recv_match(type='COMMAND_ACK', blocking=False)
if ack and ack.command == mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM:
if ack.result != mavutil.mavlink.MAV_RESULT_ACCEPTED:
print(f"[CTRL] Arm rejected: {ack.result}")
print("[CTRL] Arm timeout")
return False
def takeoff(self, altitude=5.0):
"""Take off to specified altitude."""
print(f"[CTRL] Taking off to {altitude}m...")
self.mav.mav.command_long_send(
self.mav.target_system,
self.mav.target_component,
mavutil.mavlink.MAV_CMD_NAV_TAKEOFF,
0,
0, 0, 0, 0, 0, 0,
altitude
)
# Wait for takeoff
start_time = time.time()
while time.time() - start_time < 30:
msg = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True, timeout=1)
if msg:
current_alt = msg.relative_alt / 1000.0 # mm to m
print(f"[CTRL] Altitude: {current_alt:.1f}m / {altitude}m")
if current_alt >= altitude * 0.9:
print("[CTRL] Takeoff complete!")
return True
print("[CTRL] Takeoff timeout")
return False
def goto_local(self, north, east, down, yaw=0):
"""Go to local position (NED frame)."""
print(f"[CTRL] Going to N:{north:.1f} E:{east:.1f} D:{down:.1f}")
# Use SET_POSITION_TARGET_LOCAL_NED
self.mav.mav.set_position_target_local_ned_send(
0, # timestamp
self.mav.target_system,
self.mav.target_component,
mavutil.mavlink.MAV_FRAME_LOCAL_NED,
0b0000111111111000, # position only mask
north, east, down, # position
0, 0, 0, # velocity
0, 0, 0, # acceleration
yaw, 0 # yaw, yaw_rate
)
def wait_for_position(self, north, east, down, tolerance=1.0, timeout=30):
"""Wait until vehicle reaches position."""
start_time = time.time()
while time.time() - start_time < timeout:
msg = self.mav.recv_match(type='LOCAL_POSITION_NED', blocking=True, timeout=1)
if msg:
dist = math.sqrt(
(msg.x - north)**2 +
(msg.y - east)**2 +
(msg.z - down)**2
)
print(f"[CTRL] Position: N:{msg.x:.1f} E:{msg.y:.1f} D:{msg.z:.1f} (dist:{dist:.1f})")
if dist < tolerance:
return True
return False
def land(self):
"""Land the vehicle."""
print("[CTRL] Landing...")
self.mav.mav.command_long_send(
self.mav.target_system,
self.mav.target_component,
mavutil.mavlink.MAV_CMD_NAV_LAND,
0,
0, 0, 0, 0, 0, 0, 0
)
# Wait for land
start_time = time.time()
while time.time() - start_time < 60:
msg = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True, timeout=1)
if msg:
current_alt = msg.relative_alt / 1000.0
print(f"[CTRL] Landing... Alt: {current_alt:.1f}m")
if current_alt < 0.3:
print("[CTRL] Landed!")
return True
return False
def disarm(self):
"""Disarm the vehicle."""
print("[CTRL] Disarming...")
self.mav.mav.command_long_send(
self.mav.target_system,
self.mav.target_component,
mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
0,
0, # disarm
21196, # force
0, 0, 0, 0, 0
)
time.sleep(1)
print("[CTRL] Disarmed")
def run_hover_mission(self, altitude=5.0, duration=30):
"""Simple hover mission."""
print(f"\n[MISSION] Hover at {altitude}m for {duration}s")
if not self.wait_for_ekf():
return False
if not self.set_mode('GUIDED'):
return False
if not self.arm(force=True):
return False
if not self.takeoff(altitude):
return False
print(f"[MISSION] Hovering for {duration} seconds...")
time.sleep(duration)
self.land()
self.disarm()
print("[MISSION] Hover mission complete!")
return True
def run_square_mission(self, altitude=5.0, side=5.0):
"""Fly a square pattern."""
print(f"\n[MISSION] Square pattern: {side}m sides at {altitude}m")
if not self.wait_for_ekf():
return False
if not self.set_mode('GUIDED'):
return False
if not self.arm(force=True):
return False
if not self.takeoff(altitude):
return False
# Square waypoints (NED: North, East, Down)
waypoints = [
(side, 0, -altitude), # North
(side, side, -altitude), # North-East
(0, side, -altitude), # East
(0, 0, -altitude), # Back to start
]
for i, (n, e, d) in enumerate(waypoints):
print(f"\n[MISSION] Waypoint {i+1}/4: N:{n} E:{e}")
self.goto_local(n, e, d)
time.sleep(0.5) # Let command process
self.wait_for_position(n, e, d, tolerance=1.5, timeout=20)
time.sleep(2) # Hover at waypoint
self.land()
self.disarm()
print("[MISSION] Square mission complete!")
return True
def run_circle_mission(self, altitude=5.0, radius=5.0, points=8):
"""Fly a circular pattern."""
print(f"\n[MISSION] Circle pattern: {radius}m radius at {altitude}m")
if not self.wait_for_ekf():
return False
if not self.set_mode('GUIDED'):
return False
if not self.arm(force=True):
return False
if not self.takeoff(altitude):
return False
# Generate circle waypoints
for i in range(points + 1): # +1 to complete the circle
angle = 2 * math.pi * i / points
north = radius * math.cos(angle)
east = radius * math.sin(angle)
print(f"\n[MISSION] Circle point {i+1}/{points+1}")
self.goto_local(north, east, -altitude)
time.sleep(0.5)
self.wait_for_position(north, east, -altitude, tolerance=1.5, timeout=15)
time.sleep(1)
# Return to center
self.goto_local(0, 0, -altitude)
self.wait_for_position(0, 0, -altitude, tolerance=1.5, timeout=15)
self.land()
self.disarm()
print("[MISSION] Circle mission complete!")
return True
def main():
parser = argparse.ArgumentParser(description='Autonomous UAV Controller')
parser.add_argument('--connection', default='tcp:127.0.0.1:5760',
help='MAVLink connection string')
parser.add_argument('--mission', choices=['hover', 'square', 'circle'],
default='hover', help='Mission type')
parser.add_argument('--altitude', type=float, default=5.0,
help='Flight altitude in meters')
parser.add_argument('--size', type=float, default=5.0,
help='Mission size (side length or radius)')
parser.add_argument('--duration', type=float, default=30.0,
help='Hover duration in seconds')
args = parser.parse_args()
print("=" * 50)
print(" Autonomous UAV Controller")
print("=" * 50)
print(f" Connection: {args.connection}")
print(f" Mission: {args.mission}")
print(f" Altitude: {args.altitude}m")
print("=" * 50)
print()
try:
controller = AutonomousController(args.connection)
if args.mission == 'hover':
controller.run_hover_mission(args.altitude, args.duration)
elif args.mission == 'square':
controller.run_square_mission(args.altitude, args.size)
elif args.mission == 'circle':
controller.run_circle_mission(args.altitude, args.size)
except KeyboardInterrupt:
print("\n[CTRL] Interrupted by user")
except Exception as e:
print(f"[ERROR] {e}")
import traceback
traceback.print_exc()
if __name__ == '__main__':
main()

55
src/control/uav_controller.py
View File

@@ -29,16 +29,24 @@ class UAVController(Node):
self.declare_parameter('takeoff_altitude', 5.0)
self.declare_parameter('position_tolerance', 0.3)
self.declare_parameter('namespace', '/uav')
self.declare_parameter('auto_arm', True)
self.declare_parameter('auto_takeoff', True)
self.declare_parameter('startup_delay', 15.0) # Wait for EKF
self.takeoff_altitude = self.get_parameter('takeoff_altitude').value
self.position_tolerance = self.get_parameter('position_tolerance').value
ns = self.get_parameter('namespace').value
self.auto_arm = self.get_parameter('auto_arm').value
self.auto_takeoff = self.get_parameter('auto_takeoff').value
self.startup_delay = self.get_parameter('startup_delay').value
self.state = FlightState.DISARMED
self.mavros_state = None
self.current_pose = None
self.target_pose = None
self.home_position = None
self.startup_complete = False
self.ekf_ready = False
self.state_sub = self.create_subscription(
State, f'{ns}/mavros/state', self.state_callback, 10)
@@ -72,8 +80,55 @@ class UAVController(Node):
self.setpoint_timer = self.create_timer(0.05, self.publish_setpoint)
self.status_timer = self.create_timer(0.5, self.publish_status)
# Delayed auto-start (wait for EKF initialization)
if self.auto_arm or self.auto_takeoff:
self.get_logger().info(f'Auto-mission enabled. Starting in {self.startup_delay}s...')
self.startup_timer = self.create_timer(self.startup_delay, self.auto_startup)
self.get_logger().info('UAV Controller Started - GPS-denied mode')
def auto_startup(self):
"""Auto arm and takeoff after startup delay."""
# Only run once
if self.startup_complete:
return
self.startup_complete = True
self.startup_timer.cancel()
self.get_logger().info('Auto-startup: Beginning autonomous sequence...')
# Check if MAVROS is connected
if self.mavros_state is None:
self.get_logger().warn('MAVROS not connected yet, retrying in 5s...')
self.startup_timer = self.create_timer(5.0, self.auto_startup)
self.startup_complete = False
return
# Set GUIDED mode first
self.get_logger().info('Auto-startup: Setting GUIDED mode...')
self.set_mode('GUIDED')
# Wait a moment then arm
self.create_timer(2.0, self._auto_arm_callback, callback_group=None)
def _auto_arm_callback(self):
"""Callback to arm after mode is set."""
if self.auto_arm:
self.get_logger().info('Auto-startup: Arming...')
self.arm()
# Wait then takeoff
if self.auto_takeoff:
self.create_timer(3.0, self._auto_takeoff_callback, callback_group=None)
def _auto_takeoff_callback(self):
"""Callback to takeoff after arming."""
if self.mavros_state and self.mavros_state.armed:
self.get_logger().info('Auto-startup: Taking off...')
self.takeoff()
else:
self.get_logger().warn('Auto-startup: Not armed, cannot takeoff')
def state_callback(self, msg):
self.mavros_state = msg
if msg.armed and self.state == FlightState.DISARMED: