Chapter 08 · Map, localize, navigate, and recover with Nav2
Today in the field story
One problem, then the next
Run the Medicine-Cart A* starter before selecting Nav2 plugins. The open set represents frontier choices, accumulated cost records travel already paid, and an admissible Manhattan heuristic estimates what remains without exaggeration. Then trace one NavigateToPose request through planner, optional smoother, repeated controller, progress checker, goal checker, and terminal result. A path proposal and physical tracking remain different jobs.
- Why now
Programmers need one inspectable search model before plugin names hide path-planning fundamentals.
- Ignore today
Ignore proofs beyond this bounded grid and avoid treating A* output as a drive command.
- Unlocks next
A defensible planner/controller comparison with clear timing and acceptance boundaries.
Understand
Build the physical picture first
The planner chooses a useful road on the larger map; the controller repeatedly steers the real moving robot along that road using what it sees now.
A navigation goal is a pose, not just a point. It contains x and y position, orientation, frame, and timestamp or timing meaning. Nav2 commonly receives it through a long-running ROS 2 action such as NavigateToPose. The action can report feedback while work continues, accept cancellation, and return a structured result. Before planning, Nav2 needs an active lifecycle stack, a valid current pose, a goal that transforms into the global frame, and costmaps that are current.
The planner server asks a selected planner plugin to compute a path from the current pose to the goal using the global environmental representation. Different planners fit different robot shapes and motion constraints. A grid planner for a circular differential-drive base and a kinematically feasible planner for a car-like base solve different problems. A returned path is a proposal: inspect frame, collision validity, length, curvature or heading changes, unknown-space policy, and whether it starts near the real current pose.
A smoother may improve a rough path after planning, but it must preserve collision validity and the robot's motion limits. Smoothing is not the same as local control. The controller server runs repeatedly with the latest pose and local costmap, chooses a feasible command that follows the path, and reports progress. Nav2 calls these components controller plugins; some older material calls them local planners. Beneath Nav2, the base driver and motor controllers still convert body velocity into wheel behavior.
Goal and progress checkers define observable completion and lack of progress. A goal checker may accept position and orientation within declared tolerances rather than demand impossible exact equality. A progress checker can detect that the robot has not moved enough within a time allowance. These policies influence results and recovery, so record them with planner and controller parameters. A controller that stops near the target is not successful until the action reaches the intended terminal state and the final observed pose satisfies the rule.
Planner and controller rates express different work. Global planning may run when a goal arrives or periodically for replanning; control runs much faster because nearby conditions and tracking error change continuously. More frequency is not automatically better: each cycle needs fresh transforms and costmaps and must finish before its next deadline. Measure compute time, command age, path age, and missed cycles under the same simulation load used for evaluation.
Words you need
Name each idea precisely
- Goal pose
The requested final position and orientation together with its coordinate frame.
Physical example:A delivery robot must stop at x=4.2 m, y=1.8 m in
mapand face the shelf.- Global planner
A component that computes a collision-valid path from current pose to goal using a larger environmental model.
Physical example:It chooses the east aisle because a permanent wall blocks the direct line.
- Controller
A frequently running component that uses current state and nearby obstacles to produce feasible commands for following a path.
Physical example:It slows and steers around a trolley that entered the local costmap.
- Smoother
An optional component that improves path shape while maintaining required feasibility and collision checks.
Physical example:It rounds a stair-step grid path so the base does not command a sharp heading change at every cell.
- Goal checker
The rule that decides whether the final observed pose is close enough to the requested goal.
Physical example:A docking approach may require tighter angle tolerance than a hallway waypoint.
- Progress checker
A rule that detects whether the robot has moved enough within a configured time window.
Physical example:Wheel commands continue for ten seconds, but the base position changes by only one centimetre.
Math, one line at a time
Work through today’s relationship
Prerequisite rescue · optionalOccupancy probability and path cost
Navigation converts uncertain map cells into a collision-aware route.
- p(occupied)
- belief that a map cell contains an obstacleUnit: probability from 0 to 1
- g(n)
- cost already travelled to cell nUnit: cost or metres
- h(n)
- estimated remaining costUnit: same as g
For an A* node, suppose g = 4 m and admissible h = 3 m.
Total priority f = g + h = 7 m.
The planner compares f values, but the final path must also clear the inflated robot footprint.
It resembles shortest-path routing in a network, but each node represents physical space and the robot has width.
What is f when g = 2.5 m and h = 1.5 m?
f = 4.0 m.
Average speed is
Using straight-line distance over traveled path,
Build an honest time budget for one navigation goal
A planner returns a 7.2 m collision-valid path in 0.11 s. Smoothing takes 0.02 s. The configured average travel speed for this scenario is 0.40 m/s, and the controller runs at 20 Hz.
Compute the ideal travel time along the actual path: 7.2 m / 0.40 m/s = 18.0 s.
Add one-time pre-motion computation: 0.11 s + 0.02 s = 0.13 s.
Compute the controller period: 1 / 20 Hz = 0.05 s, or 50 ms between planned command updates.
Do not multiply 18 s by the controller rate; the 360 control cycles occur during travel rather than adding 360 separate travel times.
Declare additional observed categories separately: startup wait, turns and acceleration, obstacle delay, recovery, and final goal-settling time.
After the run, compare observed total with 18.13 s and attribute the difference using timestamps instead of calling all extra time 'Nav2 overhead'.
The ideal lower-bound estimate is 18.13 s before acceleration, turning, dynamic obstacles, goal settling, or recovery, while each controller cycle has a 50 ms timing budget.
A useful performance budget separates one-time planning, concurrent control cadence, physical travel, and exceptional delay.
Physical examples
Where this appears in real life
Cycling route and handlebars
A map application chooses streets for a trip, while the rider continuously turns the handlebars and brakes based on the current lane and traffic.
Route choice and moment-to-moment steering solve different time scales; a good street route does not guarantee safe control.
Tape path for a toy car
A curved tape line crosses a floor, and a child repeatedly adjusts a toy car to stay near it while someone moves a small box nearby.
The tape is the global path. Repeated steering uses live error and obstacles; picking a new tape route is replanning.
Hands-on exercise
Make the idea observable
Use the localized Gazebo robot and validated costmaps. Keep one world, start pose, goal pose, seed, and Nav2 parameter version fixed.
Send one NavigateToPose goal and record its exact pose, frame, planner ID, controller ID, start acceptance, feedback, terminal result, and final observed pose.
Display the raw global path, any smoothed path, local costmap, footprint, and executed odometry trail with distinct labels rather than identical colors.
Measure planner duration, path length, controller frequency, command age, total goal time, and final position and yaw error.
Swap only the planner plugin or its relevant configuration while holding the controller and scenario fixed; compare path feasibility, length, compute time, and shape.
Swap only the controller while holding the planner and path scenario fixed; compare tracking error, oscillation, minimum clearance, and completion.
Plant one wrong goal frame or impossible goal and verify the system returns a visible bounded failure rather than silently transforming or retrying forever.
The planner should own route generation, the controller should own repeated tracking commands, and the action trace should explain acceptance, progress, completion, cancellation, or failure.
A reviewer can trace one goal from request through path and commands to terminal result, compare one controlled plugin change, and reproduce one invalid-goal failure.
Build today
Launch one pinned Gazebo, SLAM Toolbox, AMCL, and Nav2 stack; survive cancellation, obstruction, stale localization, and bounded recovery; then freeze the world, seed, bag, map, graph, configuration, transform snapshot, scenarios, and raw results.
Evidence to save
DONE when a deterministic “Nav2 planners, controllers, smoothers, and goal execution” failure test reports expected versus actual behavior and passes after the documented fix.
Common mistakes
Catch the wrong mental model
Calling the global planner the component that continuously steers the wheels.
The planner produces a path; the controller repeatedly computes body commands from live state, and lower-level drivers actuate the wheels.
Comparing two planners while also changing controller, costmap, start pose, and goal.
Freeze the scenario and all other components so the planner change has an interpretable effect.
Declaring success because the base looks close to the marker in RViz.
Require the correct action terminal state plus measured final position and orientation under the declared goal-checker tolerance.
Increasing control frequency without checking compute deadline or data freshness.
Measure each cycle, transform and costmap age, command latency, and missed periods under representative load.
Job connection
How this becomes employable evidence
Expose planner, controller, path age, goal tolerances, and action state in an operator tool, then diagnose whether a slow AMR route comes from path choice, local tracking, physical speed limits, or recovery.
Relevant target roles
- Robotics Software Engineer — ROS 2 / AMR
- Robotics Application / ROS 2 Integration Engineer
- Robot HMI / Control & Monitoring Engineer
Chapter 08 interview drill
Interview questions: Nav2 planners, controllers, smoothers, and goal execution
Practise a 60–90 second answer: define the idea, connect it to a physical robot, state assumptions, frames, and units when relevant, then finish with the failure signal or evidence you would inspect.
Primary interview scenario
A global path looks valid, but the robot oscillates and never reports success near the goal. Separate planner, smoother, controller, progress checker, goal checker, and base-driver responsibilities and name the evidence you would collect.
Answer shape: clarify the situation → trace the physical and software path → test the most likely boundaries → name the evidence that would confirm the result.
Technical follow-up questions
Q1What does the global planner return?
A path or route proposal from current pose to goal, expressed in a frame and checked against the configured global representation.
Q2Why does the controller need the local costmap after a path exists?
It must compute feasible current commands while accounting for tracking error and obstacles that can change near the robot.
Q3What proves that a NavigateToPose goal completed?
The action reaches the intended terminal result and the observed final pose satisfies the declared goal-checking rule.