Global planner (A*, Dijkstra) in ROS - Mini Project: Build & Apply

Global Planner with A* and Dijkstra in ROS

📖 Scenario: You are building a robot navigation system using ROS. The robot needs to find the shortest path from its current position to a goal position on a grid map. You will implement two popular global path planning algorithms: A* and Dijkstra.This project will guide you through setting up the map data, configuring the planner type, implementing the core pathfinding logic, and completing the ROS node to publish the planned path.

🎯 Goal: Build a ROS global planner node that can compute paths using A* or Dijkstra algorithms on a grid map. The node will publish the path as a sequence of grid coordinates for the robot to follow.

📋 What You'll Learn

Create a 2D grid map as a list of lists with obstacles and free spaces

Add a configuration variable to select the planner type ('astar' or 'dijkstra')

Implement the core pathfinding logic for A* and Dijkstra algorithms

Complete the ROS node to publish the computed path as a ROS message

💡 Why This Matters

🌍 Real World

Robots use global planners like A* and Dijkstra to find safe and efficient routes in environments with obstacles.

💼 Career

Understanding and implementing path planning algorithms is essential for robotics engineers working on autonomous navigation systems.

1

Setup the 2D grid map data

Create a variable called grid_map as a 5x5 list of lists representing the map. Use 0 for free space and 1 for obstacles. The map must be exactly:
[[0, 0, 0, 0, 0],
[0, 1, 1, 1, 0],
[0, 0, 0, 1, 0],
[0, 1, 0, 0, 0],
[0, 0, 0, 0, 0]]

ROS

# Create the grid_map variable with the exact 5x5 map
# Your code here

Hint

Use a list of lists with 0 and 1 values exactly as shown.

2

Add planner type configuration

Create a variable called planner_type and set it to the string 'astar' to select the A* algorithm.

ROS

grid_map = [
    [0, 0, 0, 0, 0],
    [0, 1, 1, 1, 0],
    [0, 0, 0, 1, 0],
    [0, 1, 0, 0, 0],
    [0, 0, 0, 0, 0]
]
# Set planner_type to 'astar'
# Your code here

Hint

Assign the string 'astar' exactly to planner_type.

3

Implement core pathfinding logic

Define a function called find_path that takes start and goal tuples and returns a list of tuples representing the path. Inside, use an if statement to check if planner_type is 'astar' or 'dijkstra'. For this step, return a hardcoded path [(0,0), (0,1), (0,2), (0,3), (0,4)] for both cases.

ROS

grid_map = [
    [0, 0, 0, 0, 0],
    [0, 1, 1, 1, 0],
    [0, 0, 0, 1, 0],
    [0, 1, 0, 0, 0],
    [0, 0, 0, 0, 0]
]
planner_type = 'astar'

# Define find_path function with start and goal parameters
# Use if planner_type == 'astar' or 'dijkstra' to return the hardcoded path
# Your code here

Hint

Define the function and use an if-elif to check planner_type and return the fixed path.

4

Complete ROS node to publish the path

Import rospy and Path from nav_msgs.msg. Initialize a ROS node called 'global_planner'. Create a publisher path_pub publishing Path messages on topic '/planned_path'. Call find_path with start=(0,0) and goal=(0,4). Publish the path as a Path message with poses converted from the list of tuples. Add rospy.spin() at the end.

ROS

grid_map = [
    [0, 0, 0, 0, 0],
    [0, 1, 1, 1, 0],
    [0, 0, 0, 1, 0],
    [0, 1, 0, 0, 0],
    [0, 0, 0, 0, 0]
]
planner_type = 'astar'

def find_path(start, goal):
    if planner_type == 'astar':
        return [(0,0), (0,1), (0,2), (0,3), (0,4)]
    elif planner_type == 'dijkstra':
        return [(0,0), (0,1), (0,2), (0,3), (0,4)]

# Import rospy and Path
# Initialize node 'global_planner'
# Create publisher path_pub on '/planned_path' with Path message
# Call find_path with start=(0,0) and goal=(0,4)
# Convert path to Path message and publish
# Add rospy.spin()
# Your code here

Hint

Use rospy to create the node and publisher. Convert each path point to a PoseStamped and add to Path message.

Practice

1. What is the main difference between the A* algorithm and Dijkstra's algorithm in ROS global planners?

easy

A. Dijkstra uses a heuristic, while A* explores all paths evenly.

B. A* uses a heuristic to guide the search, while Dijkstra explores all paths evenly.

C. Both algorithms use heuristics but in different ways.

D. Neither algorithm considers obstacles in the map.

Solution

1. Step 1: Understand Dijkstra's algorithm behavior

   Dijkstra explores all possible paths evenly without using heuristics, ensuring the shortest path but potentially slower.

2. Step 2: Understand A* algorithm behavior

   A* uses a heuristic function to estimate the cost to the goal, guiding the search more efficiently and often faster.

3. Final Answer:

   A* uses a heuristic to guide the search, while Dijkstra explores all paths evenly. -> Option B

4. Quick Check:

   Heuristic guides A*; Dijkstra explores evenly [OK]

Hint: Remember: A* uses heuristics, Dijkstra does not [OK]

Common Mistakes:

• Confusing which algorithm uses heuristics
• Thinking both ignore obstacles
• Assuming both explore paths the same way

2. Which of the following is the correct way to initialize a Dijkstra planner in ROS using C++?

easy

A. auto planner = new DijkstraPlanner();

B. DijkstraPlanner planner = new DijkstraPlanner();

C. auto planner = DijkstraPlanner();

D. DijkstraPlanner planner();

Solution

1. Step 1: Understand object initialization in modern C++

   Using 'auto' with direct initialization calls the constructor properly and manages type automatically.

2. Step 2: Analyze each option

   auto planner = new DijkstraPlanner(); uses 'new' which returns a pointer, not an object. DijkstraPlanner planner = new DijkstraPlanner(); attempts to assign a pointer to an object, causing a type mismatch compile error. DijkstraPlanner planner(); is a function declaration, not an object. auto planner = DijkstraPlanner(); correctly initializes an object with automatic type deduction.

3. Final Answer:

   auto planner = DijkstraPlanner(); -> Option C

4. Quick Check:

   Use auto with constructor for clean initialization [OK]

Hint: Use 'auto planner = ClassName();' for simple object creation [OK]

Common Mistakes:

• Using parentheses that declare a function, not an object
• Using 'new' without managing pointers
• Not using auto for type deduction

3. Given the following ROS global planner code snippet using A*:

std::vector<Point> path = planner.makePlan(start, goal);
std::cout << path.size() << std::endl;

If the planner finds a path with 5 points, what will be printed?

medium

A. 0

B. 4

C. 6

D. 5

Solution

1. Step 1: Understand the return value of makePlan()

   The makePlan function returns a vector of points representing the path from start to goal, including both points.

2. Step 2: Analyze the output of path.size()

   If the planner finds a path with 5 points, the vector size is 5, so printing path.size() outputs 5.

3. Final Answer:

   5 -> Option D

4. Quick Check:

   Path points count equals vector size [OK]

Hint: Path size equals number of points including start and goal [OK]

Common Mistakes:

• Assuming path size excludes start or goal
• Confusing index with size
• Thinking path size counts edges, not points

4. You have this snippet in your ROS global planner node:

std::vector<Point> path = planner.makePlan(start, goal);
for (int i = 0; i <= path.size(); i++) {
  std::cout << path[i].x << "," << path[i].y << std::endl;
}

What is the error in this code?

medium

A. Using <= in the loop causes out-of-bounds access.

B. The loop variable should be a float, not int.

C. The path vector is not initialized.

D. The makePlan function does not return a vector.

Solution

1. Step 1: Analyze the loop condition

   The loop runs from i = 0 to i <= path.size(), which means it tries to access path[path.size()] on the last iteration.

2. Step 2: Understand vector indexing

   Vectors are zero-indexed, so valid indices are 0 to path.size() - 1. Accessing path[path.size()] causes out-of-bounds error.

3. Final Answer:

   Using <= in the loop causes out-of-bounds access. -> Option A

4. Quick Check:

   Loop index must be less than size, not less or equal [OK]

Hint: Use i < path.size() in loops to avoid out-of-bounds [OK]

Common Mistakes:

• Using <= instead of < in loops
• Confusing vector size with last index
• Assuming makePlan returns empty vector

5. In a ROS global planner using A*, you want to modify the heuristic to prefer paths farther from obstacles for safety. Which approach correctly applies this change?

hard

A. Add a penalty to the heuristic cost for points near obstacles.

B. Subtract a penalty from the heuristic cost for points near obstacles.

C. Add a penalty to the heuristic cost for points far from obstacles.

D. Ignore the heuristic and use Dijkstra instead.

Solution

1. Step 1: Understand heuristic modification goal

   To prefer paths farther from obstacles, the heuristic should favor those points by lowering their cost or penalizing points near obstacles.

2. Step 2: Analyze penalty application

   Adding a penalty to the heuristic cost for points near obstacles increases their cost, which discourages paths near obstacles, as desired for safety.

3. Step 3: Choose correct approach

   Add a penalty to the heuristic cost for points near obstacles. correctly adds a penalty to points near obstacles, increasing their heuristic cost and discouraging proximity for safer paths.

4. Final Answer:

   Add a penalty to the heuristic cost for points near obstacles. -> Option A

5. Quick Check:

   Modify heuristic cost to prefer points near obstacles [OK]

Hint: Adjust heuristic cost to favor desired path traits [OK]

Common Mistakes:

• Confusing penalty direction for heuristic
• Ignoring heuristic and switching algorithms unnecessarily
• Not understanding heuristic impact on path choice