Autonomous Navigation Stack | Robotics Development Hub

Navigation 2 (Nav2) is the ROS 2 standard for autonomous mobile robot navigation. This guide provides a deep dive into Nav2 architecture: behavior trees, costmap layers, recovery behaviors, controller plugins, and advanced customization for dynamic environments. 1. Nav2 Architecture Overview 1.1 Core Components # Nav2 node graph # /bt_navigator - Behavior Tree executor (high-level logic) # /planner_server - Global planner (A*, Dijkstra, Smac, Theta*) # /controller_server - Local controller (DWB, TEB, MPPI, RPP) # /behavior_server - Recovery behaviors (spin, backup, wait) # /waypoint_follower - Waypoint following action # /global_costmap - Global costmap (static + inflation) # /local_costmap - Local costmap (rolling window) # /map_server - Map provider (static, costmap filters) # /lifecycle_manager - Manages lifecycle states 1.2 Lifecycle Management # All Nav2 nodes are lifecycle nodes # States: unconfigured → inactive → active → finalized # Bringup launch manages transitions # Manual CLI control ros2 lifecycle set /planner_server configure ros2 lifecycle set /planner_server activate 2. Behavior Trees (BT Navigator) 2.1 Default NavigateToPose BT 2.2 Custom Behavior Tree Nodes # Custom BT node (C++) #include class CheckBattery : public BT::ConditionNode { public: CheckBattery(const std::string& name, const BT::NodeConfiguration& config) : BT::ConditionNode(name, config) {} static BT::PortsList providedPorts() { return { BT::InputPort ("threshold", 20.0) }; } BT::NodeStatus tick() override { double threshold; getInput("threshold", threshold); double battery = getBatteryLevel(); // Your implementation return (battery > threshold) ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE; } }; // Register BT::RegisterRosNode ("CheckBattery"); 2.3 BT Navigator Configuration # bt_navigator params bt_navigator: ros__parameters: use_sim_time: true global_frame: map robot_base_frame: base_link odom_topic: /odom bt_loop_duration: 10 # ms default_server_timeout: 20 # seconds action_server_result_timeout: 900.0 default_bt_xml_filename: "navigate_to_pose_bt.xml" plugin_lib_names: - nav2_compute_path_to_pose_action_bt_node - nav2_follow_path_action_bt_node - nav2_back_up_action_bt_node - nav2_spin_action_bt_node - nav2_wait_action_bt_node - nav2_clear_costmap_service_bt_node 3. Costmaps 3.1 Global Costmap # global_costmap params global_costmap: ros__parameters: use_sim_time: true global_frame: map robot_base_frame: base_link update_frequency: 1.0 publish_frequency: 1.0 static_map: true rolling_window: false resolution: 0.05 width: 50.0 height: 50.0 origin_x: -25.0 origin_y: -25.0 plugins: - {name: static_layer, type: "nav2_costmap_2d::StaticLayer"} - {name: inflation_layer, type: "nav2_costmap_2d::InflationLayer"} - {name: voxel_layer, type: "nav2_costmap_2d::VoxelLayer"} - {name: range_layer, type: "nav2_costmap_2d::RangeSensorLayer"} static_layer: plugin: "nav2_costmap_2d::StaticLayer" map_topic: /map subscribe_to_updates: true lethal_cost_threshold: 100 inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" inflation_radius: 0.55 cost_scaling_factor: 3.0 voxel_layer: plugin: "nav2_costmap_2d::VoxelLayer" enabled: true origin_z: -0.1 z_resolution: 0.05 z_voxels: 10 max_obstacle_height: 2.0 mark_threshold: 0 observation_sources: "scan" scan: topic: /scan max_obstacle_height: 2.0 clearing: true marking: true data_type: "LaserScan" raytrace_max_range: 3.0 obstacle_max_range: 2.5 3.2 Local Costmap # local_costmap params local_costmap: ros__parameters: use_sim_time: true global_frame: odom robot_base_frame: base_link update_frequency: 5.0 publish_frequency: 2.0 static_map: false rolling_window: true width: 5.0 height: 5.0 resolution: 0.05 plugins: - {name: obstacle_layer, type: "nav2_costmap_2d::ObstacleLayer"} - {name: inflation_layer, type: "nav2_costmap_2d::InflationLayer"} - {name: range_layer, type: "nav2_costmap_2d::RangeSensorLayer"} obstacle_layer: plugin: "nav2_costmap_2d::ObstacleLayer" enabled: true observation_sources: "scan" scan: topic: /scan max_obstacle_height: 2.0 clearing: true marking: true data_type: "LaserScan" raytrace_max_range: 3.0 obstacle_max_range: 2.5 min_obstacle_height: 0.0 max_obstacle_height: 2.0 inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" inflation_radius: 0.55 cost_scaling_factor: 3.0 3.3 Costmap Filters (Nav2 Costmap Filters) filter_info: type: 0 # 0=keepout, 1=speed filter_mask_topic: "/costmap_filter_mask" base: 0 multiplier: 1.0 keepout_filter: type: 0 filter_info_topic: "/costmap_filter_info" enabled: true filter_mask_topic: "/keepout_filter_mask" base: 0 multiplier: 1.0 speed_filter: type: 1 # Speed filter filter_info_topic: "/speed_filter_info" enabled: true filter_mask_topic: "/speed_filter_mask" base: 0 multiplier: 0.5 # 50% speed in zone 3.4 Custom Costmap Layer # Custom costmap layer plugin #include #include namespace custom_layers { class DynamicObstacleLayer : public nav2_costmap_2d::Layer { public: void onInitialize() override { ros::NodeHandle nh("~/" + name_); current_ = true; sub_ = nh.subscribe("dynamic_obstacles", 1, &DynamicObstacleLayer::obstacleCallback, this); } void updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y, double* max_x, double* max_y) override { std::lock_guard lock(mutex_); for (const auto& obs : obstacles_) { *min_x = std::min(*min_x, obs.x - obs.radius); *min_y = std::min(*min_y, obs.y - obs.radius); *max_x = std::max(*max_x, obs.x + obs.radius); *max_y = std::max(*max_y, obs.y + obs.radius); } } void updateCosts(nav2_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) override { std::lock_guard lock(mutex_); for (const auto& obs : obstacles_) { int mx, my; if (master_grid.worldToMap(obs.x, obs.y, mx, my)) { int radius_cells = obs.radius / master_grid.getResolution(); for (int dx = -radius_cells; dx <= radius_cells; ++dx) { for (int dy = -radius_cells; dy <= radius_cells; ++dy) { if (dx*dx + dy*dy <= radius_cells*radius_cells) { master_grid.setCost(mx+dx, my+dy, nav2_costmap_2d::LETHAL_OBSTACLE); } } } } } } }; } #include PLUGINLIB_EXPORT_CLASS(custom_layers::DynamicObstacleLayer, nav2_costmap_2d::Layer) 4. Global Planners 4.1 Planner Server Configuration # planner_server params planner_server: ros__parameters: use_sim_time: true planner_plugins: ["GridBased", "SmacPlanner"] GridBased: plugin: "nav2_navfn_planner/NavfnPlanner" tolerance: 0.5 use_astar: true allow_unknown: true visualize: false SmacPlanner: plugin: "nav2_smac_planner/SmacPlannerHybrid" tolerance: 0.25 downsample_costmap: true downsampling_factor: 1 allow_unknown: true max_iterations: 1000000 max_on_approach_iterations: 1000 terminal_checking_interval: 10 motion_model_for_search: "DUBIN" analytic_expansion_ratio: 3.5 analytic_expansion_max_length: 3.0 analytic_expansion_max_cost: 1.2 cache_obstacle_heuristic: true allow_reversing: true 4.2 SMAC Planner Variants Planner Motion Model Use Case SmacPlanner2D 2D grid (4/8-connected) Differential drive, omnidirectional SmacPlannerHybrid Hybrid A* (Reeds-Shepp/Dubins) Ackermann, car-like SmacPlannerLattice State lattice High-dimension, custom motions 5. Local Controllers 5.1 Controller Server Configuration # controller_server params controller_server: ros__parameters: use_sim_time: true controller_plugins: ["FollowPath"] FollowPath: plugin: "nav2_dwb_controller/DWBController" debug_trajectory_details: true min_vel_x: 0.0 min_vel_y: 0.0 min_vel_theta: 0.0 max_vel_x: 0.5 max_vel_y: 0.0 max_vel_theta: 1.0 acc_lim_x: 2.5 acc_lim_y: 0.0 acc_lim_theta: 3.2 decel_lim_x: -2.5 decel_lim_y: 0.0 decel_lim_theta: -3.2 vx_samples: 20 vy_samples: 5 vtheta_samples: 20 sim_time: 1.7 sim_granularity: 0.025 xy_goal_tolerance: 0.25 yaw_goal_tolerance: 0.25 trans_stopped_vel: 0.025 short_circuit_trajectory_evaluation: true critics: - "ObstacleCritic" - "GoalAlignCritic" - "GoalDistCritic" - "PathAlignCritic" - "PathDistCritic" - "TwirlingCritic" - "VelocityDeadbandCritic" ObstacleCritic: cost_power: 1.0 repulsion_weight: 1.5 critical_cost: 254 consider_footprint: true GoalAlignCritic: scale: 1.0 cost_power: 1.0 5.2 Alternative Controllers TEB (Timed Elastic Band) TEBController: plugin: "nav2_teb_controller/TebController" trajectory: teb_autosize: true dt_ref: 0.3 dt_hysteresis: 0.1 global_plan_overwrite_orientation: true max_global_plan_lookahead_dist: 3.0 force_reinit_new_goal_dist: 1.0 force_reinit_new_goal_angular: 0.5 robot: max_vel_x: 0.5 max_vel_x_backwards: 0.2 max_vel_y: 0.0 max_vel_theta: 1.0 acc_lim_x: 2.5 acc_lim_theta: 3.2 min_turning_radius: 0.0 # 0 for diff drive footprint_model: type: "polygon" vertices: [[0.3, 0.2], [0.3, -0.2], [-0.3, -0.2], [-0.3, 0.2]] goal_tolerance: xy_goal_tolerance: 0.2 yaw_goal_tolerance: 0.1 obstacles: min_obstacle_dist: 0.5 inflation_dist: 1.0 include_costmap_obstacles: true costmap_obstacles_behind_robot_dist: 1.5 optim: weight_max_vel_x: 2.0 weight_max_vel_theta: 1.0 weight_acc_lim_x: 1.0 weight_acc_lim_theta: 1.0 weight_kinematics_nh: 1000.0 weight_kinematics_fwd_drive: 10.0 weight_obstacle: 50.0 weight_dynamic_obstacle: 10.0 weight_viapoint: 1.0 weight_adapt_factor: 2.0 MPPI (Model Predictive Path Integral) MPPIController: plugin: "nav2_mppi_controller/MPPIController" time_steps: 56 model_dt: 0.05 batch_size: 2000 vx_std: 0.2 vy_std: 0.0 vtheta_std: 0.4 vx_max: 0.5 vtheta_max: 1.0 iteration_count: 1 prune_distance: 1.5 transform_tolerance: 0.1 temperature: 0.3 gamma: 0.015 costs: - {name: "CriticCost", weight: 1.0, cost_power: 1.0} - {name: "GoalCost", weight: 1.0, cost_power: 1.0} - {name: "ObstacleCost", weight: 1.0, cost_power: 1.0} 6. Recovery Behaviors 6.1 Built-in Recoveries # behavior_server params behavior_server: ros__parameters: use_sim_time: true local_costmap_topic: local_costmap/costmap_raw global_costmap_topic: global_costmap/costmap_raw local_footprint_topic: local_costmap/published_footprint global_footprint_topic: global_costmap/published_footprint cycle_frequency: 10.0 behavior_plugins: ["spin", "backup", "wait", "clear_costmap"] spin: plugin: "nav2_behaviors/Spin" spin_dist: 1.57 spin_speed: 0.5 simulate_ahead_time: 2.0 max_rotational_vel: 1.0 min_rotational_vel: 0.4 rotational_acc_lim: 3.2 backup: plugin: "nav2_behaviors/BackUp" backup_dist: 0.15 backup_speed: 0.025 time_allowance: 10.0 wait: plugin: "nav2_behaviors/Wait" wait_duration: 5.0 clear_costmap: plugin: "nav2_behaviors/ClearCostmap" service_name: "global_costmap/clear_entirely_global_costmap" 6.2 Custom Recovery Behavior # Custom recovery (C++) #include class DockRecovery : public nav2_behaviors::BehaviorBase { public: DockRecovery() : nav2_behaviors::BehaviorBase("dock_recovery") {} void onConfigure() override { dock_client_ = action_client (node_, "dock_robot"); } void onActivate() override { auto goal = DockAction::Goal(); goal.dock_id = "home"; auto future = dock_client_->async_send_goal(goal); // Handle result... } void onDeactivate() override { dock_client_->async_cancel_all_goals(); } nav2_behaviors::BehaviorStatus onRun(const std::shared_ptr & context) override { // Check if docking succeeded if (dock_succeeded_) return Status::SUCCEEDED; if (dock_failed_) return Status::FAILED; return Status::RUNNING; } }; 7. Advanced: Multi-Robot Navigation 7.1 Namespaced Nav2 7.2 Collision Avoidance Between Robots # Costmap layer for other robots # Other robots appear as dynamic obstacles # Custom layer subscribes to /robot2/pose, /robot3/pose # Adds them as circular obstacles to local costmap # Or use Nav2's Multi-Robot collision checker (experimental) collision_checker: plugin: "nav2_collision_checker/CollisionChecker" robots: - {name: "robot2", radius: 0.4, topic: "/robot2/pose"} - {name: "robot3", radius: 0.4, topic: "/robot3/pose"} 8. Nav2 with Custom Messages 8.1 Custom Goal Checker # Goal checker plugin #include class PrecisionGoalChecker : public nav2_core::GoalChecker { public: void initialize(const rclcpp_lifecycle::LifecycleNode::SharedPtr parent, const std::string& plugin_name) override { node_ = parent; plugin_name_ = plugin_name; xy_tolerance_ = 0.05; // 5cm yaw_tolerance_ = 0.05; // ~3 deg } bool isGoalReached(const geometry_msgs::msg::Pose& query_pose, const geometry_msgs::msg::Pose& goal_pose, const nav2_msgs::msg::GoalInfo& goal_info) override { double dx = query_pose.position.x - goal_pose.position.x; double dy = query_pose.position.y - goal_pose.position.y double dist = std::hypot(dx, dy); double dyaw = angles::shortest_angular_distance( tf2::getYaw(query_pose.orientation), tf2::getYaw(goal_pose.orientation)); return (dist <= xy_tolerance_) && (std::abs(dya