Chapter 08 · Map, localize, navigate, and recover with Nav2
Today in the field story
One problem, then the next
The cart meets a blocked corridor, so express the Medicine-Cart Route Trial as a ticked behavior tree whose nodes return success, failure, or running. Trace the exact branch, target recoveries to observed causes, bound attempts across robot and fleet layers, and require lifecycle readiness before accepting work. A repeated clear-and-retry loop is not resilience.
- Why now
Goal execution needs visible control flow and bounded failure handling around planning and control.
- Ignore today
Ignore a large mission language; inspect one blocked-goal tree and its state.
- Unlocks next
A recovery path that cancellation and obstruction tests can challenge.
Understand
Build the physical picture first
A behavior tree is a visible rulebook that keeps asking, “Is this step done, still running, or failed, and which allowed branch comes next?”
Nav2 is a group of modular servers, not one giant navigate function. The Behavior Tree Navigator coordinates those servers for tasks such as NavigateToPose. A behavior-tree node returns one of three important statuses when ticked: Success, Failure, or Running. Control nodes decide how child statuses are combined. A Sequence normally advances when a child succeeds, while a Fallback tries another child when one fails. The exact tree and plugin definitions matter more than the labels drawn in a simplified diagram.
Planning and following are often action nodes inside the tree. Conditions can ask whether the goal changed, a path is valid, or some other requirement holds. Decorators can limit rate, repeat, retry, or transform a child's result. Blackboard entries carry typed data such as the goal and path between nodes. A behavior tree does not make a bad planner safe or repair stale transforms; it makes orchestration, branches, and retry policy explicit enough to inspect and test.
A recovery should target a diagnosed class of failure. Clearing a local costmap may help when a temporary bad observation remains, but it is a poor response to a real wall or wrong transform. Waiting may help a moving person pass. Spinning can gather lidar coverage when the space is clear enough. Backing up may leave a trapped pose only when rear clearance is verified. Replanning can use a changed world model. Each behavior needs preconditions, timeout, retry budget, observable outcome, and a safe final failure path.
Nav2 servers are managed lifecycle nodes. They move through states such as unconfigured, inactive, active, and finalized, with configure, activate, deactivate, cleanup, and shutdown transitions. Configuration can allocate resources and validate parameters without accepting active work. Activation starts the operational interfaces. The lifecycle manager brings related servers up and down in an intended order and uses bonds to notice loss of a managed server. A node process merely existing does not mean it is ready to receive goals.
Production application logic belongs above raw navigation primitives. A fleet mission may check battery, request a door, navigate through a zone, verify arrival, and report a durable mission result. Nav2 behavior trees can be customized or embedded in higher-level orchestration, but business retries and navigation recoveries should not multiply each other without a shared budget. One mission ID, one current owner, and correlated tree transitions keep the robot and backend from issuing duplicate work.
Words you need
Name each idea precisely
- Behavior tree
A tree of control and task nodes whose statuses decide which allowed work runs next.
Physical example:Plan a path; if planning succeeds, follow it; if a recoverable failure occurs, perform one checked recovery and try again.
- Tick
One request for a behavior-tree node to update and return Success, Failure, or Running.
Physical example:Following the path returns Running on many ticks until the goal is reached or an error occurs.
- Fallback
A control rule that tries a later child when an earlier child fails, according to the tree's semantics.
Physical example:If the first planning attempt fails, a bounded recovery branch may run before the tree returns failure.
- Blackboard
Shared typed data used by behavior-tree nodes, such as the current goal, path, or error code.
Physical example:ComputePathToPose writes a path that FollowPath later reads.
- Managed lifecycle
Explicit node states and transitions that separate construction, configuration, active operation, cleanup, and shutdown.
Physical example:A controller server loads its plugin while configuring but accepts control work only after activation.
- Recovery budget
A declared limit on how many recovery attempts or how much recovery time a goal may use.
Physical example:One costmap clear and one replan are allowed before the task fails and requests an operator.
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.
With two bounded attempts, one follow action and at most three recoveries per attempt,
major actions before the tree reaches terminal failure.
Trace one bounded navigation recovery
A tree computes a path, follows it, and allows one contextual recovery attempt. A temporary bad lidar mark makes FollowPath fail, but the real corridor is clear in the frozen simulation case.
Tick the planning branch: the planner returns Success and places a valid path on the blackboard.
Tick FollowPath repeatedly: it returns Running until the stale local obstacle causes a controller failure with a recorded error code.
Enter the recovery branch only because this error class and current sensor evidence allow it; increment the shared recovery count from 0 to 1.
Clear the affected local costmap region, wait for a fresh scan and transform, then verify that the obstacle source no longer reports the mark.
Recompute the path and follow it; if the goal checker passes, the subtree returns Success with one recovery recorded.
If the same fault happens again, the budget is exhausted, so the tree returns Failure and requests the defined operator or mission-level response instead of clearing forever.
The goal succeeds only after a targeted, observable, one-attempt recovery; a repeated fault becomes a bounded terminal failure.
Recovery is an engineered branch with evidence and a budget, not a loop that hides the original error.
Physical examples
Where this appears in real life
Blocked school route rulebook
A child follows written rules: walk to the gate; if construction blocks it, check the side gate once; if both are blocked, return and report failure.
The fallback is bounded and visible. “Keep trying something” is not an acceptable branch.
Workshop machine startup
A machine can be assembled, configured, and inspected while inactive before the enable key permits operation.
Process alive, configured, and operationally active are different states; startup order and failed checks matter.
Hands-on exercise
Make the idea observable
Use the localized Gazebo stack, Groot or readable behavior-tree XML, lifecycle commands, and structured Nav2 logs. Keep the world and goal fixed.
Open the actual behavior-tree XML used by NavigateToPose and mark the planner action, controller action, conditions, recovery nodes, retry controls, and blackboard path.
Launch Nav2 without autostart, inspect every managed node state, and prove a navigation goal is rejected or held until the required servers become active.
Activate through the lifecycle manager, run one nominal goal, and correlate tree node statuses with planner, controller, and action timestamps.
Inject one repeatable stale local obstacle or controller failure and trace the exact error into the chosen recovery branch and retry counter.
Change only the recovery budget to zero, rerun the same fault, and confirm the system now reaches the expected terminal failure without the recovery action.
Deactivate one managed server or end its process in simulation, observe bond/lifecycle response, and verify commands stop and the goal cannot be reported as normal success.
Tree status, action result, lifecycle state, recovery count, and command output should tell one consistent story from startup through failure or success.
A reviewer can point to the exact XML branch taken in nominal and injected-failure runs, explain the retry limit, and reproduce lifecycle gating of navigation work.
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 the integrated “Nav2 behavior trees, recovery, and lifecycle management” path is observable, cancelable, and leaves the prior baseline reproducible.
Common mistakes
Catch the wrong mental model
Reading a behavior tree once from top to bottom as if it were a simple script.
Trace ticks and returned statuses; Running nodes and control-node semantics determine what is revisited and when.
Clearing costmaps for every navigation failure.
Use the error and current evidence to select a targeted recovery; never erase a real obstacle merely to make a path appear.
Assuming a launched process is ready because it appears in the ROS graph.
Check managed lifecycle state and required bonds, interfaces, transforms, and costmaps before accepting a goal.
Allowing a fleet service and the on-robot tree to retry independently without one total budget.
Correlate attempts by mission and goal identity and enforce one bounded policy across orchestration layers.
Job connection
How this becomes employable evidence
Customize an AMR navigation tree so obstruction, localization loss, door timeout, and operator escalation have separate bounded responses, then correlate those transitions with one fleet mission ID.
Relevant target roles
- Robotics Software Engineer — ROS 2 / AMR
- Robotics Application / ROS 2 Integration Engineer
- Robotics Deployment, Integration & Validation Engineer
- Robot Fleet Backend / Platform Engineer
Chapter 08 interview drill
Interview questions: Nav2 behavior trees, recovery, and lifecycle management
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 robot repeatedly clears both costmaps and retries the same blocked route. Explain how behavior-tree statuses, error codes, lifecycle state, recovery preconditions, and a shared retry budget should change that design.
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 three statuses does a normal behavior-tree action communicate?
It returns Success when complete, Failure when it cannot meet the condition, and Running while work is still in progress.
Q2When is clearing a local costmap a reasonable recovery?
When evidence indicates a stale or false local observation and the system waits for fresh sensor and transform data before retrying.
Q3Why should an inactive Nav2 server not accept normal navigation work?
Lifecycle activation is the explicit boundary that says configuration succeeded and operational interfaces are ready; process existence alone is insufficient.