A* Algorithm

The A* (A-Star) algorithm is a best-first search algorithm that finds the shortest path between two nodes on a weighted graph.

How It Works

A* uses a heuristic function combined with the actual cost to estimate the total cost .

Term	Meaning
g(n)	Cost from start to current node
h(n)	Estimated cost from current node to goal
f(n)	Total estimated cost

Python Implementation

import heapq

def astar(grid, start, goal):
    open_set = []
    heapq.heappush(open_set, (0, start))
    came_from = {}
    g_score = {start: 0}

    while open_set:
        _, current = heapq.heappop(open_set)
        if current == goal:
            path = []
            while current in came_from:
                path.append(current)
                current = came_from[current]
            return path[::-1]

        for neighbor in get_neighbors(grid, current):
            tentative_g = g_score[current] + 1
            if tentative_g < g_score.get(neighbor, float('inf')):
                came_from[neighbor] = current
                g_score[neighbor] = tentative_g
                f = tentative_g + heuristic(neighbor, goal)
                heapq.heappush(open_set, (f, neighbor))
    return []

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

Usage in ROS 2

In Nav2, A* is available as the nav2_navfn_planner plugin. Set the planner in your nav2_params.yaml:

planner_server:
  ros__parameters:
    planner_plugins: ["GridBased"]
    GridBased:
      plugin: "nav2_navfn_planner/NavfnPlanner"
      use_astar: true

TIP

Use the Manhattan distance heuristic for grid maps and Euclidean distance for free-space planning.