Project
Introduction
This project implements a comprehensive ROS2 navigation stack featuring a custom Dijkstra path planning algorithm written from scratch as a Nav2 plugin, demonstrating deep understanding of both graph search algorithms and ROS2 plugin architecture. The system provides full autonomous navigation capabilities through the integration of Cartographer SLAM for real-time 2D mapping, AMCL for adaptive Monte Carlo localization, and the custom Dijkstra global planner supporting 8-directional movement for optimal path generation. Unlike standard grid-based planners, this implementation incorporates dynamic cost integration from the costmap, enabling obstacle proximity penalties that create safer, more natural paths. The complete navigation pipeline includes DWB local planning for trajectory execution, behavior trees for complex navigation behaviors with recovery actions, and multi-layered costmaps combining static, obstacle, and inflation layers. Through careful implementation of the Nav2 plugin interface, the custom planner seamlessly integrates with the existing navigation infrastructure, achieving sub-100ms planning times on 50x50m maps with 95% navigation
Objectives
-
To develop a custom Dijkstra global path planner from scratch as a fully integrated Nav2 plugin
-
To implement 8-directional graph search with diagonal movement support for smoother path generation
-
To integrate dynamic costmap values for obstacle proximity penalties in path planning
-
To create complete autonomous navigation stack with SLAM, localization, and path execution
-
To demonstrate plugin architecture mastery through Nav2 lifecycle management integration
-
To achieve real-time performance with <100ms planning on large-scale environments
Tools and Technologies
-
Framework: ROS2 Humble with Nav2 navigation stack
-
Custom Planner: C++ Dijkstra implementation with pluginlib integration
-
SLAM: Cartographer for 2D LiDAR-based mapping
-
Localization: AMCL with 8000 particles for pose estimation
-
Local Planner: DWB (Dynamic Window Approach)
-
Graph Search: 8-connected grid with priority queue
-
Costmap: Multi-layered 2D occupancy grid (5cm resolution)
-
Behavior Control: Nav2 BT Navigator with recovery behaviors
-
Plugin System: Pluginlib for dynamic loading
-
Visualization: RViz2 for path and costmap display
-
Build System: CMake with ament_cmake
-
Version Control: Git
Source Code
-
GitHub Repository: ROS2 Navigation Dijkstra
-
Documentation: README with algorithm details
Video Result
-
Navigation Demo: Custom Dijkstra planner generating optimal paths with Nav2 integration​
-
System Architecture: Complete navigation stack with SLAM, localization, and custom global planner
-
Performance Metrics: <100ms path planning, 95% success rate, ±10cm localization accuracy
Process and Development
The project is structured into five critical components: Dijkstra algorithm implementation, Nav2 plugin architecture integration, 8-directional neighbor detection system, costmap integration for dynamic costs, and complete navigation stack configuration.
Task 1: Core Dijkstra Algorithm Implementation
Priority Queue Structure: Implemented sorted open list using std::vector<pair<int, double>> storing node indices with g-costs, maintaining ascending order for optimal node expansion.
Graph Traversal Logic: Developed main search loop extracting minimum g-cost nodes, updating neighbor costs through relaxation, maintaining closed set to prevent revisiting explored nodes.
Path Reconstruction: Created backtracking system using parent hash map, building shortest path from goal to start by following parent pointers in reverse order
Task 2: Nav2 Plugin Architecture Integration
Plugin Interface Implementation: Extended nav2_core::GlobalPlanner base class, implementing configure(), activate(), deactivate(), and createPlan() virtual methods for lifecycle management.
Costmap Integration: Accessed nav2_costmap_2d::Costmap2D through ROS interface, extracting resolution, origin, and dimensions for coordinate transformations.
Plugin Registration: Created nav2_dijkstra_planner_plugin.xml for pluginlib discovery, registering DijkstraGlobalPlanner class with PLUGINLIB_EXPORT_CLASS macro.
Task 3: 8-Directional Neighbor Detection
Cardinal Direction Handling: Implemented 4-connected neighbors (up, down, left, right) with step cost equal to grid resolution, checking array bounds and map edges.
Diagonal Movement Support: Added 4 diagonal neighbors with √2 cost scaling (1.41421 × resolution), validating corner cutting prevention and boundary conditions.
Lethal Obstacle Checking: Integrated costmap value checking with lethal_cost threshold=1, excluding occupied cells from valid neighbor set.
Task 4: Dynamic Cost Integration
Costmap Penalty Calculation: Modified step costs as resolution + (costmap_value/255.0), creating gradient field around obstacles for safer path generation.
Cost Propagation: Updated g-cost calculation incorporating accumulated path cost plus dynamic step cost, enabling paths that naturally avoid obstacle proximity.
Flat Array Conversion: Transformed 2D costmap to 1D vector for efficient indexing, mapping (x,y) coordinates to linear indices using y*width + x formula.
Task 5: Complete Navigation Stack Configuration
Cartographer SLAM Setup: Configured 2D laser-based mapping with 5cm resolution, online scan matching, and loop closure detection for consistent map generation.
AMCL Localization: Tuned adaptive particle filter with 200-8000 particles, differential drive model, and likelihood field sensor model for ±10cm accuracy.
DWB Controller Integration: Set local planner parameters with 0.26 m/s max velocity, 2Hz control frequency, and critics for path following and obstacle avoidance.
Results
The custom Dijkstra planner successfully integrates with Nav2, generating optimal paths in under 100ms for typical indoor environments. The 8-directional search produces smoother trajectories compared to 4-connected planners, reducing total path length by approximately 15%. Dynamic cost integration creates natural obstacle clearance of 0.55m through inflation radius configuration. The complete navigation stack achieves 95% success rate in reaching goals within ±25cm tolerance. Cartographer SLAM builds accurate 2D maps with 5cm resolution suitable for navigation planning. AMCL localization maintains ±10cm position accuracy with adaptive particle resampling. The system handles dynamic obstacles through 1Hz global replanning and 5Hz local costmap updates.
Key Insights
-
Plugin Architecture Power: Nav2 plugin system enables seamless integration of custom algorithms while leveraging existing infrastructure for costmaps and transforms.
-
8-Direction Advantage: Diagonal movement support reduces path length and creates more natural trajectories but requires careful boundary checking to prevent array access violations.
-
Cost Integration Importance: Incorporating costmap values beyond binary occupied/free creates safer paths with natural obstacle clearance without explicit inflation.
-
Open List Efficiency: Simple sorted vector outperforms priority_queue for small-medium graphs due to cache locality despite O(n) insertion complexity.
-
Lifecycle Management: Proper implementation of Nav2 lifecycle callbacks ensures clean initialization and shutdown preventing resource leaks.
Future Work
-
A Enhancement:* Extend to A* algorithm adding heuristic function for faster convergence while maintaining optimality
-
Anytime Planning: Implement anytime variant returning suboptimal paths quickly then improving solution quality with available time
-
Multi-Resolution Search: Add hierarchical planning using quadtree decomposition for faster long-distance path planning
-
Dynamic Replanning: Optimize incremental search using D* Lite for efficient replanning when obstacles appear
-
GPU Acceleration: Parallelize neighbor exploration using CUDA for massive speedup on large maps
-
Machine Learning Integration: Train cost prediction network learning traversability from sensor data beyond geometric obstacles