Controller Update
This commit is contained in:
3
.gitignore
vendored
Normal file
3
.gitignore
vendored
Normal file
@@ -0,0 +1,3 @@
|
|||||||
|
__pycache__
|
||||||
|
.venv
|
||||||
|
|
||||||
143
README.md
143
README.md
@@ -1,30 +1,30 @@
|
|||||||
# UAV-UGV Simulation
|
# 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:
|
- **GPS-Denied Navigation**: Visual odometry, optical flow, and EKF sensor fusion
|
||||||
- Visual odometry from cameras
|
- **GPS-Based Geofencing**: Safety boundaries using GPS (navigation remains GPS-free)
|
||||||
- Optical flow sensors
|
- **Multi-Vehicle Support**: Coordinated UAV and UGV operations
|
||||||
- IMU integration
|
- **ArduPilot SITL**: Full flight controller simulation
|
||||||
- Local coordinate frames
|
- **Gazebo Harmonic**: Modern physics simulation
|
||||||
|
- **ROS 2 Integration**: Full ROS 2 Humble/Jazzy support
|
||||||
**GPS Usage**: GPS is ONLY used for geofencing (safety boundaries), NOT for navigation or position control.
|
|
||||||
|
|
||||||
## Requirements
|
## Requirements
|
||||||
|
|
||||||
- Ubuntu 22.04 or 24.04 (WSL2 supported)
|
- Ubuntu 22.04 or 24.04 (WSL2 supported)
|
||||||
- 16GB RAM recommended
|
- 16GB RAM recommended
|
||||||
- 50GB disk space
|
- 50GB disk space
|
||||||
|
- NVIDIA GPU recommended (software rendering available)
|
||||||
|
|
||||||
## Installation
|
## Installation
|
||||||
|
|
||||||
```bash
|
```bash
|
||||||
git clone https://git.sirblob.co/SirBlob/simulation.git
|
git clone <repository-url> ~/sim/uav_ugv_simulation
|
||||||
cd simulation
|
cd ~/sim/uav_ugv_simulation
|
||||||
bash setup.sh
|
bash setup.sh
|
||||||
```
|
```
|
||||||
|
|
||||||
@@ -32,72 +32,101 @@ Installation takes 20-40 minutes (builds ArduPilot from source).
|
|||||||
|
|
||||||
## Quick Start
|
## Quick Start
|
||||||
|
|
||||||
|
### Autonomous Mode (Recommended)
|
||||||
|
|
||||||
|
The UAV automatically arms, takes off, and executes a mission:
|
||||||
|
|
||||||
```bash
|
```bash
|
||||||
cd ~/simulation
|
cd ~/sim/uav_ugv_simulation
|
||||||
source activate_venv.sh
|
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
|
bash scripts/run_simulation.sh
|
||||||
|
# Wait for EKF initialization (~15 seconds), then:
|
||||||
|
# mode guided
|
||||||
|
# arm throttle force
|
||||||
|
# takeoff 5
|
||||||
```
|
```
|
||||||
|
|
||||||
For WSL with graphics issues:
|
### ROS 2 Mode
|
||||||
```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:
|
|
||||||
|
|
||||||
```bash
|
```bash
|
||||||
# Arm
|
source /opt/ros/humble/setup.bash
|
||||||
ros2 service call /mavros/cmd/arming mavros_msgs/srv/CommandBool "{value: true}"
|
ros2 launch uav_ugv_simulation full_simulation.launch.py
|
||||||
|
|
||||||
# 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}}}"
|
|
||||||
```
|
```
|
||||||
|
|
||||||
## Simulation Options
|
## Mission Types
|
||||||
|
|
||||||
```bash
|
| Mission | Description |
|
||||||
# Default
|
|---------|-------------|
|
||||||
bash scripts/run_simulation.sh
|
| `hover` | Take off, hover for 30 seconds, land |
|
||||||
|
| `square` | Fly a 5m square pattern |
|
||||||
|
| `circle` | Fly a circular pattern |
|
||||||
|
|
||||||
# Custom world
|
## GPS-Denied Navigation
|
||||||
bash scripts/run_simulation.sh --world iris_runway
|
|
||||||
|
|
||||||
# Rover instead of copter
|
**Navigation** uses only relative positioning:
|
||||||
bash scripts/run_simulation.sh --vehicle Rover
|
- Visual odometry from cameras
|
||||||
|
- Optical flow sensors
|
||||||
|
- IMU integration
|
||||||
|
- Local coordinate frames (NED)
|
||||||
|
|
||||||
# Software rendering (WSL)
|
**GPS is ONLY used for geofencing** (safety boundaries), NOT for navigation or position control.
|
||||||
bash scripts/run_simulation.sh --software-render
|
|
||||||
|
## 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
|
| Command | Description |
|
||||||
bash scripts/uninstall.sh # Remove ArduPilot and plugin
|
|---------|-------------|
|
||||||
bash scripts/uninstall.sh --all # Remove everything
|
| `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
|
## Documentation
|
||||||
|
|
||||||
- [Setup Guide](docs/setup_guide.md)
|
- [Setup Guide](docs/setup_guide.md)
|
||||||
- [WSL Setup Guide](docs/wsl_setup_guide.md)
|
|
||||||
- [Usage Guide](docs/usage.md)
|
- [Usage Guide](docs/usage.md)
|
||||||
- [Architecture](docs/architecture.md)
|
- [Architecture](docs/architecture.md)
|
||||||
- [GPS-Denied Navigation](docs/gps_denied_navigation.md)
|
- [GPS-Denied Navigation](docs/gps_denied_navigation.md)
|
||||||
|
- [WSL Setup](docs/wsl_setup_guide.md)
|
||||||
- [Troubleshooting](docs/troubleshooting.md)
|
- [Troubleshooting](docs/troubleshooting.md)
|
||||||
|
|
||||||
|
## License
|
||||||
|
|
||||||
|
MIT License
|
||||||
|
|||||||
@@ -88,8 +88,8 @@ FLTMODE6 9 # Land
|
|||||||
# ====================
|
# ====================
|
||||||
# Arming Checks
|
# Arming Checks
|
||||||
# ====================
|
# ====================
|
||||||
# Relax arming checks for GPS-denied operation
|
# Disable arming checks for simulation testing
|
||||||
ARMING_CHECK 14 # Skip GPS check (bit 2 = 4)
|
ARMING_CHECK 0 # Disable all checks (for sim only)
|
||||||
|
|
||||||
# ====================
|
# ====================
|
||||||
# Logging
|
# Logging
|
||||||
|
|||||||
@@ -1,120 +1,177 @@
|
|||||||
# Architecture
|
# System Architecture
|
||||||
|
|
||||||
System architecture for GPS-denied UAV/UGV navigation.
|
|
||||||
|
|
||||||
## Overview
|
## Overview
|
||||||
|
|
||||||
The system uses vision-based navigation with GPS reserved only for geofencing.
|
|
||||||
|
|
||||||
```
|
```
|
||||||
Vision Sensors (Cameras)
|
┌─────────────────────────────────────────────────────────────────┐
|
||||||
|
|
│ Simulation │
|
||||||
v
|
├───────────────────┬─────────────────────┬───────────────────────┤
|
||||||
Visual Odometry & Optical Flow
|
│ Gazebo Harmonic │ ArduPilot SITL │ ROS 2 Nodes │
|
||||||
|
|
│ (Physics/3D) │ (Flight Control) │ (Perception/Nav) │
|
||||||
v
|
└───────────────────┴─────────────────────┴───────────────────────┘
|
||||||
Position Estimator (EKF)
|
│ │ │
|
||||||
|
|
└────────────────────┼──────────────────────┘
|
||||||
v
|
│
|
||||||
ArduPilot Flight Controller
|
┌─────────┴─────────┐
|
||||||
|
|
│ MAVLink/MAVROS │
|
||||||
v
|
└───────────────────┘
|
||||||
Motor Control
|
|
||||||
```
|
```
|
||||||
|
|
||||||
## Components
|
## Components
|
||||||
|
|
||||||
### Perception
|
### 1. Gazebo Harmonic
|
||||||
|
|
||||||
- **Forward Camera**: Visual odometry, feature tracking
|
**Role:** 3D simulation, physics, sensors
|
||||||
- **Downward Camera**: Optical flow, ground plane detection
|
|
||||||
- **IMU**: Angular velocity, acceleration
|
|
||||||
|
|
||||||
### 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
|
### 2. ArduPilot SITL
|
||||||
- **Optical Flow**: Velocity estimation from downward camera
|
|
||||||
- **EKF Fusion**: Combines all sensor inputs into position estimate
|
|
||||||
|
|
||||||
### Navigation
|
**Role:** Flight controller simulation
|
||||||
|
|
||||||
- **Waypoint Navigation**: Relative coordinates (meters from origin)
|
- **EKF3**: State estimation using external vision
|
||||||
- **Path Planning**: Collision-free paths using local map
|
- **Flight modes**: GUIDED, LOITER, RTL, LAND
|
||||||
- **Position Hold**: Maintain position using vision
|
- **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
|
### 3. ROS 2 Nodes
|
||||||
- **MAVROS**: ROS interface to ArduPilot
|
|
||||||
- **Velocity/Position Control**: Low-level motor commands
|
|
||||||
|
|
||||||
### Safety
|
#### Vision Pipeline
|
||||||
|
|
||||||
- **Geofencing**: GPS-based boundary checking (safety only)
|
```
|
||||||
- **Altitude Limits**: Maximum flight ceiling
|
Camera → Visual Odometry → Position Estimator → Controller
|
||||||
- **Failsafe**: RTL on signal loss or boundary breach
|
↓
|
||||||
|
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
|
## Coordinate Frames
|
||||||
|
|
||||||
| Frame | Description |
|
| Frame | Description |
|
||||||
|-------|-------------|
|
|-------|-------------|
|
||||||
| body | Attached to vehicle, X forward, Y right, Z down |
|
| `odom` | Local origin (takeoff point) |
|
||||||
| odom | Origin at takeoff, accumulates drift |
|
| `base_link` | Vehicle body frame |
|
||||||
| map | Fixed world frame (may be corrected) |
|
| `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
|
src/
|
||||||
|
|
├── vision/
|
||||||
v
|
│ ├── visual_odometry.py # Feature tracking VO
|
||||||
Feature Detection (ORB/SIFT)
|
│ ├── optical_flow.py # LK optical flow
|
||||||
|
|
│ └── camera_processor.py # Image processing
|
||||||
v
|
├── localization/
|
||||||
Feature Matching (frame to frame)
|
│ ├── position_estimator.py # Weighted fusion
|
||||||
|
|
│ └── ekf_fusion.py # EKF fusion
|
||||||
v
|
├── navigation/
|
||||||
Motion Estimation (Essential Matrix)
|
│ ├── local_planner.py # Path planning
|
||||||
|
|
│ └── waypoint_follower.py # Waypoint tracking
|
||||||
v
|
├── control/
|
||||||
EKF Update (fuse with IMU)
|
│ ├── uav_controller.py # UAV flight control
|
||||||
|
|
│ ├── ugv_controller.py # UGV drive control
|
||||||
v
|
│ └── mission_planner.py # Coordination
|
||||||
Position Estimate (x, y, z, roll, pitch, yaw)
|
└── safety/
|
||||||
|
|
├── geofence_monitor.py # GPS boundaries
|
||||||
v
|
└── failsafe_handler.py # Emergency handling
|
||||||
ArduPilot (external position input)
|
|
||||||
|
|
|
||||||
v
|
|
||||||
PID Control -> Motor PWM
|
|
||||||
```
|
```
|
||||||
|
|
||||||
## ArduPilot Integration
|
## Configuration
|
||||||
|
|
||||||
ArduPilot receives external position via MAVLink:
|
### ArduPilot EKF Sources
|
||||||
|
|
||||||
1. Visual odometry → `VISION_POSITION_ESTIMATE`
|
```yaml
|
||||||
2. EKF uses external source (EK3_SRC1_POSXY=6)
|
EK3_SRC1_POSXY: 6 # External Nav
|
||||||
3. GPS disabled for navigation (GPS_TYPE=0 or EK3_SRC1_POSXY!=1)
|
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:
|
```yaml
|
||||||
|
vo_weight: 0.6 # Visual odometry
|
||||||
1. GPS position → lat/lon
|
optical_flow: 0.3 # Optical flow
|
||||||
2. Check against polygon/circle boundary
|
imu: 0.1 # IMU integration
|
||||||
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 |
|
|
||||||
|
|||||||
@@ -1,128 +1,211 @@
|
|||||||
# GPS-Denied Navigation
|
# 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? |
|
**GPS is ONLY used for geofencing (safety boundaries).**
|
||||||
|----------|-----------|
|
|
||||||
| Position estimation | No - visual odometry |
|
|
||||||
| Waypoint navigation | No - local coordinates |
|
|
||||||
| Velocity control | No - optical flow |
|
|
||||||
| Geofencing | Yes - safety only |
|
|
||||||
|
|
||||||
## Position Estimation
|
## How It Works
|
||||||
|
|
||||||
### Visual Odometry
|
### Visual Odometry
|
||||||
|
|
||||||
1. Detect features in camera image (ORB, SIFT)
|
```
|
||||||
2. Match features between consecutive frames
|
Frame N-1 Frame N
|
||||||
3. Estimate camera motion from feature displacement
|
│ │
|
||||||
4. Accumulate motion into position estimate
|
▼ ▼
|
||||||
|
┌────────┐ ┌────────┐
|
||||||
|
│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
|
### Optical Flow
|
||||||
|
|
||||||
1. Capture ground images from downward camera
|
```
|
||||||
2. Measure pixel displacement between frames
|
Downward Camera
|
||||||
3. Convert to velocity using altitude
|
│
|
||||||
4. Integrate for position
|
▼
|
||||||
|
┌───────┐
|
||||||
|
│ 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
|
### Sensor Fusion
|
||||||
|
|
||||||
Extended Kalman Filter combines:
|
```
|
||||||
- Visual odometry (position)
|
Visual Odometry ─┬─→ Weighted Average ─→ Position Estimate
|
||||||
- Optical flow (velocity)
|
Optical Flow ────┤
|
||||||
- IMU (acceleration, rotation)
|
IMU ─────────────┘
|
||||||
- Barometer (altitude)
|
```
|
||||||
|
|
||||||
Output: Full 6-DOF pose estimate
|
**Weights (configurable):**
|
||||||
|
- Visual Odometry: 60%
|
||||||
|
- Optical Flow: 30%
|
||||||
|
- IMU: 10%
|
||||||
|
|
||||||
## ArduPilot Configuration
|
## ArduPilot Configuration
|
||||||
|
|
||||||
Key parameters for GPS-denied operation:
|
### EKF3 External Navigation
|
||||||
|
|
||||||
```
|
```
|
||||||
# EKF Source Configuration
|
# GPS Type - Disabled
|
||||||
EK3_SRC1_POSXY = 6 # External Nav for position
|
GPS_TYPE 0
|
||||||
EK3_SRC1_VELXY = 6 # External Nav for velocity
|
GPS_TYPE2 0
|
||||||
EK3_SRC1_POSZ = 1 # Barometer for altitude
|
|
||||||
|
|
||||||
# Disable GPS for navigation
|
# EKF3 Source Configuration
|
||||||
GPS_TYPE = 0 # No GPS (or keep for geofence)
|
AHRS_EKF_TYPE 3 # Use EKF3
|
||||||
|
EK3_ENABLE 1
|
||||||
|
EK2_ENABLE 0
|
||||||
|
|
||||||
# Enable external navigation
|
# Position from External Nav
|
||||||
VISO_TYPE = 1 # Enable visual odometry input
|
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
|
# Vision Position Input
|
||||||
ARMING_CHECK = 0 # Disable pre-arm checks (for testing)
|
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
|
```python
|
||||||
# VISION_POSITION_ESTIMATE message
|
# Square pattern (5m sides)
|
||||||
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
|
|
||||||
waypoints = [
|
waypoints = [
|
||||||
{"x": 0, "y": 0, "z": -5}, # Takeoff to 5m
|
(5, 0, -5), # North 5m, altitude 5m
|
||||||
{"x": 10, "y": 0, "z": -5}, # 10m north
|
(5, 5, -5), # North-East corner
|
||||||
{"x": 10, "y": 10, "z": -5}, # 10m east
|
(0, 5, -5), # East
|
||||||
{"x": 0, "y": 0, "z": -5}, # Return
|
(0, 0, -5), # Back to start
|
||||||
{"x": 0, "y": 0, "z": 0}, # Land
|
|
||||||
]
|
]
|
||||||
```
|
```
|
||||||
|
|
||||||
## Limitations
|
## Limitations
|
||||||
|
|
||||||
- Drift accumulates over distance/time
|
### Visual Odometry
|
||||||
- Requires visual features (fails in featureless environments)
|
- **Scale drift**: Position error grows over time
|
||||||
- Requires sufficient lighting
|
- **Texture needed**: Poor in featureless environments
|
||||||
- Performance degrades with fast motion or blur
|
- **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`.
|
||||||
|
|||||||
@@ -1,87 +1,106 @@
|
|||||||
# Setup Guide
|
# 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
|
```bash
|
||||||
git clone https://git.sirblob.co/SirBlob/simulation.git
|
cd ~/sim/uav_ugv_simulation
|
||||||
cd simulation
|
|
||||||
bash setup.sh
|
bash setup.sh
|
||||||
```
|
```
|
||||||
|
|
||||||
The script installs:
|
### What Gets Installed
|
||||||
- ROS 2 (Humble or Jazzy)
|
|
||||||
- Gazebo Harmonic
|
|
||||||
- ArduPilot SITL
|
|
||||||
- ardupilot_gazebo plugin
|
|
||||||
- Python dependencies
|
|
||||||
|
|
||||||
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
|
## Manual Installation
|
||||||
|
|
||||||
If you prefer to install components separately:
|
### Step 1: Install ROS 2
|
||||||
|
|
||||||
### 1. ROS 2
|
|
||||||
|
|
||||||
```bash
|
```bash
|
||||||
# Add ROS 2 repository
|
# Ubuntu 22.04
|
||||||
sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key \
|
sudo apt install software-properties-common
|
||||||
-o /usr/share/keyrings/ros-archive-keyring.gpg
|
sudo add-apt-repository universe
|
||||||
|
sudo apt update && sudo apt install curl -y
|
||||||
echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] \
|
sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg
|
||||||
http://packages.ros.org/ros2/ubuntu $(lsb_release -cs) main" | \
|
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 tee /etc/apt/sources.list.d/ros2.list
|
|
||||||
|
|
||||||
sudo apt update
|
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
|
```bash
|
||||||
sudo wget https://packages.osrfoundation.org/gazebo.gpg \
|
sudo apt install -y wget
|
||||||
-O /usr/share/keyrings/pkgs-osrf-archive-keyring.gpg
|
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
|
||||||
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 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
|
```bash
|
||||||
git clone --recurse-submodules https://github.com/ArduPilot/ardupilot.git ~/ardupilot
|
cd ~
|
||||||
cd ~/ardupilot
|
git clone --recurse-submodules https://github.com/ArduPilot/ardupilot.git
|
||||||
|
cd ardupilot
|
||||||
Tools/environment_install/install-prereqs-ubuntu.sh -y
|
Tools/environment_install/install-prereqs-ubuntu.sh -y
|
||||||
. ~/.profile
|
. ~/.profile
|
||||||
./waf configure --board sitl
|
./waf configure --board sitl
|
||||||
./waf copter
|
./waf copter
|
||||||
```
|
```
|
||||||
|
|
||||||
### 4. ardupilot_gazebo Plugin
|
### Step 4: Install ardupilot_gazebo Plugin
|
||||||
|
|
||||||
```bash
|
```bash
|
||||||
git clone https://github.com/ArduPilot/ardupilot_gazebo.git ~/ardupilot_gazebo
|
cd ~
|
||||||
cd ~/ardupilot_gazebo
|
git clone https://github.com/ArduPilot/ardupilot_gazebo.git
|
||||||
|
cd ardupilot_gazebo
|
||||||
mkdir build && cd build
|
mkdir build && cd build
|
||||||
cmake .. -DCMAKE_BUILD_TYPE=Release
|
cmake .. -DCMAKE_BUILD_TYPE=RelWithDebInfo
|
||||||
make -j$(nproc)
|
make -j4
|
||||||
```
|
```
|
||||||
|
|
||||||
### 5. Python Environment
|
### Step 5: Install Python Dependencies
|
||||||
|
|
||||||
```bash
|
```bash
|
||||||
cd ~/simulation
|
cd ~/sim/uav_ugv_simulation
|
||||||
python3 -m venv venv
|
python3 -m venv venv
|
||||||
source venv/bin/activate
|
source venv/bin/activate
|
||||||
pip install -r requirements.txt
|
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
|
## Verify Installation
|
||||||
|
|
||||||
```bash
|
```bash
|
||||||
@@ -91,26 +110,23 @@ gz sim --version
|
|||||||
# Check ArduPilot
|
# Check ArduPilot
|
||||||
sim_vehicle.py --help
|
sim_vehicle.py --help
|
||||||
|
|
||||||
# Check plugin
|
# Check Python deps
|
||||||
ls ~/ardupilot_gazebo/build/libArduPilotPlugin.so
|
python3 -c "import pymavlink; print('pymavlink OK')"
|
||||||
```
|
python3 -c "import cv2; print('opencv OK')"
|
||||||
|
|
||||||
## Running the Simulation
|
|
||||||
|
|
||||||
```bash
|
|
||||||
cd ~/simulation
|
|
||||||
source activate_venv.sh
|
|
||||||
bash scripts/run_simulation.sh
|
|
||||||
```
|
```
|
||||||
|
|
||||||
## Uninstall
|
## Uninstall
|
||||||
|
|
||||||
```bash
|
```bash
|
||||||
bash scripts/uninstall.sh # ArduPilot and plugin only
|
# Remove ArduPilot and plugin only
|
||||||
bash scripts/uninstall.sh --all # Everything including project
|
bash scripts/uninstall.sh
|
||||||
|
|
||||||
|
# Remove everything including venv
|
||||||
|
bash scripts/uninstall.sh --all
|
||||||
```
|
```
|
||||||
|
|
||||||
To remove ROS 2 and Gazebo:
|
## Next Steps
|
||||||
```bash
|
|
||||||
sudo apt remove ros-humble-* gz-harmonic
|
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
|
||||||
|
|||||||
@@ -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"
|
||||||
|
|
||||||
```
|
**Solution:**
|
||||||
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:
|
|
||||||
```bash
|
```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)
|
# Then force arm:
|
||||||
|
|
||||||
```
|
|
||||||
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:
|
|
||||||
```
|
|
||||||
mode guided
|
mode guided
|
||||||
|
arm throttle force
|
||||||
|
takeoff 5
|
||||||
```
|
```
|
||||||
|
|
||||||
4. Arm the drone:
|
### Gazebo Won't Start
|
||||||
```
|
|
||||||
arm throttle
|
**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:
|
**Symptoms:**
|
||||||
```
|
- "Waiting for heartbeat"
|
||||||
# In MAVProxy console
|
- Connection timeout
|
||||||
arm check
|
- "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:
|
### MAVROS Connection Failed
|
||||||
```
|
|
||||||
# Disable GPS check for GPS-denied operation
|
**Symptoms:**
|
||||||
param set ARMING_CHECK 0
|
- "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
|
## WSL-Specific Issues
|
||||||
|
|
||||||
### DISPLAY not set
|
### Display Not Available
|
||||||
|
|
||||||
For WSLg (Windows 11):
|
**Symptoms:**
|
||||||
|
- "cannot open display"
|
||||||
|
- GUI won't show
|
||||||
|
|
||||||
|
**Solution:**
|
||||||
```bash
|
```bash
|
||||||
|
# Install VcXsrv on Windows, then:
|
||||||
export DISPLAY=:0
|
export DISPLAY=:0
|
||||||
|
|
||||||
|
# Or use WSLg (Windows 11)
|
||||||
|
# Should work automatically
|
||||||
```
|
```
|
||||||
|
|
||||||
For VcXsrv (Windows 10):
|
### Graphics Performance
|
||||||
```bash
|
|
||||||
export DISPLAY=$(cat /etc/resolv.conf | grep nameserver | awk '{print $2}'):0
|
|
||||||
```
|
|
||||||
|
|
||||||
### VcXsrv connection refused
|
**Symptoms:**
|
||||||
|
- Very slow rendering
|
||||||
1. Ensure XLaunch is running
|
- Low FPS in Gazebo
|
||||||
2. Disable access control in XLaunch
|
|
||||||
3. Check Windows Firewall allows VcXsrv
|
|
||||||
|
|
||||||
### Slow graphics performance
|
|
||||||
|
|
||||||
|
**Solution:**
|
||||||
```bash
|
```bash
|
||||||
# Use software rendering
|
# Use software rendering
|
||||||
bash scripts/run_simulation.sh --software-render
|
|
||||||
|
|
||||||
# Or set environment
|
|
||||||
export LIBGL_ALWAYS_SOFTWARE=1
|
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
|
```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
|
### View Logs
|
||||||
|
|
||||||
Logs are saved in:
|
|
||||||
```
|
|
||||||
~/ardupilot/logs/
|
|
||||||
```
|
|
||||||
|
|
||||||
### Check ROS topics
|
|
||||||
|
|
||||||
```bash
|
```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 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
|
```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
|
bash scripts/kill_simulation.sh
|
||||||
|
|
||||||
# Clean rebuild of ArduPilot
|
# Wait 5 seconds
|
||||||
cd ~/ardupilot
|
sleep 5
|
||||||
./waf clean
|
|
||||||
./waf copter
|
|
||||||
|
|
||||||
# Clean rebuild of plugin
|
# Restart
|
||||||
cd ~/ardupilot_gazebo
|
bash scripts/run_autonomous.sh --mission hover
|
||||||
rm -rf build
|
|
||||||
mkdir build && cd build
|
|
||||||
cmake ..
|
|
||||||
make -j$(nproc)
|
|
||||||
```
|
```
|
||||||
|
|
||||||
## Full Reinstall
|
### Full Reset
|
||||||
|
|
||||||
```bash
|
```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
|
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
206
docs/usage.md
@@ -1,136 +1,154 @@
|
|||||||
# Usage Guide
|
# 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
|
```bash
|
||||||
cd ~/simulation
|
|
||||||
source activate_venv.sh
|
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
|
bash scripts/run_simulation.sh
|
||||||
```
|
```
|
||||||
|
|
||||||
This launches:
|
Wait for EKF initialization messages (~15 seconds):
|
||||||
1. Gazebo with the drone model
|
```
|
||||||
2. ArduPilot SITL (flight controller)
|
EKF3 IMU0 initialised
|
||||||
3. MAVProxy console (for commands)
|
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
|
```bash
|
||||||
# Default (iris_runway world)
|
source /opt/ros/humble/setup.bash
|
||||||
bash scripts/run_simulation.sh
|
source activate_venv.sh
|
||||||
|
ros2 launch uav_ugv_simulation full_simulation.launch.py
|
||||||
# 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
|
|
||||||
```
|
```
|
||||||
|
|
||||||
## 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
|
||||||
|
|
||||||
```
|
### UAV Topics
|
||||||
mode guided # Switch to GUIDED mode (required for commands)
|
|
||||||
arm throttle # Arm motors
|
|
||||||
takeoff 5 # Takeoff to 5 meters altitude
|
|
||||||
|
|
||||||
# Fly to position (North, East, Down in meters)
|
| Topic | Type | Description |
|
||||||
guided 10 0 -5 # 10m north, 0m east, 5m altitude
|
|-------|------|-------------|
|
||||||
guided 10 10 -5 # 10m north, 10m east, 5m altitude
|
| `/uav/mavros/state` | `mavros_msgs/State` | Armed/mode status |
|
||||||
guided 0 0 -5 # Return to origin at 5m altitude
|
| `/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
|
### UGV Topics
|
||||||
land # Land at current position
|
|
||||||
disarm # Disarm motors (after landing)
|
|
||||||
```
|
|
||||||
|
|
||||||
### 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
|
```bash
|
||||||
# Arm
|
# Send command to UAV
|
||||||
ros2 service call /mavros/cmd/arming mavros_msgs/srv/CommandBool "{value: true}"
|
ros2 topic pub /uav/controller/command std_msgs/String "data: 'takeoff'"
|
||||||
|
|
||||||
# Set GUIDED mode
|
# Send waypoint
|
||||||
ros2 service call /mavros/set_mode mavros_msgs/srv/SetMode "{custom_mode: 'GUIDED'}"
|
ros2 topic pub /uav/setpoint_position geometry_msgs/PoseStamped \
|
||||||
|
"{header: {frame_id: 'odom'}, pose: {position: {x: 10, y: 5, z: 5}}}"
|
||||||
|
|
||||||
# Takeoff
|
# Send UGV goal
|
||||||
ros2 service call /mavros/cmd/takeoff mavros_msgs/srv/CommandTOL "{altitude: 5}"
|
ros2 topic pub /ugv/goal_pose geometry_msgs/PoseStamped \
|
||||||
|
"{header: {frame_id: 'odom'}, pose: {position: {x: 5, y: 5, z: 0}}}"
|
||||||
# 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 "{}"
|
|
||||||
```
|
```
|
||||||
|
|
||||||
### Monitoring
|
## Mission Planner
|
||||||
|
|
||||||
|
Run coordinated multi-vehicle missions:
|
||||||
|
|
||||||
```bash
|
```bash
|
||||||
# List topics
|
ros2 run uav_ugv_simulation mission_planner
|
||||||
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
|
|
||||||
```
|
```
|
||||||
|
|
||||||
## Flight Modes
|
Send commands:
|
||||||
|
```bash
|
||||||
|
# Load demo mission
|
||||||
|
ros2 topic pub /mission/command std_msgs/String "data: 'load'"
|
||||||
|
|
||||||
| Mode | Description |
|
# Start mission
|
||||||
|------|-------------|
|
ros2 topic pub /mission/command std_msgs/String "data: 'start'"
|
||||||
| STABILIZE | Manual control with attitude stabilization |
|
|
||||||
| ALT_HOLD | Maintain altitude, manual position |
|
# Pause/Resume
|
||||||
| LOITER | Hold position and altitude |
|
ros2 topic pub /mission/command std_msgs/String "data: 'pause'"
|
||||||
| GUIDED | Accept position commands |
|
ros2 topic pub /mission/command std_msgs/String "data: 'resume'"
|
||||||
| AUTO | Follow pre-planned mission |
|
|
||||||
| RTL | Return to launch point |
|
# Abort
|
||||||
| LAND | Controlled descent and landing |
|
ros2 topic pub /mission/command std_msgs/String "data: 'abort'"
|
||||||
|
```
|
||||||
|
|
||||||
## Stopping the Simulation
|
## Stopping the Simulation
|
||||||
|
|
||||||
Press `Ctrl+C` in the terminal running the simulation.
|
|
||||||
|
|
||||||
Or run:
|
|
||||||
```bash
|
```bash
|
||||||
|
# Kill all processes
|
||||||
bash scripts/kill_simulation.sh
|
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
|
## Next Steps
|
||||||
# Forward camera (visual odometry)
|
|
||||||
ros2 topic echo /uav/camera/forward/image_raw
|
|
||||||
|
|
||||||
# Downward camera (optical flow)
|
- [Architecture Overview](architecture.md)
|
||||||
ros2 topic echo /uav/camera/downward/image_raw
|
- [GPS-Denied Navigation](gps_denied_navigation.md)
|
||||||
```
|
- [Troubleshooting](troubleshooting.md)
|
||||||
|
|
||||||
## 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.
|
|
||||||
|
|||||||
@@ -1,129 +1,187 @@
|
|||||||
# WSL Setup Guide
|
# 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
|
- 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:
|
Open PowerShell as Administrator:
|
||||||
|
|
||||||
|
```powershell
|
||||||
|
wsl --install
|
||||||
|
```
|
||||||
|
|
||||||
|
### 2. Install Ubuntu
|
||||||
|
|
||||||
```powershell
|
```powershell
|
||||||
wsl --install -d Ubuntu-22.04
|
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/)
|
Verify:
|
||||||
2. Run XLaunch with these settings:
|
|
||||||
- Multiple windows
|
|
||||||
- Start no client
|
|
||||||
- Disable access control (checked)
|
|
||||||
3. In WSL, set DISPLAY:
|
|
||||||
```bash
|
```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
|
## Installation
|
||||||
|
|
||||||
|
Same as native Ubuntu:
|
||||||
|
|
||||||
```bash
|
```bash
|
||||||
git clone https://git.sirblob.co/SirBlob/simulation.git
|
cd ~/sim/uav_ugv_simulation
|
||||||
cd simulation
|
|
||||||
bash setup.sh
|
bash setup.sh
|
||||||
```
|
```
|
||||||
|
|
||||||
The setup script automatically:
|
## Running Simulation
|
||||||
- Detects WSL environment
|
|
||||||
- Installs GUI support packages
|
|
||||||
- Creates WSL environment file
|
|
||||||
- Configures DISPLAY variable
|
|
||||||
|
|
||||||
## Running the Simulation
|
**Always use software rendering on WSL:**
|
||||||
|
|
||||||
```bash
|
```bash
|
||||||
cd ~/simulation
|
|
||||||
source activate_venv.sh
|
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:
|
## Performance Tips
|
||||||
```bash
|
|
||||||
bash scripts/run_simulation.sh --software-render
|
### 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
|
## Troubleshooting
|
||||||
|
|
||||||
### Black screen or no display
|
### "cannot open display"
|
||||||
|
|
||||||
Check DISPLAY variable:
|
|
||||||
```bash
|
```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`
|
### Graphics freeze/crash
|
||||||
For VcXsrv (Windows 10), should be `<IP>:0`
|
|
||||||
|
|
||||||
Test with:
|
|
||||||
```bash
|
|
||||||
xcalc
|
|
||||||
```
|
|
||||||
|
|
||||||
### Gazebo crashes immediately
|
|
||||||
|
|
||||||
Use software rendering:
|
|
||||||
```bash
|
```bash
|
||||||
|
# Always use software rendering
|
||||||
export LIBGL_ALWAYS_SOFTWARE=1
|
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
|
### Slow performance
|
||||||
|
|
||||||
- Close unnecessary Windows applications
|
- Increase WSL memory (see Performance Tips)
|
||||||
- Allocate more RAM to WSL in `.wslconfig`:
|
- Use software rendering
|
||||||
```ini
|
- Close other applications
|
||||||
[wsl2]
|
|
||||||
memory=8GB
|
|
||||||
processors=4
|
|
||||||
```
|
|
||||||
|
|
||||||
- Use software rendering flag
|
### Network issues
|
||||||
|
|
||||||
## Environment Variables
|
|
||||||
|
|
||||||
The `activate_venv.sh` script sets these automatically:
|
|
||||||
|
|
||||||
```bash
|
```bash
|
||||||
export DISPLAY=:0 # or IP:0 for VcXsrv
|
# If MAVLink connection fails
|
||||||
export LIBGL_ALWAYS_INDIRECT=0
|
# Check Windows firewall allows WSL traffic
|
||||||
export MESA_GL_VERSION_OVERRIDE=3.3
|
|
||||||
```
|
```
|
||||||
|
|
||||||
## Uninstall
|
## Quick Start Commands
|
||||||
|
|
||||||
```bash
|
```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)
|
||||||
|
|||||||
@@ -31,17 +31,21 @@ def generate_launch_description():
|
|||||||
'use_ground_truth', default_value='true', description='Use Gazebo ground truth'
|
'use_ground_truth', default_value='true', description='Use Gazebo ground truth'
|
||||||
)
|
)
|
||||||
|
|
||||||
|
# Gazebo Harmonic (gz sim) instead of Gazebo Classic
|
||||||
gazebo = ExecuteProcess(
|
gazebo = ExecuteProcess(
|
||||||
cmd=['gazebo', '--verbose', LaunchConfiguration('world')],
|
cmd=[
|
||||||
|
'gz', 'sim', '-v4', '-r',
|
||||||
|
os.path.expanduser('~/ardupilot_gazebo/worlds/iris_runway.sdf')
|
||||||
|
],
|
||||||
output='screen',
|
output='screen',
|
||||||
additional_env={
|
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')}",
|
||||||
'GAZEBO_RESOURCE_PATH': f"{pkg_share}/worlds:" + os.path.expanduser('~/ardupilot_gazebo/worlds')
|
'GZ_SIM_SYSTEM_PLUGIN_PATH': os.path.expanduser('~/ardupilot_gazebo/build')
|
||||||
}
|
}
|
||||||
)
|
)
|
||||||
|
|
||||||
ardupilot_uav = TimerAction(
|
ardupilot_uav = TimerAction(
|
||||||
period=3.0,
|
period=5.0, # Wait for Gazebo to initialize
|
||||||
actions=[
|
actions=[
|
||||||
ExecuteProcess(
|
ExecuteProcess(
|
||||||
cmd=[
|
cmd=[
|
||||||
@@ -49,20 +53,20 @@ def generate_launch_description():
|
|||||||
'-v', 'ArduCopter',
|
'-v', 'ArduCopter',
|
||||||
'-f', 'gazebo-iris',
|
'-f', 'gazebo-iris',
|
||||||
'--model', 'JSON',
|
'--model', 'JSON',
|
||||||
'--map', '--console',
|
'--no-mavproxy', # Don't start MAVProxy, MAVROS will connect
|
||||||
'-I0',
|
'-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'),
|
cwd=os.path.expanduser('~/ardupilot'),
|
||||||
output='screen'
|
output='screen',
|
||||||
|
additional_env={
|
||||||
|
'PATH': os.environ.get('PATH', '') + ':' + os.path.expanduser('~/ardupilot/Tools/autotest')
|
||||||
|
}
|
||||||
)
|
)
|
||||||
]
|
]
|
||||||
)
|
)
|
||||||
|
|
||||||
mavros_uav = TimerAction(
|
mavros_uav = TimerAction(
|
||||||
period=8.0,
|
period=15.0, # Wait for ArduPilot SITL to start
|
||||||
actions=[
|
actions=[
|
||||||
Node(
|
Node(
|
||||||
package='mavros',
|
package='mavros',
|
||||||
@@ -73,7 +77,7 @@ def generate_launch_description():
|
|||||||
parameters=[
|
parameters=[
|
||||||
os.path.join(pkg_share, 'config', 'mavros_params.yaml'),
|
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': '',
|
'gcs_url': '',
|
||||||
'target_system_id': 1,
|
'target_system_id': 1,
|
||||||
'target_component_id': 1,
|
'target_component_id': 1,
|
||||||
|
|||||||
@@ -14,36 +14,42 @@ def generate_launch_description():
|
|||||||
|
|
||||||
world_arg = DeclareLaunchArgument(
|
world_arg = DeclareLaunchArgument(
|
||||||
'world',
|
'world',
|
||||||
default_value=os.path.join(pkg_share, 'worlds', 'empty_custom.world'),
|
default_value='iris_runway.sdf',
|
||||||
description='Path to world file'
|
description='World file name'
|
||||||
)
|
)
|
||||||
|
|
||||||
|
# Gazebo Harmonic
|
||||||
gazebo = ExecuteProcess(
|
gazebo = ExecuteProcess(
|
||||||
cmd=['gazebo', '--verbose', LaunchConfiguration('world')],
|
cmd=[
|
||||||
|
'gz', 'sim', '-v4', '-r',
|
||||||
|
os.path.expanduser('~/ardupilot_gazebo/worlds/iris_runway.sdf')
|
||||||
|
],
|
||||||
output='screen',
|
output='screen',
|
||||||
additional_env={
|
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(
|
ardupilot = TimerAction(
|
||||||
period=3.0,
|
period=5.0,
|
||||||
actions=[
|
actions=[
|
||||||
ExecuteProcess(
|
ExecuteProcess(
|
||||||
cmd=[
|
cmd=[
|
||||||
'sim_vehicle.py', '-v', 'ArduCopter', '-f', 'gazebo-iris',
|
'sim_vehicle.py', '-v', 'ArduCopter', '-f', 'gazebo-iris',
|
||||||
'--model', 'JSON', '--map', '--console', '-I0',
|
'--model', 'JSON', '--no-mavproxy', '-I0',
|
||||||
'--out', '127.0.0.1:14550',
|
|
||||||
'--add-param-file', os.path.join(pkg_share, 'config', 'ardupilot_gps_denied.parm')
|
|
||||||
],
|
],
|
||||||
cwd=os.path.expanduser('~/ardupilot/ArduCopter'),
|
cwd=os.path.expanduser('~/ardupilot'),
|
||||||
output='screen'
|
output='screen',
|
||||||
|
additional_env={
|
||||||
|
'PATH': os.environ.get('PATH', '') + ':' + os.path.expanduser('~/ardupilot/Tools/autotest')
|
||||||
|
}
|
||||||
)
|
)
|
||||||
]
|
]
|
||||||
)
|
)
|
||||||
|
|
||||||
mavros = TimerAction(
|
mavros = TimerAction(
|
||||||
period=8.0,
|
period=15.0,
|
||||||
actions=[
|
actions=[
|
||||||
Node(
|
Node(
|
||||||
package='mavros',
|
package='mavros',
|
||||||
@@ -53,7 +59,7 @@ def generate_launch_description():
|
|||||||
output='screen',
|
output='screen',
|
||||||
parameters=[
|
parameters=[
|
||||||
os.path.join(pkg_share, 'config', 'mavros_params.yaml'),
|
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'}
|
||||||
]
|
]
|
||||||
)
|
)
|
||||||
]
|
]
|
||||||
|
|||||||
@@ -2,6 +2,8 @@
|
|||||||
|
|
||||||
echo "Killing all simulation processes..."
|
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 "gazebo" 2>/dev/null || true
|
||||||
pkill -9 -f "gzserver" 2>/dev/null || true
|
pkill -9 -f "gzserver" 2>/dev/null || true
|
||||||
pkill -9 -f "gzclient" 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 "mavros" 2>/dev/null || true
|
||||||
pkill -9 -f "ArduCopter" 2>/dev/null || true
|
pkill -9 -f "ArduCopter" 2>/dev/null || true
|
||||||
pkill -9 -f "ArduRover" 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
|
pkill -9 -f "ros2" 2>/dev/null || true
|
||||||
|
|
||||||
sleep 1
|
sleep 1
|
||||||
|
|||||||
122
scripts/run_autonomous.sh
Executable file
122
scripts/run_autonomous.sh
Executable 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
36
scripts/setup_gazebo_nvidia.sh
Executable 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
368
src/autonomous_controller.py
Executable 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()
|
||||||
@@ -29,16 +29,24 @@ class UAVController(Node):
|
|||||||
self.declare_parameter('takeoff_altitude', 5.0)
|
self.declare_parameter('takeoff_altitude', 5.0)
|
||||||
self.declare_parameter('position_tolerance', 0.3)
|
self.declare_parameter('position_tolerance', 0.3)
|
||||||
self.declare_parameter('namespace', '/uav')
|
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.takeoff_altitude = self.get_parameter('takeoff_altitude').value
|
||||||
self.position_tolerance = self.get_parameter('position_tolerance').value
|
self.position_tolerance = self.get_parameter('position_tolerance').value
|
||||||
ns = self.get_parameter('namespace').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.state = FlightState.DISARMED
|
||||||
self.mavros_state = None
|
self.mavros_state = None
|
||||||
self.current_pose = None
|
self.current_pose = None
|
||||||
self.target_pose = None
|
self.target_pose = None
|
||||||
self.home_position = None
|
self.home_position = None
|
||||||
|
self.startup_complete = False
|
||||||
|
self.ekf_ready = False
|
||||||
|
|
||||||
self.state_sub = self.create_subscription(
|
self.state_sub = self.create_subscription(
|
||||||
State, f'{ns}/mavros/state', self.state_callback, 10)
|
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.setpoint_timer = self.create_timer(0.05, self.publish_setpoint)
|
||||||
self.status_timer = self.create_timer(0.5, self.publish_status)
|
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')
|
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):
|
def state_callback(self, msg):
|
||||||
self.mavros_state = msg
|
self.mavros_state = msg
|
||||||
if msg.armed and self.state == FlightState.DISARMED:
|
if msg.armed and self.state == FlightState.DISARMED:
|
||||||
|
|||||||
Reference in New Issue
Block a user