Implementing Dijkstra Algorithm as an Exercise in Path Planning
07 May 2024
Mobile robot path planning is a foundational skill in robotics, enabling robots to navigate environments autonomously and efficiently. This project served as an exercise in implementing Dijkstra’s pathfinding algorithm from scratch using ROS2 and Nav2. By swapping out Nav2’s default planner for a custom Dijkstra implementation, this project demonstrates how a widely recognized pathfinding algorithm can be used in a factory floor simulation to facilitate autonomous navigation.
The development process involved setting up the ROS2 workspace, configuring the Nav2 stack, implementing Dijkstra’s pathfinding algorithm, and testing the solution within a simulated environment.
Environment Configuration: The ROS2 workspace was prepared, and the starter code was cloned to initialize the Gazebo simulation and the Nav2 navigation stack.
Planner Configuration: In the Nav2 configuration file planner_server.yaml
, the plugin for the GridBased
planner was swapped to nav2_dijkstra_planner/DijkstraGlobalPlanner
, pointing to the new planner’s implementation.
Simulation Launch: Gazebo and Nav2 were launched to verify that the new planner loaded successfully and that the robot could be controlled via Rviz2.
Initial Setup: The initial setup required writing Dijkstra’s pathfinding algorithm in C++ within the provided nav2_dijkstra_planner.cpp
file.
Algorithm Steps: Phase I: Extract nodes with the lowest cost from the open list, marking them as visited while iterating toward the goal.
Case I and II: Evaluate neighboring nodes based on whether they are already in the open list or closed list, updating their cost and parent nodes where necessary.
Phase II: Once the goal is reached, the path is traced backward from the goal to the starting node using each node’s parent node.
Testing and Verification: The implementation was verified by launching Gazebo and Nav2 to ensure the new planner initialized correctly. Using Rviz2, goals were set at different locations on the map, and the robot navigated to them autonomously.
The project successfully demonstrated how a well-known pathfinding algorithm like Dijkstra’s could be implemented from scratch within the Nav2 framework. Although the planning process was slower than the default planner, the algorithm reliably computed paths that avoided obstacles and led the robot to the goal location.