Navigation · Theory · M3
M3-T7 — Planning on a Costmap
A planner that minimises distance hugs walls. A planner that minimises cost keeps its distance, and has to be told what to do when the goal is in the wall.
The grid planners of this module minimise a number. If that number is length, the best route runs along every wall and around every corner. The costmap of module M2 says where it is unwise to be; this lesson turns that advice into step costs, decides which cells are forbidden rather than merely dear, adds the corner rule, and deals with the goals and starts that a real building throws at a planner.
Learning objectives
After this lesson, you should be able to:
- Write a step cost that combines distance with the costmap value of the cell entered.
- Explain which cells are forbidden and which are only expensive, and why the difference matters.
- Apply the no-corner-cutting rule and a goal tolerance.
- List what a path needs for a controller to follow it, and what a planner should do with a start or goal it cannot use.
The intuition
Walking down a corridor you do not take the mathematically shortest line along the wall. You drift to the middle, partly because it is easier to see around, partly because you might be passed by a bed. The costmap lets a robot behave like that: cells close to a wall carry a cost, and the planner trades a longer route against a cheaper one. How much longer it will go depends on how steeply the cost rises.
Two kinds of cell must not be confused. Cells that are merely expensive may be used when it pays: a doorway is expensive and has to be used. Cells in which the robot's centre would touch an obstacle, the inscribed and lethal cells, are not expensive but impossible. A planner that treats them as costly will, sooner or later, find a route through them because it is a little shorter.
The mathematics
Step cost
A common form multiplies the length of a move by a factor that grows with the costmap value c of the cell entered:
- α
- how much a fully costly cell is worse than a free one. With α = 4 a cell at cost 252 is five times as dear as free floor.
- c(b)
- costmap value of the cell entered, 0 to 252 for passable cells; 253 (inscribed) and 254 (lethal) cells are removed from the graph.
- 1 + α·c/252
- the multiplier. It is also the break-even detour ratio: a route through cells of multiplier m is preferred to a detour only if the detour is less than m times as long.
Dijkstra and A* work unchanged. The distance heuristic of lesson M3-T4 stays admissible, because the multiplier is never below 1.
Corner cutting
A diagonal move from a to b passes the corners of the two cells beside the move. The strict rule allows the move only if both of those cells are passable. A looser rule needs only one. The strict rule is what stops a path from slipping through the gap between two walls that touch at a corner, and from shaving a corner so close that the robot's body hits it. Because the inflation already keeps the centre away from walls, the rule matters most with a thin inflation or a planner that works on the raw map.
Start and goal
| Situation | What goes wrong | Common handling |
|---|---|---|
| Goal cell is lethal or inscribed | the goal is not a node; the search never finishes | goal tolerance: plan to the reachable cell closest to the goal within the tolerance |
| Start cell is inside the inflated band | no legal first move, because every neighbour is also forbidden | allow the start cell and a few cells around it to be traversed, or push the start to the nearest free cell |
| Goal in unknown space | unknown cells have no cost | treat unknown as free (risky) or as blocked, by a policy set in lesson M2-T1 |
| Goal unreachable | the search expands the whole map before failing | bound the search, return failure fast and hand over to recovery (module M7) |
What a controller needs
A grid path is a list of cells. A controller needs a sequence of poses it can follow: positions spaced closely enough to track, headings, and curvature that the robot can drive. The planner's job ends with a path that is collision-free on the inflated costmap, starts at the robot and ends within the tolerance of the goal. Turning cells into a smooth trajectory with speeds is the subject of module M5; the controller should receive the path in the map frame with a timestamp so it can reject a stale one.
A worked example
Multipliers. Take the costmap of lesson M2-T2: inscribed radius 0.22 m, inflation radius 0.55 m, decay factor 6, on a 0.1 m grid, with α = 4.
distance to wall cost c multiplier 1 + 4c/252 0.20 m 253 forbidden 0.30 m 155 3.46 0.40 m 85 2.35 0.50 m 46 1.73 0.55 m 34 1.54 0.60 m 0 1.00
Wall or middle? Scout must travel 30 m (300 cells) down a corridor 1.6 m wide. Hugging the wall at 0.30 m costs 300 × 3.46 = 1038 units. Driving down the middle costs 300 units plus the cost of getting there, a few cells at multipliers between 3.46 and 1. The middle is cheaper by a factor of three, so the planner takes it. The break-even is also useful in the other direction: a detour is worth taking only if it is shorter than 3.46 times the length of the wall-hugging stretch it avoids, so with α = 4 a planner will go a long way round to stay out of the dear band. With α = 0.5 the multiplier at cost 155 is 1.31, and the same planner cuts close to walls to save 30%.
What a cut corner saves. Cutting a wall corner with a diagonal rather than going round it saves 2 − √2 = 0.586 cells, 5.9 cm on a 0.1 m grid. That is a tiny gain against the risk of contact, which is why the strict rule costs almost nothing in path length.
The pharmacy hatch. The hatch is a cell in the wall, so it is lethal. Cells at 0.1 m and 0.2 m from the wall are inscribed (0.22 m), so the closest passable cell is 0.3 m from the hatch. A tolerance of 0.25 m gives no path; a tolerance of 0.30 m finds the cell, and the path ends in front of the hatch. The tolerance is a permission: the planner must end as near as it can, not at the edge of the allowed circle (lab M3-L3).
Engineering implications and limitations
Choose α with a map you can see
The factor α is a tuning knob with no right answer. Too small, and paths hug walls and cut through the dear band; too large, and the robot takes long detours to avoid slightly costly cells or refuses narrow corridors in favour of wide ones. Tune it by drawing the costmap and several routes on top, and by testing the narrowest door the robot must use. Planners in the Nav2 family expose a similar scale between distance and cost; check the name and default in the documentation of the planner you use.
Forbidden is not expensive
Never encode inscribed cells as a very large finite cost. A large cost is eventually beaten by a long enough detour, and the robot is then told to go through a wall rather than around the building. Remove those cells from the graph, and report failure when no route exists.
Use the same costmap as the controller
If the global planner uses one footprint or inflation and the local planner another, the path will pass a door that the controller refuses. Define the footprint once (lesson M1-T4) and share it.
Path quality checks
Before handing a path to the controller, check that it is free on the current costmap, that its first pose is near the robot, and that its length is not far above the straight-line distance. A path that is ten times longer than the straight line has probably gone round a closed door and should be reported.
Replanning needs hysteresis
Costs change as people walk by. Keep the current path unless the new one is clearly better, otherwise the robot switches between routes of nearly equal cost (lesson M3-T6).
Common misconceptions
“The cheapest route is the shortest route with a safety margin.”
It is a different route: the planner pays for every cell it uses, so it can go a long way round a dear band. The margin is not additive.
“Giving inscribed cells a huge cost is the same as forbidding them.”
A finite cost can be beaten by a long enough detour or a long enough route of cheap cells; forbidden cells are not in the graph at all.
“If the goal is in a wall the planner is broken.”
The goal is not reachable as a cell. A tolerance makes the request sensible, and the planner should say how far from the goal it stopped.
“A collision-free grid path is ready to drive.”
It is a sequence of cells with 45-degree turns. It must be smoothed and given speeds, and the controller must be able to follow it within its limits (module M5).
Engineering takeaways
- Step cost = length × (1 + α·c/252) for passable cells; the multiplier is the detour ratio at which the planner would rather go round.
- Inscribed and lethal cells are removed from the graph, not made expensive.
- Allow a diagonal only when both cells beside it are free; the rule costs almost no path length.
- A goal inside the inflated band needs a tolerance, and the planner should end as near to the goal as it can.
- Share one footprint and costmap between planner and controller, and hand over a path in the map frame that is free on the current costmap.
References and further reading
Costmap-based planning in Nav2 and the layered costmap paper.
- D. V. Lu, D. Hershberger and W. D. Smart, “Layered Costmaps for Context-Sensitive Navigation”, IEEE/RSJ International Conference on Intelligent Robots and Systems, 2014.
- S. Macenski, F. Martín, R. White and J. Ginés Clavero, “The Marathon 2: A Navigation System”, IEEE/RSJ International Conference on Intelligent Robots and Systems, 2020.
- Nav2 documentation, “Navigation Plugins: Planners” and “Costmap 2D” configuration pages, docs.nav2.org.
Knowledge check
Three conceptual questions. Write an answer, then reveal the explanation. Your answers stay in your browser.
Question 1
With α = 4, what is the step multiplier in a cell of cost 126? A stretch of 4 such cells can be avoided by a detour of 9 cells of free floor. Is the detour taken?
Explanation
1 + 4 × 126/252 = 3. The four cells cost 4 × 3 = 12 units; the detour costs 9 × 1 = 9 units. It is cheaper, so the planner takes the detour.
Question 2
A diagonal move goes from (5,5) to (6,6). Cell (5,6) is free and (6,5) is a wall. May the planner take it?
Explanation
Not under the strict rule: both cells beside the move must be free. The body would pass the corner of the wall cell (6,5).
Question 3
The inscribed radius is 0.22 m on a 0.1 m grid and the goal is a wall cell. What is the smallest tolerance that gives a path?
Explanation
0.3 m: the cells at 0.1 and 0.2 m from the wall are inscribed, so the closest passable cell is 0.3 m away.
What’s next
Next: the labs. Implement Dijkstra, A* with a replay, fix a corner-cutting planner and replan when a corridor closes.
Related content
Nothing below is required to finish this lesson.
- BeforeM3-T6: Replanning: D* Lite and LPA*
Describe how incremental search reuses earlier work when costs change, and when replanning from scratch is simpler.
- LabM3-L3: Fix a Corner-Cutting Planner
The route squeezes diagonally past a wall corner and scrapes it: fix the planner so it never cuts corners.
Get notified when the next lesson and new labs are released.
One email per new lesson or lab.