Path Planners NavFn and Smac

The global costmap tells us the "toll" for every cell. So how do we find the route from start to goal that pays the least total toll? That is exactly the job of the path planner.

If You View the Costmap as a Graph, It Becomes Shortest-Path

Treating each costmap cell as a node and its neighbors as edges reduces this to a classic graph-based shortest-path problem. Nav2 ships two primary planner implementations.

• NavFn — Dijkstra/A* family. Propagates a "potential field" across the costmap from the goal, then backtracks from the goal to extract the path.
• Smac Planner — grid-based (2D) or Hybrid-A* (kinematically constrained). Suited for robots with a non-negligible turning radius.

Rendering diagram…

The intuition: a wave expands outward from the start, accumulating cost. When it reaches the goal, the path is traced back down the lowest-cost gradient. Lethal cells (walls) block the wave, so the path naturally routes around them.

In code, this wavefront propagation is just Dijkstra.

# propagate cost outward from the start like a wavefront (Dijkstra / NavFn)
frontier = [(0, start)]
while frontier:
    cost, cell = heappop(frontier)
    for nb in neighbors(cell):
        new = cost + costmap[nb]          # accumulate cell toll
        if new < dist[nb]:
            dist[nb] = new
            heappush(frontier, (new, nb))
# backtrace dist from the goal to get the shortest path

Planner and Controller Have Separate Responsibilities

An important distinction: the planner only determines where to go (the global path). Actually translating that path into wheel velocities is the controller's job, covered in the next post.

Rendering diagram…

This separation of concerns lets the planner focus on long-range routing and the controller focus on real-time tracking and avoidance.

▶ Simulation footage — a warehouse aisle seen from above. The straight cyan dashed line down the middle of the aisle is the path the planner produced; the winding ribbon beside it is the track the controller actually drove. Ribbon colour encodes speed (blue = near standstill, cyan/green = moving), and here it stays blue throughout — HO_A in this run was commanded at 1.2 m/s but averaged 0.25 m/s while waiting to pass an oncoming robot. The plan is straight and the track bends — planner and controller are solving different problems on the way to the same goal.

When the Costmap Cannot Reach the Goal — Off Grid

The most common planner failure is not an algorithmic one; it is a costmap coverage problem.

⚠️ "Pose Goes Off Grid" — if the goal (or the robot itself) is outside the area covered by the costmap, the planner cannot compute a path to an unknown coordinate and fails (failed to create plan). This typically happens when the environment extends beyond the positive-quadrant boundary but the costmap size is set too small.

💡 Plugin name notation — depending on the Nav2 version, plugin path notation varies (/ vs ::). A mismatch prevents the planner from loading entirely. Check the notation convention for your distribution.

When a planner fails, the first thing to check is not the algorithm — it is whether the costmap covers both the start and the goal.

In a nutshell

📌 The planner treats the costmap as a graph and solves for the lowest-cost path (NavFn = Dijkstra/A*, Smac = grid/Hybrid-A*). Path planning (planner) and path following (controller) are separate responsibilities. The most common planner failure is not an algorithm bug — it is the goal falling Off Grid outside the costmap coverage.