Chapter 08 · Map, localize, navigate, and recover with Nav2
Today in the field story
One problem, then the next
An operator cancels the Medicine-Cart Route Trial while a trolley enters the local path. Correlate cancel request, server acceptance, final nonzero command, measured simulated stop, stopping distance, path version, collision-monitor state, and action result. Separate temporary obstruction, stale localization, infeasible control, and drive failure; replan only from fresh changed evidence and keep one total retry budget.
- Why now
A resilient robot must prove bounded stop and failure classification, not merely accept a cancel message.
- Ignore today
Ignore safety-certification claims for software Collision Monitor.
- Unlocks next
Hard command-integrity gates and realistic failure rows for evaluation.
Understand
Build the physical picture first
A cancel button is only a request sent into a moving system; completion means commands became quiet and observed motion reached the declared stopped condition.
NavigateToPose is an action, so a client may request cancellation while the goal is running. The server must accept or reject that request, halt the relevant tree work, stop producing navigation commands, and return a terminal cancellation result. The physical or simulated base has inertia and lower-level command timeouts, so action cancellation and motion stopped are different timestamps. Measure from cancel request to cancel acknowledgement, last nonzero command, and observed stopped velocity.
Obstruction detection needs more than “the goal is taking too long.” The local costmap may show a new obstacle, the controller may report no valid command, a progress checker may see insufficient displacement, localization may be stale, or wheels may command motion without odometry progress. These causes demand different responses. A stopped person in the aisle may justify waiting; a lost transform requires a state-estimation response; a blocked permanent corridor requires a new route or terminal failure.
Replanning means computing a path again from fresh current pose and environmental state. It is useful when the start moved, the goal changed, or obstacles changed the valid route. Replanning at a fixed rate can keep a route current, but excessive replanning consumes CPU and can make the chosen path flicker. Reusing stale costmaps or repeatedly producing the same invalid path is not recovery. Compare path identity, world-model version, pose age, and failure reason before spending another attempt.
Nav2 Collision Monitor can filter velocity commands below the navigation stack using sensor-defined stop, slowdown, limit, or approach regions. It should be the final velocity-processing link when configured for that role, and stale source data needs a declared blocking policy. This is an additional software collision-avoidance layer, not proof of a safety-rated system and not a replacement for a hardware emergency stop or required machinery-safety controls. Test its actual input and output topics so no parallel publisher bypasses it.
A bounded response policy joins these pieces. Declare what counts as progress, how long fresh data remains valid, when to wait, when to replan, how many attempts are allowed, what cancellation latency must pass, and which conditions force immediate failure or operator help. Preserve the original fault in logs; a successful retry does not erase the fact that the first attempt failed.
Words you need
Name each idea precisely
- Cancellation latency
Elapsed time from the cancel request to a declared cancellation or stopped-motion milestone.
Physical example:The client requests cancel at 8.00 s, the last nonzero command occurs at 8.12 s, and observed speed falls below the stop threshold at 8.28 s.
- Obstruction
A condition that prevents intended progress, supported by evidence such as occupied space or repeated infeasible control.
Physical example:A trolley spans the aisle and the local costmap leaves no footprint-valid passage.
- Replanning
Computing a new path using a fresh start pose, goal, and environmental representation.
Physical example:The global planner chooses the next aisle after a pallet blocks the original route.
- Collision Monitor
A Nav2 component that independently filters velocity commands from sensor-defined near-robot regions.
Physical example:Lidar points entering a front slowdown polygon scale the command before it reaches the base.
- Retry budget
A maximum number of attempts or amount of time permitted before a goal fails or escalates.
Physical example:The robot waits once and replans once; a still-blocked route then becomes an operator-visible failure.
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.
Reaction travel is
Adding measured braking distance gives
Measure cancel response and stopping distance
A simulated base moves at 0.50 m/s. A cancel request is sent at 12.000 s, accepted at 12.035 s, the last nonzero velocity command is stamped 12.110 s, and odometry falls below the declared 0.02 m/s stopped threshold at 12.310 s.
Compute cancel-acknowledgement latency: 12.035 - 12.000 = 0.035 s, or 35 ms.
Compute command-quiet latency: 12.110 - 12.000 = 0.110 s, or 110 ms.
Compute observed-stop latency: 12.310 - 12.000 = 0.310 s, or 310 ms.
If a measured 0.11 m of travel occurs after the request, report it as stopping distance rather than estimating it from the final timestamp alone.
Check the terminal action state is Canceled and no later publisher emits an uncorrelated nonzero command.
Judge every quantity against a predeclared simulation criterion; a fast acknowledgement cannot compensate for late commands or continued motion.
The software acknowledges in 35 ms, commands become quiet in 110 ms, observed motion stops in 310 ms, and the base travels 0.11 m after cancellation.
Cancellation quality is a timeline across client, action server, command chain, and observed body—not one callback duration.
Physical examples
Where this appears in real life
Stopping a rolling office chair
A person says “stop pushing” instantly, but the chair rolls a short distance before friction brings it below a chosen stopped-speed threshold.
Request time, command stop, and physical stop are separate; speed and surface determine stopping distance.
Road closed versus traffic light
A driver waits at a red light but chooses another road when construction permanently closes the street.
The same lack of progress can require wait, replan, or abort depending on the observed cause and retry policy.
Hands-on exercise
Make the idea observable
Use a fixed Gazebo route with action feedback, stamped velocity commands, odometry, local/global costmaps, Collision Monitor state, and one bag-recording launch.
Run a nominal goal, request cancellation at a fixed route marker, and calculate acknowledgement, command-quiet, observed-stop, and terminal-result times from one clock.
Place a temporary obstacle on the path, label the first detecting sensor and costmap update, and record whether the controller, progress checker, or tree reports the obstruction.
Remove the obstacle after a fixed wait and verify fresh clearing plus one replan succeeds without exceeding the shared recovery budget.
Repeat with a permanent blockage and verify the planner chooses a genuinely different valid route or reaches the declared terminal failure.
Configure Collision Monitor stop and slowdown regions in simulation, confirm the controller output passes through its input and output topics, and inject stale source data to test the fail policy.
Plant a rogue or mis-remapped velocity publisher, demonstrate how it bypasses the intended chain, then add an automated graph assertion that rejects the launch.
Action state, command stream, measured motion, sensor freshness, costmap change, path version, collision-monitor state, and retry count should remain correlated.
Nominal cancel, temporary obstruction, permanent obstruction, stale collision source, and command-chain bypass each produce the declared measurable outcome with no endless retry.
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 comparison table for “Cancellation, obstruction, collision monitoring, and replanning” contains the test condition, metric, result, and justified engineering decision.
Common mistakes
Catch the wrong mental model
Measuring only the time until the action server accepts cancellation.
Also measure the last nonzero command, observed stopped velocity, stopping distance, and correct terminal result.
Calling every no-progress event an obstacle and clearing costmaps.
Separate real occupancy, stale transforms, localization loss, controller infeasibility, drive failure, and temporary blockage using correlated evidence.
Treating Collision Monitor as a safety-certified emergency stop.
Use it as an additional software collision-avoidance layer and retain required independent hardware and system safety controls.
Replanning without proving that pose, costmap, goal, or constraints changed.
Record input versions and compare the new path; otherwise the retry may repeat the same failure with extra latency.
Job connection
How this becomes employable evidence
Build the operator-visible cancellation and obstruction tests for an AMR release, proving where commands stop, how far the base travels, which component replans, and when the system asks for help.
Relevant target roles
- Robotics Deployment, Integration & Validation Engineer
- Robotics Application / ROS 2 Integration Engineer
- Robotics Software Engineer — ROS 2 / AMR
- Robot HMI / Control & Monitoring Engineer
Chapter 08 interview drill
Interview questions: Cancellation, obstruction, collision monitoring, and replanning
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
The UI says a goal is canceled, but the robot rolls for another second. Describe the timestamps, topics, action states, command publishers, Collision Monitor path, odometry, and physical controls you would inspect.
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
Q1Why is cancel acknowledgement not the same as a stopped robot?
The action protocol may acknowledge before command generation, lower-level control, and physical motion have all reached their stopped conditions.
Q2When is replanning useful?
When fresh pose, goal, or environmental information can produce a different valid path and a retry budget permits another attempt.
Q3Where should Collision Monitor sit in its intended velocity chain?
As the final velocity-filtering component before the actual cmd_vel consumer, with topic ownership checked so no publisher bypasses it.