This project implements an autonomous quadcopter system for terrain surveying and path planning. The system utilises a quadcopter equipped with sonar and laser sensors to map rough terrain, determine traversable paths, and validate goal reachability for ground vehicle operations.
The quadcopter autonomously flies through provided goals, builds an elevation map using sonar data, detects obstacles with laser scanning, and determines if paths between goals are traversable based on terrain gradients.
- Autonomous Flight Control: Complete takeoff, flight, and landing control
- Terrain Mapping: Real-time elevation mapping using sonar sensor data
- Obstacle Detection: 360-degree laser scanning for obstacle avoidance
- Path Planning: Traversability analysis based on terrain gradients
- TSP Optimisation: Advanced mode with Travelling Salesman Problem solving
- Multi-threaded Architecture: Separate threads for control and sensor processing
- ROS2 Integration: Full ROS2 component-based software engineering framework
The system consists of two main ROS2 nodes:
- Main control node managing ROS related drone operations
- Handles mission planning and execution
- Processes goals and generates flight paths
- Implements obstacle avoidance algorithms
- Dedicated sensor data processing for ROS end
- Real-time terrain mapping using TerrainMap class
- Laser scan processing for obstacle detection
- Grid map generation and publishing
- Handles all drone movement commands (takeoff, landing, flight)
- Implements PID-style control for goal reaching
- Emergency obstacle avoidance with altitude adjustment
- Mission state management and progress tracking
- Incremental 1×1m cell-by-cell terrain mapping
- Drone-centric grid expansion during flight
- Gradient calculation for traversability analysis
- High-resolution elevation data storage
- Traversability analysis between goals
- Waypoint generation with configurable spacing
- Gradient-based path validation
- Visualisation marker management
- Constructs traversability graphs between waypoints
- Implements Travelling Salesman Problem solving
- Optimises mission paths for efficiency
- Supports complex multi-goal scenarios
- Sequential goal processing
- Direct path traversability checking
- Basic terrain mapping and analysis
- Gradient-based goal validation
- TSP-optimised path planning
- Complex traversability graph construction
- Multiple valid path analysis
- Advanced visualisation of all possible routes
/drone/gt_odom- Robot odometry data/drone/laserscan- 360-degree laser scan data/drone/sonar- Ground distance measurements/mission/goals- Goal positions for surveying
/drone/cmd_vel- Velocity commands for drone control/drone/takeoff- Takeoff command/drone/landing- Landing command/grid_map- Real-time elevation map/mission/path- Traversable waypoints between goals/visualisation_marker- RViz visualisation markers
/mission/control- Start/stop mission control service
- Process sonar readings at current drone position
- Update elevation map with new height data
- Calculate gradients around updated regions
- Expand map boundaries as drone moves
- Maintain high-resolution terrain representation
- Continuously monitor 360-degree laser scans
- Detect obstacles within minimum safe distance (3m)
- Execute emergency altitude increase when needed
- Resume normal flight when obstacles cleared
- Maintain altitude control using sonar feedback
- Calculate terrain gradients between waypoints
- Compare gradients against maximum threshold (3% default)
- Generate intermediate waypoints at 1m intervals
- Validate entire path for ground vehicle access
- Abort mission if any segment non-traversable
- Build complete traversability graph between all goals
- Calculate costs for all valid path segments
- Apply TSP solver to find optimal visiting order
- Generate optimised mission sequence
- Visualise all valid paths and highlight optimal route
road_gradient- Maximum traversable gradient (default: 3%)advanced- Enable advanced TSP mode (default: false)- Various control parameters for PID tuning and safety margins
- ROS2 (tested with appropriate distribution)
- Required dependencies for RViz and navigation
-
Launch the RViz GUI:
ros2 launch pfms a3_terrain.launch.py
-
Launch drone node:
cd ~/ros2_ws/ ros2 run a3_skeleton drone_node
This will start both the controller and drone node
-
For advanced mode:
ros2 run a3_skeleton drone_node advanced:=true
-
Send goals in a separate terminal:
ros2 topic pub /mission/goals geometry_msgs/msg/PoseArray '{poses: [...]}'Goals can be in JSON or YAML format. Goals do not need a Z-coordinate because of altitude control. If you publish a goal with Z > 2, altitude control will be overridden.
-
Start mission in separate terminal:
ros2 service call /mission/control std_srvs/srv/SetBool '{data: true}' -
To pause/stop mission:
ros2 service call /mission/control std_srvs/srv/SetBool '{data: false}'For stop mission, the drone will traverse to its current goal then stop. It will not stop immediately.
Various scripts have been developed to assist in testing drone code and functionality. These scripts can be found in the scripts directory of the package.
-
Launch drone terrain and start drone node:
ros2 launch pfms a3_terrain.launch.py
In another terminal:
cd ~/ros2_ws/ ros2 run a3_skeleton drone_node
-
Run Data collector script (from ros2_ws):
./src/a3_skeleton/scripts/collect_test_data.sh collect
This script will publish goals and start the service for drone navigation. It's a 3-part process that records three different bags for gridmap test, traversability, and TSP testing.
-
Run test script:
cd ~/ros2_ws/build/a3_skeleton ./drone_survey_tests
Note: The test script has hard-coded paths. Make sure to adjust the paths on lines 124, 214, and 308 of drone_survey_tests.cpp for your environment.
The system implements comprehensive thread safety using:
- Mutex protection for shared sensor data
- Atomic variables for mission state tracking
- Condition variables for thread coordination
- Lock-free communication where possible
The system provides rich visualisation through RViz:
- Real-time elevation map display
- Traversable waypoint markers (green cylinders)
- Non-traversable path indicators (red markers)
- Drone position and orientation tracking
- Emergency obstacle avoidance with immediate altitude increase (not functional)
- Mission abort capability for non-traversable terrain
- Safe return-to-origin functionality
- Altitude control with sonar-based ground clearance
- Comprehensive error handling and recovery
The system employs a threaded architecture for the controller with a main controller thread that handles drone control functionality. In theory, multiple drones can be controlled and have independent missions.
- STL containers for efficient waypoint storage and manipulation
- Thread-safe queues for inter-thread communication
- Grid-based elevation maps with dynamic expansion
- Graph structures for TSP path optimisation
- Graceful degradation when goals are unreachable
- Automatic return-to-home on mission failure
- Comprehensive logging for debugging and analysis
- Real-time constraint compliance for safety-critical operations
- Efficient memory management for large terrain maps
- Optimised algorithms for rapid obstacle detection
- Scalable TSP solving for varying numbers of goals
Jackson Russell
Version 1.0 - June 2025