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

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)