Global planner (A*, Dijkstra) in ROS

Introduction

A global planner helps a robot find the best path from start to goal on a map. It uses smart algorithms like A* or Dijkstra to avoid obstacles and reach the destination efficiently.

When a robot needs to move safely from one place to another in a known environment.

When you want to find the shortest or fastest path on a map with obstacles.

When planning routes for delivery robots inside buildings or warehouses.

When combining with local planners to navigate complex spaces smoothly.

Syntax

ROS

class GlobalPlanner:
    def __init__(self, costmap):
        self.costmap = costmap

    def plan(self, start, goal):
        # Implement A* or Dijkstra algorithm here
        pass

The planner class needs a costmap that shows obstacles and free space.

The plan method takes start and goal positions and returns a path.

Examples

Dijkstra explores all nodes evenly to find the shortest path.

ROS

def dijkstra(self, start, goal):
    # Initialize distances and priority queue
    # Explore nodes with lowest cost first
    # Update distances and parents
    # Return path when goal reached

A* uses a heuristic to guide the search faster towards the goal.

ROS

def a_star(self, start, goal):
    # Use a heuristic to estimate distance to goal
    # Explore nodes with lowest cost + heuristic
    # Update distances and parents
    # Return path when goal reached

Choose between A* or Dijkstra based on settings.

ROS

def plan(self, start, goal):
    if use_a_star:
        return self.a_star(start, goal)
    else:
        return self.dijkstra(start, goal)

Sample Program

This program creates a simple 5x5 grid map with some obstacles. It defines a GlobalPlanner class that can find paths using Dijkstra or A* algorithms. The planner checks neighbors, calculates costs, and reconstructs the path. It prints the map and the paths found by both algorithms.

ROS

import heapq

class GlobalPlanner:
    def __init__(self, costmap):
        self.costmap = costmap
        self.width = len(costmap[0])
        self.height = len(costmap)

    def neighbors(self, node):
        x, y = node
        results = []
        for dx, dy in [(-1,0),(1,0),(0,-1),(0,1)]:
            nx, ny = x + dx, y + dy
            if 0 <= nx < self.width and 0 <= ny < self.height:
                if self.costmap[ny][nx] == 0:  # 0 means free space
                    results.append((nx, ny))
        return results

    def dijkstra(self, start, goal):
        dist = {start: 0}
        prev = {}
        queue = [(0, start)]

        while queue:
            current_dist, current_node = heapq.heappop(queue)
            if current_node == goal:
                break
            for neighbor in self.neighbors(current_node):
                alt = current_dist + 1  # cost between neighbors is 1
                if alt < dist.get(neighbor, float('inf')):
                    dist[neighbor] = alt
                    prev[neighbor] = current_node
                    heapq.heappush(queue, (alt, neighbor))

        path = []
        node = goal
        while node != start:
            path.append(node)
            node = prev.get(node)
            if node is None:
                return []  # no path
        path.append(start)
        path.reverse()
        return path

    def a_star(self, start, goal):
        def heuristic(a, b):
            return abs(a[0] - b[0]) + abs(a[1] - b[1])

        dist = {start: 0}
        prev = {}
        queue = [(heuristic(start, goal), start)]

        while queue:
            current_f, current_node = heapq.heappop(queue)
            if current_node == goal:
                break
            for neighbor in self.neighbors(current_node):
                tentative_g = dist[current_node] + 1
                if tentative_g < dist.get(neighbor, float('inf')):
                    dist[neighbor] = tentative_g
                    prev[neighbor] = current_node
                    f = tentative_g + heuristic(neighbor, goal)
                    heapq.heappush(queue, (f, neighbor))

        path = []
        node = goal
        while node != start:
            path.append(node)
            node = prev.get(node)
            if node is None:
                return []  # no path
        path.append(start)
        path.reverse()
        return path

    def plan(self, start, goal, use_a_star=True):
        if use_a_star:
            return self.a_star(start, goal)
        else:
            return self.dijkstra(start, goal)

# Example costmap: 0 = free, 1 = obstacle
costmap = [
    [0, 0, 0, 0, 0],
    [0, 1, 1, 1, 0],
    [0, 0, 0, 1, 0],
    [0, 1, 0, 0, 0],
    [0, 0, 0, 1, 0],
]

planner = GlobalPlanner(costmap)
start = (0, 0)
goal = (4, 4)

print("Costmap:")
for row in costmap:
    print(row)

print(f"\nPlanning path from {start} to {goal} using Dijkstra:")
path_dijkstra = planner.plan(start, goal, use_a_star=False)
print(path_dijkstra)

print(f"\nPlanning path from {start} to {goal} using A*:")
path_a_star = planner.plan(start, goal, use_a_star=True)
print(path_a_star)

Important Notes

Both A* and Dijkstra have time complexity O(E + V log V), where E is edges and V is nodes.

Space complexity depends on map size and data stored for nodes.

Common mistake: forgetting to check if neighbors are inside map and free space.

Use Dijkstra when no heuristic is available; use A* when you can estimate distance to goal for faster search.

Summary

Global planners find paths on maps avoiding obstacles.

Dijkstra explores all paths evenly; A* uses a heuristic to speed up search.

Both return a list of points from start to goal for the robot to follow.

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