A* Path Planning, Step by Step on an Obstacle Grid
How much search can a good estimate remove? Edit the walls, switch between A* and Dijkstra, and compare path length with the number of expanded cells.
Primary sources checked: 31 August 2026
01
Watch the search front move
Click cells to edit obstacles. Step expands one node; Run continues until the goal or until no route remains.
Expanded
0
Frontier
1
Path length
—
Green is start, red is goal, navy is closed, yellow is the frontier, and the white trace is the final path.
02
A* adds one thing: an estimate of what remains
Dijkstra's algorithm expands locations by the cost already paid, g. A* adds an estimate h of the remaining cost and chooses the next location by f = g + h.
This demo allows only four-direction movement, so Manhattan distance is a natural h. Set the algorithm to Dijkstra and h becomes zero. The final route may stay the same while the explored area changes sharply.
03
A heuristic is guidance, not a shortcut guarantee
When h does not overestimate the true remaining cost, A* can preserve shortest-path optimality while focusing the search. Change the map, motion model, or cost definition and the appropriate heuristic changes too.
A real robot planner must handle footprint, turning radius, inflated costs, changing obstacles, and replanning. Nav2's Smac Planner includes 2D A*, Hybrid-A*, and State Lattice options; this four-neighbour grid is a teaching model, not a drop-in replacement.
04
A minimal Python implementation
It uses only heapq and returns the cells from start to goal.
Python 3a_star_grid.py
from heapq import heappop, heappush
def astar(grid, start, goal):
"""4-neighbour A* for a grid where 1 means blocked."""
height, width = len(grid), len(grid[0])
frontier = [(0, start)]
came_from = {start: None}
cost = {start: 0}
def heuristic(cell):
return abs(cell[0] - goal[0]) + abs(cell[1] - goal[1])
while frontier:
_, current = heappop(frontier)
if current == goal:
break
row, col = current
for next_cell in ((row-1, col), (row+1, col),
(row, col-1), (row, col+1)):
r, c = next_cell
if not (0 <= r < height and 0 <= c < width):
continue
if grid[r][c] == 1:
continue
new_cost = cost[current] + 1
if next_cell not in cost or new_cost < cost[next_cell]:
cost[next_cell] = new_cost
priority = new_cost + heuristic(next_cell)
heappush(frontier, (priority, next_cell))
came_from[next_cell] = current
if goal not in came_from:
return []
path, current = [], goal
while current is not None:
path.append(current)
current = came_from[current]
return path[::-1]
05
What this sample deliberately leaves out
Diagonal motion, terrain costs, and robot footprint
Velocity, acceleration, and turning constraints
Sensor updates, dynamic obstacles, and path smoothing