ROS 2 Navigation2

The Behavior Tree Engine is the central executive that orchestrates the navigation task flow. It replaces the finite-state machine used in the original ROS Navigation Stack with a more flexible and maintainable tree of actions and conditions.

• Nodes represent actions (e.g., ComputePathToPose), conditions (e.g., IsGoalReached), or control flow (e.g., Sequence, Fallback).
• Reactive Execution allows the tree to be ticked at a high frequency, enabling dynamic replanning and recovery behaviors based on sensor feedback.
• This architecture cleanly separates high-level task logic from the low-level server implementations, making the system more debuggable and extensible.

Navigation2 decomposes the navigation problem into three primary server types, each managed as a ROS 2 Lifecycle Node for controlled state transitions.

• Planner Server: Calculates a global path from the robot's current position to a goal. Plugins like Nav2 Smac Planner (using hybrid A* or State Lattice search) or Global Planner are loaded dynamically.
• Controller Server: Generares local velocity commands to follow the global path while avoiding dynamic obstacles. It uses plugins like DWB (Dynamic Window Approach) or MPC (Model Predictive Controller).
• Recovery Server: Executes a series of fallback behaviors (e.g., clear costmap, spin in place, back up) when the robot is stuck or receives invalid sensor data.

The Costmap is a central 2D or 3D occupancy grid representation used by both the planner and controller. It is built from multiple sensor layers for robustness.

• Static Layer: Loads a pre-existing map (e.g., from SLAM) as the persistent baseline.
• Obstacle Layer: Dynamically inserts and removes obstacles from real-time sensor data (e.g., LiDAR, depth cameras).
• Inflation Layer: Expands obstacle boundaries by a user-defined radius to create a cost gradient, ensuring the robot plans paths with a safe clearance.
• This layered approach allows different sensors and map sources to be fused into a single, coherent representation of traversable space.

BT Navigator Servers are specialized action servers that wrap the core Planner, Controller, and Recovery servers with a behavior tree interface. They translate high-level navigation requests into the specific sequence of BT node executions.

• NavigateToPose: The primary action for autonomous navigation to a goal pose (x, y, θ). Its internal tree handles global planning, controller activation, and recovery behaviors.
• NavigateThroughPoses: An action for following a sequence of intermediate waypoints.
• ComputePathToPose: A planning-only action that returns a path without executing it, useful for previewing or hybrid planning systems.
• These servers expose the navigation system's capabilities as standard ROS 2 actions, providing feedback and cancelation support.

All core servers in Navigation2 are implemented as ROS 2 Lifecycle Nodes. This provides a structured state machine for system initialization, error recovery, and shutdown.

• States: Unconfigured → Inactive → Active → (Error Processing) → Finalized.
• Controlled Activation: The system can be brought up in a configured but inactive state, allowing parameters to be set before any computation begins.
• System Resilience: If a critical component fails (e.g., a sensor driver crashes), the lifecycle manager can deactivate dependent nodes, attempt recovery, and reactivate them, preventing undefined behavior.

Navigation2 uses a pluginlib-based architecture for its core algorithms, allowing developers to swap or extend components without modifying the framework's core code.

• Planner Plugins: Implement the nav2_core::GlobalPlanner interface (e.g., SmacPlanner, Theta*).
• Controller Plugins: Implement the nav2_core::Controller interface (e.g., DWB, Regulated Pure Pursuit).
• Recovery Behavior Plugins: Implement the nav2_core::Recovery interface.
• This design promotes experimentation and customization, enabling the use of specialized algorithms for different robot kinematics (differential drive, Ackermann, holonomic) or environmental constraints.

A Behavior Tree is a hierarchical, modular control architecture used to structure the high-level decision-making logic of an autonomous agent. In Navigation2, a BT orchestrates the navigation pipeline, dynamically selecting and executing navigation behaviors (like ComputePathToPose, FollowPath, Recovery) based on task success, failure, or ongoing conditions. This replaces the older, more rigid finite-state machine approach with a more flexible and maintainable system for complex task sequencing.

A Costmap 2D is a grid-based, 2.5D representation of the environment where each cell holds an occupancy cost value. Navigation2 uses layered costmaps to fuse data from different sources:

• Static Layer: The pre-existing map (from SLAM).
• Obstacle Layer: Dynamic obstacles from sensors like LiDAR.
• Inflation Layer: Adds a gradient cost around obstacles to keep the robot at a safe distance. The planner uses this aggregated costmap to compute safe, low-cost paths.

Localization is the process of estimating a robot's pose (position and orientation) within a known map. Navigation2 primarily uses the Adaptive Monte Carlo Localization (AMCL) algorithm, a probabilistic particle filter. AMCL estimates the robot's pose by:

• Maintaining a set of pose hypotheses (particles).
• Weighing them against incoming sensor data (e.g., laser scans).
• Resampling to focus on the most likely areas. This provides the crucial pose estimate required for global planning and local control.

A Global Planner is an algorithm that computes a feasible, long-distance path from the robot's start pose to a goal pose, using the global costmap (which includes the static map). Its role is strategic, finding a topologically correct route. Navigation2 includes planners like:

• NavFn: Uses a potential field/Dijkstra algorithm.
• Smac Planners (Grid-based, State Lattice): More kinematically feasible options. The output is a coarse path for the local planner to follow.

A Local Planner (or Controller) is responsible for generating velocity commands (linear and angular) to follow the global path while avoiding immediate, dynamic obstacles visible in the local costmap. It operates on a shorter horizon and higher frequency than the global planner. Navigation2's default is the DWB (Dynamic Window Approach) Local Planner, which:

• Generates a sampling space of feasible velocities.
• Simulates trajectories for each.
• Scores them based on path alignment, obstacle clearance, and speed.
• Selects the optimal command to send to the robot's base.

Recovery Behaviors are fallback actions triggered by the Behavior Tree when the robot is stuck (e.g., the planner fails or the robot is blocked). They are designed to clear the local costmap and attempt to regain a valid state. Common recoveries in Navigation2 include:

• Clear Costmap Service: Clears obstacles from a specified costmap layer.
• Spin: Rotates in place to potentially find a new path.
• Back Up: Moves backward a short distance. These behaviors are executed in a sequence defined by the BT's recovery subtree.