ProjekinAja. Robotics Stage 6 · Planning Deployment Lesson 6.2
● Stage 6 · Lesson 6.2

Graph Search: A*, Heuristics, and Costmap Planning

A* is only as good as its heuristic — an inadmissible heuristic produces fast plans that are not optimal. An overestimating heuristic can cause A* to skip the optimal path, finding a longer but valid route around obstacles, because the heuristic misleads the search into expanding nodes in the wrong order first.

⏱ ~22 min read 🗺 Path planning ⚙ Nav2 global planner

🎯 By the end you'll understand

How a 2D occupancy grid becomes a graph that A* can search. Why Dijkstra is simply A* with a zero heuristic — and why that is inefficient. What admissibility means and why violating it destroys optimality guarantees. How planning directly on the raw occupancy grid produces paths that graze obstacles, and why the inflated costmap fixes this. How D* Lite replans incrementally when the map changes. And the specific failure mode — inadmissible heuristic or missing inflation — most likely to bite FollowBot in a real corridor.

KEY INSIGHTThe heuristic is the opinion; admissibility is the contract

You can make A* faster by making its heuristic bolder. But if the heuristic ever overestimates the true cost to the goal, the contract is broken — and A* will return a path that is not the shortest one. Worse, with a large enough overestimate, it can return a path that collides.

Path planning on a grid is a graph search problem with one extra ingredient: a heuristic — an estimate of how much work remains from any node to the goal. Get the heuristic right (never overestimate) and A* is provably optimal and complete. Get it wrong and you trade correctness for speed, sometimes catastrophically. Every Nav2 global planner parameter that says "inflation" or "heuristic weight" is touching this tradeoff. This lesson teaches you to feel it, not just name it.

🔑 The core tradeoff

A* with h = 0 is Dijkstra: optimal but explores every node within the cost horizon. A* with h = true distance is perfect: optimal and explores only the minimum nodes. A* with h > true distance is Weighted A*: fast but suboptimal — and if h >> true distance near obstacles, the suboptimality can exceed the obstacle margin, producing a plan that grazes or enters obstacle cells.

CORE CONCEPTThe grid is a discretized configuration space

Lesson 6.1 introduced configuration space (C-space): the abstract space of all robot configurations. For a non-holonomic differential-drive robot like FollowBot operating in 2D, the C-space is three-dimensional — (x, y, θ). But global path planning on a grid drops the orientation dimension and treats the robot as a point mass in 2D space. This is the standard approximation used by Nav2's A* global planner, and it works in practice because:

  1. The local planner (DWA) handles orientation and kinematic feasibility within a short horizon.
  2. Global collision checking is done via inflation: expanding obstacles by the robot's footprint radius, so the shrunken point-mass robot is safe wherever the inflated map is free.

Once you have a 2D occupancy grid (a W × H array where each cell is free, occupied, or unknown), you construct a graph:

Occupancy grid ▓ ▓ free = 0 occ = 100 → Graph (free nodes only) S G S = start G = goal Inflated costmap high cost (near wall) medium cost free (far from wall)
Left: raw occupancy grid (0 = free, 100 = occupied). Middle: graph extracted from free cells — wall cells are excluded, edges connect 8-connected free neighbours. Right: inflated costmap — proximity to walls adds traversal cost, steering plans away from obstacles even in theoretically free space.
💡 Why 8-connected?

A 4-connected grid only allows N/S/E/W moves. The shortest path between two diagonally displaced points on a 4-connected grid overestimates the true Euclidean distance (it requires zigzagging). An 8-connected grid allows diagonal moves at cost √2 and produces paths much closer to Euclidean-optimal. Nav2's NavFn planner uses a 5-point Laplacian (effectively 4-connected), while the newer Smac Planner 2D uses 8-connected A*.

DEFINITIONDijkstra: optimal, complete, and slow on large grids

Dijkstra's algorithm is the correct foundation. It maintains a priority queue (open set) ordered by g(n) — the cost of the cheapest known path from the start to node n. It expands the lowest-g node, updates its neighbours, and repeats until the goal is reached.

PropertyDijkstraA*
Priority keyg(n)f(n) = g(n) + h(n)
Heuristich(n) = 0 alwaysh(n) = admissible estimate
Nodes exploredAll within cost horizonOnly guided by h toward goal
Optimal?YesYes (if h admissible)
Complete?Yes (no negative edges)Yes (same condition)
Typical speedO(V log V)O(b^d) — b = branching factor directed by h

On a 20 × 20 grid with 400 cells, Dijkstra might expand all 350 free cells before reaching the goal in the far corner. A* with a good heuristic might expand only 60. On a 1000 × 1000 map — Nav2's typical SLAM output at 5 cm resolution — this difference is the line between a planner that responds in 50 ms and one that times out at 500 ms.

A* is Dijkstra with a non-zero heuristic added to the priority key:

a_star.pyPython · core loop
import heapq, math

def astar(grid, start, goal, h_fn):
    # grid: 2D list of costs (0=free, None=wall)
    rows, cols = len(grid), len(grid[0])
    g = {start: 0.0}
    parent = {start: None}
    open_heap = [(h_fn(start, goal), start)]   # f, node
    closed = set()

    while open_heap:
        _, node = heapq.heappop(open_heap)
        if node == goal:
            return reconstruct(parent, goal)
        if node in closed: continue
        closed.add(node)

        r, c = node
        for dr, dc in [(-1,0),(1,0),(0,-1),(0,1),
                        (-1,-1),(-1,1),(1,-1),(1,1)]:
            nr, nc = r+dr, c+dc
            if not (0 <= nr < rows and 0 <= nc < cols): continue
            if grid[nr][nc] is None: continue   # wall
            move_cost = math.sqrt(2) if (dr and dc) else 1.0
            ng = g[node] + move_cost + grid[nr][nc]  # costmap adds weight
            if (nr, nc) not in g or ng < g[(nr, nc)]:
                g[(nr, nc)] = ng
                parent[(nr, nc)] = node
                f = ng + h_fn((nr, nc), goal)
                heapq.heappush(open_heap, (f, (nr, nc)))

The only difference between Dijkstra and A* is line 20: f = ng + h_fn((nr, nc), goal). Remove h_fn and you have Dijkstra. The heuristic function is the entire engineering choice.

HOW IT WORKSA* admissibility, common heuristics, and the overestimation failure

A heuristic h(n) is admissible if it never overestimates the true cost from n to the goal:

heuristics.pyPython · four heuristic functions
import math

def h_zero(n, g):
    return 0           # Dijkstra: admissible, maximally uninformed

def h_manhattan(n, g):  # L1: admissible on 4-connected grids only
    return abs(n[0]-g[0]) + abs(n[1]-g[1])

def h_euclidean(n, g):  # L2: admissible on 8-connected grids (move cost >= 1)
    return math.sqrt((n[0]-g[0])**2 + (n[1]-g[1])**2)

def h_overestimate(n, g, w=3.0):  # inadmissible: violates guarantee
    return w * math.sqrt((n[0]-g[0])**2 + (n[1]-g[1])**2)

The subtle one is Manhattan vs. Euclidean. On a 4-connected grid (no diagonals), the true minimum cost of a path is exactly Manhattan distance, so Manhattan is admissible. On an 8-connected grid, diagonal moves cost √2, so the true minimum distance is the Chebyshev-like "octile distance" — and Manhattan now overestimates (it counts two cardinal moves for what costs √2 diagonally). Use Euclidean or octile distance for 8-connected grids.

⚠ Manhattan distance + 8-connected grid = inadmissible

This is a common mistake. Manhattan distance assumes each step costs exactly 1. On an 8-connected grid, the diagonal shortcut costs √2 ≈ 1.414 for two-cell displacement vs. Manhattan's cost of 2. Manhattan overestimates by up to ~41.4% (√2 − 1 ≈ 0.4142) — enough to produce suboptimal paths near diagonal shortcuts. Nav2's SmacPlanner uses octile distance (max(|dr|, |dc|) + (√2 - 1) · min(|dr|, |dc|)) for this reason.

The overestimation failure is more serious than suboptimality. If the heuristic weight is large near a narrow gap (where the true traversal cost spikes), A* will prefer a path that circumvents the gap by skimming through an inflated-cost zone — or, if inflation is absent, will route straight through the wall's shadow zone. The widget below lets you see this empirically.

SEE ITStep through A* — heuristics, inflation, and path quality

This widget runs A* step by step on a 12 × 18 costmap grid. Draw walls with the brush, set the heuristic, toggle costmap inflation, then step through the search or run it to completion. The worst choice you can make: pick the overestimating heuristic and disable inflation — watch the path graze (or enter) obstacle-adjacent cells and expand fewer nodes but return a worse route.

⚙ Interactive · A* step-runner on a costmap

Left-click/drag to draw walls. Right-click to erase. Drag S/G markers first to reposition start and goal (use the "Set" buttons below). Then pick a heuristic and step through.

Start
Goal
Wall
Open
Closed
Path
Inflated
Draw mode
Heuristic
Costmap inflation
Step: 0  |  Open: 0  |  Closed: 0
Path length: —
Heuristic: L2 Euclidean
Status: Ready

Key observations to look for: with h=0 (Dijkstra), the open set expands uniformly in all directions — correct but slow. With L2 (Euclidean), it expands primarily toward the goal — fast and still optimal. With 3× overestimate, it races to the goal but may pick a path adjacent to walls when the true optimal detours slightly. Toggle off inflation to see the most dangerous case: paths that skim wall boundaries, acceptable in simulation geometry but fatal in the real world where the robot occupies physical space.

IN PRACTICEPlanning on the costmap, not the occupancy grid

The single most important implementation detail in Nav2 global planning is this: never plan directly on the raw occupancy grid. Plan on the inflated costmap. Here is what "inflation" means concretely:

1

Mark obstacle cells

Any cell with occupancy ≥ the occupied threshold (default 65 out of 100) is marked LETHAL (cost = 254 in Nav2 costmap conventions). The planner will never route through these.

2

Inflate by robot inscribed radius

All cells within the robot's inscribed radius (largest circle that fits inside the footprint) of a lethal cell are marked INSCRIBED (cost = 253). A path through an inscribed cell is geometrically infeasible for the robot's footprint center — it would collide.

3

Inflate by inflation radius

Cells within the configurable inflation radius (typically 0.3–0.5 m) of a lethal cell receive a cost that decays exponentially from 252 toward 0 as distance to the nearest obstacle increases. The planner sees this as a "soft barrier" and routes through the center of corridors.

4

Plan on the costmap

A* runs on this costmap. Edge traversal costs include both the move cost (1.0 / √2) and the cell's inflation cost. Result: paths that are not just collision-free, but maximally clear of obstacles. A path through the center of a 1 m corridor costs less than one that skims the wall at 0.1 m.

⚠ Planning without inflation: the grazing path failure

If you plan on the raw occupancy grid (wall = blocked, everything else = equal cost), A* finds paths that are geometrically correct for a point robot. But FollowBot is not a point: its footprint is ~0.35 m × 0.40 m. A path that routes the point-mass robot through a cell 0.05 m from a wall will physically collide the robot's body into the wall. The symptom: the planner returns a valid path, the local planner follows it, and the robot grinds against the wall while the nav stack reports success. This is not a rare edge case — any corridor narrower than twice the robot radius will trigger it. Always plan on the inflated costmap.

The Nav2 costmap configuration for FollowBot's global planner looks like this:

global_costmap.yamlNav2 · FollowBot global costmap params
global_costmap:
  global_costmap:
    ros__parameters:
      update_frequency: 1.0          # Hz — infrequent: SLAM map changes slowly
      publish_frequency: 1.0
      global_frame: map
      robot_base_frame: base_link
      use_sim_time: false
      robot_radius: 0.22             # inscribed radius (m) — FollowBot footprint
      resolution: 0.05               # 5 cm per cell; matches SLAM output
      track_unknown_space: true       # treat unknowns as free (less conservative)
      plugins: ["static_layer", "inflation_layer"]
      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        map_subscribe_transient_local: true
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0     # exponential decay rate — higher = steeper cliff
        inflation_radius: 0.55       # 0.55 m clearance buffer beyond inscribed radius

The inflation_radius is the critical parameter. Set it too small (say 0.1 m) and paths graze walls. Set it too large (say 1.0 m) in a narrow corridor (say 1.2 m wide) and the inflated zones from both walls overlap — the planner sees the corridor as geometrically infeasible and returns failure. The right value is a function of your environment: typically 1.5–2× the robot radius for open spaces, with a recovery behavior that reduces it for tight passages.

HOW IT WORKSD* Lite: incremental replanning for dynamic obstacles

A* computes a plan once from the current pose to the goal, assuming the map is static. In real environments, this assumption fails: a person steps in front of FollowBot, a door closes, a box is moved. When the map changes, a naive approach re-runs A* from scratch — expensive on large maps.

D* Lite (Dynamic A*) solves this with incremental replanning: it maintains the previous search tree and, when cells change, repairs only the affected portion of the tree. The key insight:

📖 D* Lite vs. A* in Nav2

Nav2's standard global planner (NavFn) uses Dijkstra/A* run-from-scratch at the global planner frequency (typically 1 Hz). This is fast enough for static maps with an occupancy grid SLAM output. For environments with frequent dynamic obstacles — a warehouse with moving forklifts — D* Lite or its cousin LPA* is preferred. The local planner (DWA or TEB) handles short-horizon dynamic obstacles at 20–50 Hz. The global–local split means D* Lite's advantages only materialize when obstacles are large, persistent, and within the global costmap's scope. For FollowBot's indoor person-following, the Nav2 default (A* or NavFn + DWA local) is appropriate.

The practical consequence for FollowBot: when the tracked person occludes a doorway and a new obstacle appears in the costmap, the global planner replans at the next 1 Hz tick. The local planner's obstacle layer handles the person-as-obstacle in real time at 20 Hz. The gap between the two layers — 50 ms for the local layer, 1 s for the global — is why the behavior tree needs an "obstacle wait" node: the robot should pause 1–2 s near unexpected large obstacles rather than charging forward on a stale global plan.

FOLLOWBOTWiring A* into the Nav2 global planner stack

At this stage milestone, FollowBot's global planning layer is fully configured. Here is the concrete architecture:

SLAM Map /map · OccupancyGrid updated 0.3–1 Hz Global Costmap static_layer inflation_layer inflation_radius: 0.55 m 1 Hz update A* Global Planner NavFn / SmacPlanner heuristic: Euclidean on navigate_to_pose DWA Local Planner + local costmap LIDAR obstacle layer 20 Hz control /map costmap global path Behavior Tree: detect_person → navigate_to_goal → maintain_distance → handle_occlusion → recover calls NavigateToPose action → triggers A* replanning at each tick cmd_vel Person Tracker tracked goal pose
FollowBot's global planning stack. The SLAM map feeds a global costmap (static + inflation layers). A* runs on that costmap at the navigate_to_pose action call. The behavior tree ticks at ~5 Hz, feeding a new tracked goal pose, which triggers replanning. DWA runs at 20 Hz against the local costmap (LIDAR-updated). The two loop rates — 1 Hz global, 20 Hz local — is a fundamental architectural choice, not an implementation detail.
🔑 Stage 6 milestone: global planning layer

FollowBot now has: A* global planner on the SLAM-generated OccupancyGrid, with inflation_radius 0.55 m. DWA local planner with FollowBot's kinematic constraints (max_vel_x 1.2, acc_lim_x 1.5 m/s²). Note: minimum_turning_radius: 0 is a SmacPlannerHybrid parameter (global planner), not a DWA parameter — it is not used in the DWB local planner config. Behavior tree: detect_person → NavigateToPose → maintain_distance (stop if < 0.5 m) → handle_occlusion (search sweep if person lost > 3 s) → recovery (clear costmap + replan). Next: Lesson 6.3 sampling-based planners for arm planning in the 6-DOF arm workspace, and Lesson 6.4 DWA deep-dive.

One honest performance note: in simulation, the A* planner on a clean SLAM map produces plans that appear optimal. On the real robot, the SLAM map has sensor noise artifacts that inflate small clusters of cells at spurious positions. A path that looks wide-open in RViz may route through a phantom-obstacle region in the real map. The correct fix is not to reduce the inflation radius — it is to tune SLAM map quality (scan match parameters, loop closure frequency) and add a map post-processing step that removes small isolated obstacles below a minimum cluster size. This is the sim-to-real gap in planning, and it is discussed in detail in the Stage 6 capstone.

WEAKNESSWhere A* and costmap planning fail in practice

⚠ Planning on the raw occupancy grid without inflation

This is the most common implementation mistake in first-time Nav2 deployments. The planner produces geometrically valid paths and the simulation looks clean. On the real robot, every tight corridor results in wall contact because the robot body is not a point. The symptom — robot scrapes the wall but never hard-crashes — is especially insidious because the behavior can be intermittent (depends on map registration accuracy) and looks like a local planner tuning problem. It is not. Always configure the inflation layer in the global costmap before any real-world deployment.

⚠ Inadmissible heuristic + weighted A* in tight spaces

Weighted A* (multiplying the heuristic by a constant w > 1) is a popular speedup for large maps: it is w-suboptimal (the returned path cost is at most w × optimal). The danger is that "w-suboptimal in cost" can mean "not w-suboptimal in distance from obstacles." If the costmap gives cells near walls a cost of 50 (out of 252), a 3× weighted heuristic can produce a path with total cost 3× the optimal but whose geometric route passes through cells with cost 50 — which are only centimeters from an obstacle. The suboptimality bound applies to cumulative cost, not to clearance. Use weighted A* only on open maps or with a clearance floor that prevents routing through high-cost cells.

⚠ Inflation radius too large for the environment

Setting inflation_radius: 1.0 in a 1.2 m corridor causes the planner to see the corridor as geometrically infeasible (inflated obstacles from both walls overlap). The planner returns failure; the behavior tree triggers recovery; the robot spins in place. This failure appears to be a sensor or localization problem — "the robot can't find itself" — but is actually a costmap configuration problem. The right inflation radius is environment-specific. For FollowBot in a typical office, 0.45–0.55 m provides adequate clearance without blocking standard doorway widths (0.9 m). Implement a recovery behavior that attempts replanning with a reduced inflation radius before declaring navigation failure.

KEY INSIGHTHow it connects

Graph search on a costmap is the workhorse of mobile robot global planning. It is not glamorous, but every failure in the list above is common in real deployments and almost never caused by the algorithm itself — it is caused by misconfigured costmap parameters or an inadmissible heuristic added to speed things up without understanding the consequences. The widget in this lesson exists specifically to make those consequences visible before you hit them in the field.

GLOSSARYKey terms

Admissible heuristic
A heuristic h(n) is admissible if h(n) ≤ h*(n) for all nodes n, where h*(n) is the true minimum cost from n to the goal. Admissibility is the condition under which A* is guaranteed to return an optimal path.
Costmap
A 2D grid in which each cell contains a cost value that encodes both the presence of obstacles (lethal/inscribed cost) and proximity to obstacles (inflation cost). The global planner in Nav2 routes through the costmap, preferring cells with low cost (far from obstacles).
Inflation layer
A Nav2 costmap layer that expands each obstacle cell by a configurable radius, assigning exponentially decaying cost to cells in the inflation zone. Ensures the point-mass planner keeps the robot's footprint away from obstacle boundaries.
Inflation radius
The distance (in metres) around an obstacle within which the inflation layer assigns non-zero traversal cost. Setting this correctly — wide enough to protect the robot body, narrow enough not to block tight passages — is one of the most important Nav2 tuning decisions.
D* Lite
An incremental replanning algorithm that maintains the previous A* search tree and repairs only the affected portion when cells change. More efficient than rerunning A* from scratch when obstacles change dynamically; useful in environments with frequent large-scale changes to the map.
Weighted A* (w-A*)
A* with the heuristic multiplied by a constant w > 1. Explores fewer nodes and returns a path within a factor w of optimal. Inadmissible (violates the h ≤ h* constraint) but provides a bounded suboptimality guarantee. Dangerous in costmap planning where suboptimality may map to insufficient obstacle clearance.
Octile distance
The admissible heuristic for 8-connected grids: max(|dr|, |dc|) + (√2 − 1) · min(|dr|, |dc|). Correctly accounts for the cost of diagonal moves (√2) and is tighter than Euclidean distance for large grids.
NavFn
Nav2's default global planner, implementing a Dijkstra/A* variant using a potential field approach (wavefront propagation from the goal). Fast and well-tested; lacks the kinematic feasibility constraints of the newer SmacPlanner series.

ACTIVE RECALLCheck your understanding

Answer in your own words first, then reveal.

1Why is A* with a zero heuristic exactly equivalent to Dijkstra's algorithm? What does setting h = 0 mean operationally?›

A* expands nodes in order of f(n) = g(n) + h(n). Setting h(n) = 0 for all n reduces the priority key to g(n) alone — exactly Dijkstra's criterion. Operationally, h = 0 is admissible (0 never overestimates any non-negative cost) and means the algorithm has no directional preference: it expands all nodes within the current cost frontier equally, regardless of their geometric relationship to the goal. This is optimal but maximally inefficient on large maps where most of the explored nodes are irrelevant to the optimal path. A positive h functions as a directional bias that prunes the search space toward the goal.

2FollowBot's global planner is returning a path that routes within 0.05 m of a wall. The robot follows this path and scrapes the wall. Is this a planner bug, a costmap configuration issue, or a local planner issue? How do you diagnose it?›

Almost certainly a costmap configuration issue: the inflation radius is too small (or the inflation layer is missing entirely), so the global planner treats cells near the wall as equally cost-free as cells in the corridor center. The planner is doing exactly what it was configured to do — route the point-mass robot through the cheapest cells. Since the robot is not a point, collision results. Diagnose by: (1) visualizing the global costmap in RViz — free cells should show a cost gradient near walls, with clearly elevated cost close to obstacles; (2) checking inflation_radius in the global costmap YAML — if it is 0 or missing, that is the fault. The local planner and the path planning algorithm itself are not at fault. Fix: set inflation_radius ≥ robot inscribed radius + desired clearance margin.

3You are tuning Nav2 and someone suggests setting the heuristic weight to 5.0 to make planning faster. What is the guaranteed property of the paths this will return, and what property is lost?›

Weighted A* with w = 5.0 guarantees a w-suboptimal path: the cost of the returned path is at most 5× the cost of the optimal path. This is the property gained (faster planning by exploring fewer nodes). The property lost is strict optimality: the path is no longer guaranteed to have the minimum cost. In practice the paths are often close to optimal, but the 5× bound can be loose in obstacle-rich environments. More importantly, the suboptimality bound applies to path cost, not obstacle clearance — a 5× costmap-cost suboptimal path may route through cells that are dangerously close to obstacles if the costmap's high-cost zone near obstacles was part of the savings. For safety-critical deployment, use admissible heuristics (w ≤ 1.0); use weighted A* only in environments where suboptimality can be demonstrated to not reduce clearance below the minimum required margin.

4When would you prefer D* Lite over A* for FollowBot's global planner, and when is the standard Nav2 A* approach adequate?›

Prefer D* Lite when: (1) the global map changes frequently and substantially — e.g., a warehouse with moving shelves or people adding large obstacles to the global costmap; (2) replanning latency is the bottleneck — D* Lite is O(changes) rather than O(map size), so it pays off when a small fraction of the map changes per planning cycle. The standard Nav2 A* approach is adequate when: (1) the global costmap is quasi-static (SLAM map from a mapped environment, not updated at runtime); (2) the local planner's obstacle layer handles the dynamic objects that appear during navigation; (3) the global planner's 1 Hz replanning from scratch is fast enough given the map size. For FollowBot in a mapped indoor office (30 m × 20 m, 5 cm resolution = 600 × 400 = 240,000 cells), A* from scratch at 1 Hz is typically 20–100 ms — fast enough. D* Lite would be the right choice for a large warehouse or a frequently updated map.

Next: sampling-based planning for high-dimensional C-spaces →