Global planner (A*, Dijkstra) in ROS - Step-by-Step Execution

Concept Flow - Global planner (A*, Dijkstra)

Start Node

↓

Add Start to Open Set

↓

While Open Set not empty

↓

Select Node with Lowest Cost

↓

Is Node Goal?

Yes→Path Found

No↓

Expand Neighbors

↓

Update Costs and Parents

↓

Add Neighbors to Open Set

Back to While Loop↓

No Path Found

↩Back to While Loop

The planner starts at the start node, explores nodes with the lowest cost, updates neighbors, and repeats until it finds the goal or exhausts options.

Execution Sample

ROS

open_set = {start}
while open_set:
  current = node with lowest cost
  if current == goal:
    return path
  for neighbor in neighbors:
    update costs
    add neighbor to open_set

This code shows the main loop of A* or Dijkstra exploring nodes until the goal is found.

Execution Table

Step	Current Node	Open Set	Closed Set	Neighbors Expanded	Action
1	Start	{Start}	{}	A, B	Add Start to closed set, add neighbors A, B to open set
2	A	{B, C}	{Start, A}	C	Move A to closed set, add neighbor C to open set
3	B	{C, D}	{Start, A, B}	D	Move B to closed set, add neighbor D to open set
4	C	{D, Goal}	{Start, A, B, C}	Goal	Move C to closed set, neighbor is goal, path found
5	-	-	-	-	Exit loop, path reconstructed
6	-	-	-	-	End

💡 Goal node reached at step 4, path found and algorithm terminates

Variable Tracker

Variable	Start	After Step 1	After Step 2	After Step 3	After Step 4	Final
open_set	{Start}	{A, B}	{B, C}	{C, D}	{D, Goal}	{D}
closed_set	{}	{Start}	{Start, A}	{Start, A, B}	{Start, A, B, C}	{Start, A, B, C, Goal}
current_node	None	Start	A	B	C	Goal
path	[]	[]	[]	[]	[Start -> A -> C -> Goal]	[Start -> A -> C -> Goal]

Key Moments - 3 Insights

Why do we add neighbors to the open set only if they are not in the closed set?

Because nodes in the closed set are already fully explored, adding them again would cause unnecessary repeated work. See execution_table rows 2-4 where closed_set grows and neighbors are only added if not closed.

How does the algorithm decide which node to explore next?

It picks the node with the lowest cost from the open set, ensuring the shortest path is found efficiently. This is shown in execution_table where current_node changes to the lowest cost node each step.

What happens if the goal is unreachable?

The open set eventually becomes empty without reaching the goal, causing the algorithm to stop and report no path. This is implied in the concept_flow where the loop ends if open set is empty.

Visual Quiz - 3 Questions

Look at the execution table, what is the open set after step 2?

A {A, B}

B {B, C}

C {C, D}

D {Goal}

💡 Hint

Check the 'Open Set' column in execution_table row for step 2

At which step does the algorithm find the goal node?

A Step 2

B Step 3

C Step 4

D Step 5

💡 Hint

Look at the 'Neighbors Expanded' and 'Action' columns in execution_table

If the neighbor 'D' was not added at step 3, what would happen to the open set at step 4?

A {C}

B {C, D}

C {Goal}

D {}

💡 Hint

Refer to the 'Open Set' changes in execution_table rows 3 and 4

Concept Snapshot

Global planner uses A* or Dijkstra to find shortest path.
Start with open set containing start node.
Repeat: pick lowest cost node, expand neighbors.
Add neighbors to open set if not visited.
Stop when goal is reached or no nodes left.
Path is reconstructed from parents.

Full Transcript

The global planner algorithm starts by placing the start node in the open set. It then enters a loop where it selects the node with the lowest cost from the open set. If this node is the goal, the path is found and the algorithm stops. Otherwise, it expands the neighbors of the current node, updating their costs and parents, and adds them to the open set if they have not been visited before. The current node is moved to the closed set to mark it as explored. This process repeats until the goal is found or the open set is empty, indicating no path exists. The execution table shows each step's current node, open and closed sets, neighbors expanded, and actions taken. Variables like open_set and closed_set change as nodes are explored. Key moments clarify why neighbors are only added if not closed, how the next node is chosen by lowest cost, and what happens if the goal is unreachable. The visual quiz tests understanding of open set contents and goal discovery steps. The concept snapshot summarizes the algorithm flow and stopping conditions.

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