Nav2 Logo
latest
  • Getting Started
    • Installation
    • Running the Example
    • Navigating
  • Development Guides
    • Build and Install
      • Install
        • For Iron and Older
        • For Jazzy and Newer
      • Build
        • Released Distribution Binaries
        • Rolling Development Source
        • Docker Container Images
      • Generate Doxygen
      • Help
        • Build Troubleshooting Guide
    • Dev Containers
      • Dev Container Guide
        • Creating Dev Containers
        • Using Dev Containers
      • What, Why, How?
        • What is a Dev Container?
        • Why use a Dev Container?
        • How do Dev Containers work?
      • Prerequisites
      • Getting started
      • Security
    • Getting Involved
      • Getting Involved
      • Process
      • Licensing
      • Developer Certification of Origin (DCO)
  • Navigation Concepts
    • ROS 2
      • Action Server
      • Lifecycle Nodes and Bond
    • Behavior Trees
    • Navigation Servers
      • Planner, Controller, Smoother, Route, and Behavior Servers
      • Planners
      • Controllers
      • Behaviors
      • Smoothers
      • Route
      • Robot Footprints
      • Waypoint Following
    • State Estimation
      • Standards
      • Global Positioning: Localization and SLAM
      • Odometry
    • Environmental Representation
      • Costmaps and Layers
      • Costmap Filters
      • Other Forms
    • Nav2 Academic Overview
  • First-Time Robot Setup Guide
    • Table of Contents
      • Setting Up Transformations
        • Transforms Introduction
        • Static Transform Publisher Demo
        • Transforms in Navigation2
        • Conclusion
      • Setting Up The URDF
        • URDF and the Robot State Publisher
        • Setting Up the Environment
        • Writing the URDF
        • Build and Launch
        • Visualization using RVIZ
        • Adding Physical Properties
        • Conclusion
      • Setting Up The SDF - Gazebo
        • About SDF
        • Writing the SDF
        • Build and Launch
        • Conclusion
      • Setting Up Odometry - Gazebo
        • Odometry Introduction
        • Setting Up Odometry on your Robot
        • Simulating an Odometry System using Gazebo
        • Conclusion
      • Smoothing Odometry using Robot Localization
        • Configuring Robot Localization
        • Launch and Build Files
        • Build, Run and Verification
      • Setting Up Sensors - Gazebo
        • Sensor Introduction
        • Simulating Sensors using Gazebo
        • Conclusion
      • Mapping and Localization
        • Costmap 2D
        • Conclusion
      • Setting Up the Robot’s Footprint
        • Footprint Introduction
        • Configuring the Robot’s Footprint
        • Build, Run and Verification
        • Visualizing Footprint in RViz
        • Conclusion
      • Setting Up Navigation Plugins
        • Planner and Controller Servers
        • Selecting the Algorithm Plugins
        • Conclusion
      • Setting Up Transformations
        • Transforms Introduction
        • Static Transform Publisher Demo
        • Transforms in Navigation2
        • Conclusion
      • Setting Up The URDF
        • URDF and the Robot State Publisher
        • Setting Up the Environment
        • Writing the URDF
        • Build and Launch
        • Visualization using RVIZ
        • Adding Physical Properties
        • Conclusion
      • Setting Up Odometry - Gazebo Classic
        • Odometry Introduction
        • Setting Up Odometry on your Robot
        • Simulating an Odometry System using Gazebo
        • Conclusion
      • Smoothing Odometry using Robot Localization
        • Configuring Robot Localization
        • Launch and Build Files
        • Build, Run and Verification
      • Setting Up Sensors - Gazebo Classic
        • Sensor Introduction
        • Simulating Sensors using Gazebo Classic
        • Conclusion
      • Mapping and Localization
        • Costmap 2D
        • Conclusion
      • Setting Up the Robot’s Footprint
        • Footprint Introduction
        • Configuring the Robot’s Footprint
        • Build, Run and Verification
        • Visualizing Footprint in RViz
        • Conclusion
      • Setting Up Navigation Plugins
        • Planner and Controller Servers
        • Selecting the Algorithm Plugins
        • Conclusion
  • Robots Using
    • Research Robots
  • ROSCon Talks
    • Nav2 Developer Talks
    • Community’s Talks
  • General Tutorials
    • Navigating with a Physical Turtlebot 3
      • Overview
      • Requirements
      • Tutorial Steps
        • 0- Setup Your Environment Variables
        • 1- Launch Turtlebot 3
        • 2- Launch Nav2
        • 3- Launch RVIZ
        • 4- Initialize the Location of Turtlebot 3
        • 5- Send a Goal Pose
    • (SLAM) Navigating While Mapping
      • Overview
      • Requirements
      • Tutorial Steps
        • 0- Launch Robot Interfaces
        • 1- Launch Navigation2
        • 2- Launch SLAM
        • 3- Working with SLAM
        • 4- Getting Started Simplification
    • (STVL) Using an External Costmap Plugin
      • Overview
      • Costmap2D and STVL
      • Tutorial Steps
        • 0- Setup
        • 1- Install STVL
        • 1- Modify Navigation2 Parameter
        • 2- Launch Navigation2
        • 3- RVIZ
    • Navigating Using GPS Localization
      • Overview
      • Requirements
      • GPS Localization Overview
      • Tutorial Steps
        • 0- Setup Gazebo World
        • 1- Setup GPS Localization system
        • 2- Setup Navigation system
        • 3- Interactive GPS Waypoint Follower
        • 4- Logged GPS Waypoint Follower & Waypoint Logging
      • Conclusion
    • Groot Tutorials
      • Overview
      • Table of Contents
        • Groot - Interacting with Behavior Trees
        • Groot2 - Interacting with Behavior Trees
    • Using VIO to Augment Robot Odometry
      • Overview
      • Setting Up the ZED X Camera
      • Setting Up ZED ROS
      • Fusing VIO Into Local State Estimate
        • Fusing VSLAM Into Global State Estimate
      • Testing it Out!
    • Dynamic Object Following
      • Overview
      • Tutorial Steps
        • 0- Create the Behavior Tree
        • 1- Setup Rviz clicked point
        • 2- Run Dynamic Object Following in Nav2 Simulation
    • Navigating with Keepout Zones
      • Overview
      • Requirements
      • Tutorial Steps
        • 1. Prepare filter mask
        • 2. Configure Costmap Filter Info Publisher Server
        • 3. Enable Keepout Filter
        • 4. Run Nav2 stack
    • Navigating with Speed Limits
      • Overview
      • Requirements
      • Tutorial Steps
        • 1. Prepare filter mask
        • 2. Configure Costmap Filter Info Publisher Server
        • 3. Enable Speed Filter
        • 4. Run Nav2 stack
    • Using Docking Server
      • Overview
      • Requirements
      • ChargingDock Plugins
      • Dock Database
      • Configuring Docking Server
      • Adding Docking Server to Launch
      • Docking Action API
      • Putting It All Together
    • Using Rotation Shim Controller
      • Overview
      • What is the Rotation Shim Controller?
      • Configuring Rotation Shim Controller
      • Configuring Primary Controller
      • Demo Execution
    • Adding a Smoother to a BT
      • Overview
      • Requirements
      • Tutorial Steps
        • 0- Familiarization with the Smoother BT Node
        • 1- Specifying a Smoother Plugin
        • 2- Modifying your BT XML
    • Using Collision Monitor
      • Overview
      • Requirements
      • Configuring Collision Monitor
      • Configuring Collision Monitor with VelocityPolygon
      • Preparing Nav2 stack
      • Demo Execution
    • Adding a New Nav2 Task Server
      • Lifecycle Nodes
      • Composition
      • Error codes
      • Conclusion
    • Filtering of Noise-Induced Obstacles
      • Overview
      • Requirements
      • Tutorial Steps
        • 1. Enable Denoise Layer
        • 2. Run Nav2 stack
      • How it works
    • Camera Calibration
      • Overview
      • Requirements
      • Tutorial Steps
    • Get Backtrace in ROS 2 / Nav2
      • Overview
      • Preliminaries
      • From a Node
      • From a Launch File
      • From Large Project
      • From Nav2 Bringup
      • Automatic backtrace on crash
    • Profiling in ROS 2 / Nav2
      • Overview
      • Preliminaries
      • Profile from a Node
      • Profile from a Launch File
      • From Nav2 Bringup
      • Interpreting Results
    • Docker for Development: Zero to Hero
      • Overview
      • Preliminaries
      • Important Docker Commands
      • Exploring Your First Container
      • Understanding ROS Docker Images
      • For Docker-Based Development
        • Building a Development Image
        • Visualizations from Docker
      • For Docker-Based Deployment
      • Conclusion
      • Appendix
        • Nav2 Development Image
        • Nav2 Deployment Image
    • Route Server Tools
      • Provided Tools
        • Using the Nav2 Route Tool
        • Route Graph Generation LIF Editor
        • Route Graph Generation
      • Description
  • Plugin Tutorials
    • Writing a New Costmap2D Plugin
      • Overview
      • Requirements
      • Tutorial Steps
        • 1- Write a new Costmap2D plugin
        • 2- Export and make GradientLayer plugin
        • 3- Enable the plugin in Costmap2D
        • 4- Run GradientLayer plugin
    • Writing a New Planner Plugin
      • Overview
      • Requirements
      • Tutorial Steps
        • 1- Creating a new Planner Plugin
        • 2- Exporting the planner plugin
        • 3- Pass the plugin name through params file
        • 4- Run StraightLine plugin
    • Writing a New Controller Plugin
      • Overview
      • Requirements
      • Tutorial Steps
        • 1- Create a new Controller Plugin
        • 2- Exporting the controller plugin
        • 3- Pass the plugin name through the params file
        • 4- Run Pure Pursuit Controller plugin
    • Writing a New Behavior Tree Plugin
      • Overview
      • Requirements
      • Tutorial Steps
        • 1- Creating a new BT Plugin
        • 2- Exporting the planner plugin
        • 3- Add plugin library name to config
        • 4- Run Your Custom plugin
      • Using custom types for Input/Output ports
      • Visualize the content of the blackboard in Groot 2 (PRO)
    • Writing a New Behavior Plugin
      • Overview
      • Requirements
      • Tutorial Steps
        • 1- Creating a new Behavior Plugin
        • 2- Exporting the Behavior Plugin
        • 3- Pass the plugin name through params file
        • 4- Run Behavior Plugin
    • Writing a New Navigator Plugin
      • Overview
      • Requirements
      • Tutorial Steps
        • 1- Create a new Navigator Plugin
        • 2- Exporting the navigator plugin
        • 3- Pass the plugin name through the params file
        • 4- Run plugin
  • Configuration Guide
    • Behavior-Tree Navigator
      • Parameters
      • NavigateToPose Parameters
      • NavigateThroughPoses Parameters
      • Example
    • Behavior Tree XML Nodes
      • Action Plugins
        • Wait
        • Spin
        • BackUp
        • DriveOnHeading
        • AssistedTeleop
        • ComputePathToPose
        • ComputeRoute
        • ComputeAndTrackRoute
        • FollowPath
        • NavigateToPose
        • ClearEntireCostmap
        • ClearCostmapExceptRegion
        • ClearCostmapAroundRobot
        • ReinitializeGlobalLocalization
        • TruncatePath
        • TruncatePathLocal
        • PlannerSelector
        • ControllerSelector
        • SmootherSelector
        • GoalCheckerSelector
        • ProgressCheckerSelector
        • NavigateThroughPoses
        • ComputePathThroughPoses
        • ComputeCoveragePath
        • CancelCoverage
        • RemovePassedGoals
        • RemoveInCollisionGoals
        • CancelControl
        • CancelBackUp
        • CancelSpin
        • CancelWait
        • CancelDriveOnHeading
        • CancelAssistedTeleop
        • CancelComputeAndTrackRoute
        • SmoothPath
        • GetPoseFromPath
        • DockRobot
        • UndockRobot
        • ConcatenatePaths
        • GetCurrentPose
        • AppendGoalPoseToGoals
        • ExtractRouteNodesAsGoals
        • GetNextFewGoals
      • Condition Plugins
        • GoalReached
        • TransformAvailable
        • DistanceTraveled
        • GoalUpdated
        • GlobalUpdatedGoal
        • InitialPoseReceived
        • IsStuck
        • IsStopped
        • TimeExpired
        • IsBatteryLow
        • IsPathValid
        • PathExpiringTimer
        • AreErrorCodesPresent
        • WouldAControllerRecoveryHelp
        • WouldAPlannerRecoveryHelp
        • WouldASmootherRecoveryHelp
        • WouldARouteRecoveryHelp
        • IsBatteryCharging
        • ArePosesNear
      • Control Plugins
        • PipelineSequence
        • RoundRobin
        • RecoveryNode
      • Decorator Plugins
        • RateController
        • DistanceController
        • SpeedController
        • GoalUpdater
        • PathLongerOnApproach
        • SingleTrigger
        • GoalUpdatedController
      • Example
    • Costmap 2D
      • Costmap2D ROS Parameters
      • Default Plugins
      • Plugin Parameters
        • Static Layer Parameters
        • Inflation Layer Parameters
        • Obstacle Layer Parameters
        • Voxel Layer Parameters
        • Range Sensor Parameters
        • Denoise Layer Parameters
        • Plugin Container Layer Parameters
      • Costmap Filters Parameters
        • Keepout Filter Parameters
        • Speed Filter Parameters
        • Binary Filter Parameters
      • Example
    • Lifecycle Manager
      • Parameters
      • Example
    • Planner Server
      • Parameters
      • Default Plugins
      • Example
    • Coverage Server
      • Parameters
      • Example
    • NavFn Planner
      • Parameters
      • Example
    • Smac Planner
      • Provided Plugins
        • Smac 2D Planner
        • Smac Hybrid-A* Planner
        • Smac State Lattice Planner
      • Description
    • Theta Star Planner
      • Parameters
      • Example
    • Controller Server
      • Parameters
      • Provided Plugins
        • SimpleProgressChecker
        • PoseProgressChecker
        • SimpleGoalChecker
        • StoppedGoalChecker
      • Default Plugins
      • Example
    • DWB Controller
      • Controller
        • DWB Controller
        • XYTheta Iterator
        • Kinematic Parameters
        • Publisher
      • Plugins
        • LimitedAccelGenerator
        • StandardTrajectoryGenerator
      • Trajectory Critics
        • BaseObstacleCritic
        • GoalAlignCritic
        • GoalDistCritic
        • ObstacleFootprintCritic
        • OscillationCritic
        • PathAlignCritic
        • PathDistCritic
        • PreferForwardCritic
        • RotateToGoalCritic
        • TwirlingCritic
      • Example
    • Regulated Pure Pursuit
      • Regulated Pure Pursuit Parameters
      • Example
    • Model Predictive Path Integral Controller
      • MPPI Parameters
        • Trajectory Visualization
        • Path Handler
        • Ackermann Motion Model
        • Constraint Critic
        • Goal Angle Critic
        • Goal Critic
        • Obstacles Critic
        • Cost Critic
        • Path Align Critic
        • Path Angle Critic
        • Path Follow Critic
        • Prefer Forward Critic
        • Twirling Critic
        • Velocity Deadband Critic
      • Example
      • Notes to Users
        • General Words of Wisdom
        • Prediction Horizon, Costmap Sizing, and Offsets
        • Obstacle, Inflation Layer, and Path Following
    • Rotation Shim Controller
      • Rotation Shim Controller Parameters
      • Example
    • Graceful Controller
      • Graceful Controller Parameters
      • Example
    • Map Server / Saver
      • Map Saver Parameters
      • Map Server Parameters
      • Costmap Filter Info Server Parameters
      • Example
    • AMCL
      • Parameters
      • Example
    • Behavior Server
      • Behavior Server Parameters
      • Default Plugins
      • Spin Behavior Parameters
      • BackUp Behavior Parameters
      • DriveOnHeading Behavior Parameters
      • AssistedTeleop Behavior Parameters
      • Example
    • Smoother Server
      • Smoother Server Parameters
      • Example
    • Simple Smoother
      • Simple Smoother Parameters
      • Example
    • Savitzky-Golay Smoother
      • Savitzky-Golay Smoother Parameters
      • Example
    • Constrained smoother
      • Smoother Server Parameters
      • Example
    • Velocity Smoother
      • Velocity Smoother Parameters
      • Example
    • Collision Monitor
      • Provided Nodes
        • Collision Monitor Node
        • Collision Detector Node
    • Waypoint Follower
      • Parameters
      • Provided Plugins
        • WaitAtWaypoint
        • PhotoAtWaypoint
        • InputAtWaypoint
      • Default Plugin
      • Example
    • Loopback Simulator
      • Parameters
      • Example
    • Docking Server
      • Parameters
      • SimpleChargingDock Parameters
      • Example
    • Route Server
      • Server Parameters
      • Edge Scorer Parameters
        • CostmapScorer
        • DistanceScorer
        • TimeScorer
        • PenaltyScorer
        • SemanticScorer
        • StartPoseOrientationScorer
        • GoalPoseOrientationScorer
        • DynamicEdgesScorer
      • Route Operations Parameters
        • AdjustSpeedLimit
        • CollisionMonitor
        • TimeMarker
        • ReroutingService
        • TriggerEvent
      • Example
  • Tuning Guide
    • Inflation Potential Fields
    • Robot Footprint vs Radius
    • Rotate in Place Behavior
    • Planner Plugin Selection
    • Controller Plugin Selection
    • Caching Obstacle Heuristic in Smac Planners
    • Costmap2D Plugins
    • Nav2 Launch Options
    • Other Pages We’d Love To Offer
  • Nav2 Behavior Trees
    • Introduction To Nav2 Specific Nodes
      • Action Nodes
      • Condition Nodes
      • Decorator Nodes
      • Control: PipelineSequence
      • Control: Recovery
      • Control: RoundRobin
    • Detailed Behavior Tree Walkthrough
      • Overview
      • Prerequisites
      • Navigate To Pose With Replanning and Recovery
      • Navigation Subtree
      • Recovery Subtree
    • Navigate To Pose
    • Navigate Through Poses
    • Navigate To Pose and Pause Near Goal-Obstacle
    • Navigate To Pose With Consistent Replanning And If Path Becomes Invalid
    • Navigate on Route Graph with Recovery
    • Follow Dynamic Point
    • Odometry Calibration
  • Navigation Plugins
    • Behavior-Tree Navigators
    • Costmap Layers
    • Costmap Filters
    • Controllers
    • Planners
    • Smoothers
    • Behaviors
    • Waypoint Task Executors
    • Goal Checkers
    • Progress Checkers
    • Behavior Tree Nodes
    • Route Plugins
      • Edge Scorers
      • Route Operations
      • Graph File Parsers
  • Migration Guides
    • Dashing to Eloquent
      • New Packages
      • New Plugins
      • Navigation2 Architectural Changes
    • Eloquent to Foxy
      • General
      • Server Updates
      • New Plugins
      • Map Server Re-Work
      • New Particle Filter Messages
      • Selection of Behavior Tree in each navigation action
      • FollowPoint Capability
      • New Costmap Layer
    • Foxy to Galactic
      • NavigateToPose Action Feedback updates
      • NavigateToPose BT-node Interface Changes
      • NavigateThroughPoses and ComputePathThroughPoses Actions Added
      • ComputePathToPose BT-node Interface Changes
      • ComputePathToPose Action Interface Changes
      • BackUp BT-node Interface Changes
      • BackUp Recovery Interface Changes
      • Nav2 Controllers and Goal Checker Plugin Interface Changes
      • FollowPath goal_checker_id attribute
      • Groot Support
      • New Plugins
      • Costmap Filters
      • SmacPlanner
      • ThetaStarPlanner
      • RegulatedPurePursuitController
      • Costmap2D current_ Usage
      • Standard time units in parameters
      • Ray Tracing Parameters
      • Obstacle Marking Parameters
      • Recovery Action Changes
      • Default Behavior Tree Changes
      • NavFn Planner Parameters
      • New ClearCostmapExceptRegion and ClearCostmapAroundRobot BT-nodes
      • New Behavior Tree Nodes
      • sensor_msgs/PointCloud to sensor_msgs/PointCloud2 Change
      • ControllerServer New Parameter failure_tolerance
      • Removed BT XML Launch Configurations
      • Nav2 RViz Panel Action Feedback Information
    • Galactic to Humble
      • Major improvements to Smac Planners
      • Simple (Python) Commander
      • Reduce Nodes and Executors
      • API Change for nav2_core
      • Extending the BtServiceNode to process Service-Results
      • Including new Rotation Shim Controller Plugin
      • Spawning the robot in Gazebo
      • Recovery Behavior Timeout
      • New parameter use_final_approach_orientation for the 3 2D planners
      • SmacPlanner2D and Theta*: fix goal orientation being ignored
      • SmacPlanner2D, NavFn and Theta*: fix small path corner cases
      • Change and fix behavior of dynamic parameter change detection
      • Dynamic Parameters
      • BT Action Nodes Exception Changes
      • BT Navigator Groot Multiple Navigators
      • Removed Kinematic Limiting in RPP
      • Added Smoother Task Server
      • Removed Use Approach Velocity Scaling Param in RPP
      • Refactored AMCL motion models as plugins
      • Dropping Support for Live Groot Monitoring of Nav2
      • Replanning Only if Path is Invalid
      • Fix CostmapLayer clearArea invert param logic
      • Dynamic Composition
      • BT Cancel Node
      • BT PathLongerOnApproach Node
      • BT TruncatePathLocal Node
      • Constrained Smoother
      • Replanning at a Constant Rate and if the Path is Invalid
      • Euclidean Distance 2D
      • Recovery To Behavior
      • Respawn Support in Launch and Lifecycle Manager
      • New Nav2 Velocity Smoother
      • Goal Checker API Changed
      • Added Assisted Teleop
    • Humble to Iron
      • New Behavior-Tree Navigator Plugins
      • Added Collision Monitor
      • Removed use_sim_time from yaml
      • Run-time Speed up of Smac Planner
      • Recursive Refinement of Smac and Simple Smoothers
      • Simple Commander Python API
      • Smac Planner Start Pose Included in Path
      • Parameterizable Collision Checking in RPP
      • Expanded Planner Benchmark Tests
      • Smac Planner Path Tolerances
      • costmap_2d_node default constructor
      • Feedback for Navigation Failures
      • Costmap Filters
      • Savitzky-Golay Smoother
      • Changes to Map yaml file path for map_server node in Launch
      • SmootherSelector BT Node
      • Publish Costmap Layers
      • Give Behavior Server Access to Both Costmaps
      • New Model Predictive Path Integral Controller
      • Behavior Tree Uses Error Codes
      • Load, Save and Loop Waypoints from the Nav2 Panel in RViz
      • DWB Forward vs Reverse Pruning
      • More stable regulation on curves for long lookahead distances
      • Publish Collision Monitor State
      • Renamed ROS-parameter in Collision Monitor
      • New safety behavior model “limit” in Collision Monitor
      • Velocity smoother applies deceleration when timeout
      • PoseProgressChecker plugin
      • Allow multiple goal checkers and change parameter progress_checker_plugin(s) name and type
      • IsBatteryChargingCondition BT Node
      • Behavior Server Error Codes
      • New Denoise Costmap Layer Plugin
      • SmacPlannerHybrid viz_expansions parameter
    • Iron to Jazzy
      • BehaviorTree.CPP upgraded to version 4.5+
      • Added TwistStamped Option for Commands
      • Add VelocityPolygon in Collision Monitor
      • Change polygon points parameter format in Collision Monitor
      • Introduction of Soft-Real Time Action Servers
      • opennav_coverage Project
      • opennav_docking Project
      • Introduce a new Multi-Robot Bringup Launch
      • New option for the Voxel and Obstacle Layers
      • use_interpolation RPP Parameter Depreciated
      • Changes to MPPI Goal Critic
      • Changes to MPPI Path Angle Critic
      • Changes to MPPI Path Handling For Directionality
      • Addition of new MPPI Cost Critic
      • MPPI Acceleration
      • Move Error Code Enumerations
      • Substitution in parameter file
      • Allow Behavior Server Plugins to Access The Action Result
      • Smac Planner Debug Param Name Change
      • Smac Planner On Approach to Goal Shortcutting Solutions
      • Added GPS Waypoint Follower Server
      • Smac Planner Hybrid-A* New Features
      • New node in nav2_collision_monitor: Collision Detector
      • Dynamic enabling/disabling of sources/polygons in Collision Monitor/Detector
      • Expose action server’s result timeout
      • RewrittenYaml could add new parameters to YAMLs
      • Simple Commander API Allows Multi-Robot Namespacing
      • Change duration type in wait_action node
      • The costmap activation fails when required transforms are not available
      • Subtrees Obtain Shared Resources
      • Collision Monitor: added watchdog mechanism based on source_timeout parameter with default blocking behavior
      • BtActionServer: use native library haltTree()
      • Global Frame Removed from 2 BT Nodes
      • Introduction of CostmapUpdate.msg
      • Full Stack Uses Node Clocks
      • New Graceful Motion Controller
      • Plugin Libraries in BT Navigator Only Includes Custom Nodes
      • New RViz Plugin for selecting Planners, Controllers, Goal Checkers, Progress Checkers and Smoothers
      • RPP new optional interpolate_curvature_after_goal behavior and fix conflict between use_rotate_to_heading and allow_reversing
      • Cancel Checker Interface For GlobalPlanner
      • New BtActionServer/BtNavigator parameter
      • New collision monitor parameter
      • New graceful cancellation API for Controllers
      • Standardization of Plugin Naming with Double Colons (::)
      • Collision monitor: dynamic radius for circle type polygons
      • Static Layer: new parameter footprint_clearing_enabled
      • Lifecycle Node: added bond_heartbeat_period parameter (and allow disabling the bond mechanism)
      • Rotation Shim Controller: new parameter rotate_to_goal_heading
      • MPPI Controller: Addition of acceleration constraints
      • RegulatedPurePursuit Controller [RPP]: new parameter use_cancel_deceleration
    • Jazzy to Kilted
      • Nav2 Route Server
      • BehaviorTree error_msg
      • TwistStamped Default CmdVel Change
      • New Nav2 Loopback Simulator
      • Docking with Static Infrastructure or Dynamic Docking
      • New RViz panel for Docking
      • BT Nodes Changes
      • New RViz Tool for Costmap Cost Cell Inspection
      • Fix flickering visualization
      • Option to limit velocity through DWB trajectory
      • Option to disable zero velocity publishing on goal exit
      • Rotation Shim Disengagement Threshold
      • Added optional collision checking for the Docking Server
      • Revamped multirobot bringup and config files to use namespaces
      • Removed global map_topic from Costmap node
      • Simplified Costmap2DROS constructors
      • Option to disable collision checking in DriveOnHeading, BackUp and Spin Actions
      • New Plugin Container Layer
      • Iterative Target Selection for the Graceful Controller
      • Conform to ROS 2 launch syntax in Turtlebot 3 multi-robot launch file
      • ComputePathThroughPoses, NavigateThroughPoses and other BT nodes now use nav_msgs/Goals instead of vector<PoseStamped>
      • MPPI controller re-implemented using Eigen library and performance improved by 40-45%
      • Enable goal orientation non-specificity
      • DriveOnHeading and BackUp behaviors: Addition of acceleration constraints
      • Rotation Shim Deceleration as a function of its target orientation
      • Rotation Shim Open-loop Control
      • Near collision cost in MPPI cost critic
      • Service introspection
      • Rotation Shim Using Path Orientations
      • MPPI - Publishing Optimal Trajectory
      • NavigateThroughPoses - Reporting waypoint statuses information
      • Groot 2 Support
      • Docking backwards as plugin parameter
  • Simple Commander API
    • Overview
    • Commander API
    • Costmap API
    • Footprint Collision Checker API
    • Examples and Demos
  • Roadmaps
    • Jazzy Roadmap
    • Iron Roadmap
    • Humble Roadmap
  • About and Contact
    • Related Projects
    • About
    • Contact
Nav2
Edit
  • Navigation Concepts

Navigation Concepts

This page is to help familiarize new roboticists to the concepts of mobile robot navigation, in particular, with the concepts required to appreciating and working with this project.

ROS 2

ROS 2 is the core middleware used for Nav2. If you are unfamiliar with this, please visit the ROS 2 documentation before continuing.

Action Server

Just as in ROS, action servers are a common way to control long running tasks like navigation. This stack makes more extensive use of actions, and in some cases, without an easy topic interface. It is more important to understand action servers as a developer in ROS 2. Some simple CLI examples can be found in the ROS 2 documentation.

Action servers are similar to a canonical service server. A client will request some task to be completed, except, this task may take a long time. An example would be moving the shovel up from a bulldozer or ask a robot to travel 10 meters to the right.

In this situation, action servers and clients allow us to call a long-running task in another process or thread and return a future to its result. It is permissible at this point to block until the action is complete, however, you may want to occasionally check if the action is complete and continue to process work in the client thread. Since it is long-running, action servers will also provide feedback to their clients. This feedback can be anything and is defined in the ROS .action along with the request and result types. In the bulldozer example, a request may be an angle, a feedback may be the angle remaining to be moved, and the result is a success or fail boolean with the end angle. In the navigation example, a request may be a position, a feedback may be the time its been navigating for and the distance to the goal, and the result a boolean for success.

Feedback and results can be gathered synchronously by registering callbacks with the action client. They may also be gathered by asynchronously requesting information from the shared future objects. Both require spinning the client node to process callback groups.

Action servers are used in this stack to communicate with the highest level Behavior Tree (BT) navigator through a NavigateToPose action message. They are also used for the BT navigator to communicate with the subsequent smaller action servers to compute plans, control efforts, and recoveries. Each will have their own unique .action type in nav2_msgs for interacting with the servers.

Lifecycle Nodes and Bond

Lifecycle (or Managed, more correctly) nodes are unique to ROS 2. More information can be found here. They are nodes that contain state machine transitions for bringup and teardown of ROS 2 servers. This helps in deterministic behavior of ROS systems in startup and shutdown. It also helps users structure their programs in reasonable ways for commercial uses and debugging.

When a node is started, it is in the unconfigured state, only processing the node’s constructor which should not contain any ROS networking setup or parameter reading. By the launch system, or the supplied lifecycle manager, the nodes need to be transitioned to inactive by configuring. After, it is possible to activate the node by transitioning through the activating stage.

This state will allow the node to process information and be fully setup to run. The configuration stage, triggering the on_configure() method, will setup all parameters, ROS networking interfaces, and for safety systems, all dynamically allocated memory. The activation stage, triggering the on_activate() method, will active the ROS networking interfaces and set any states in the program to start processing information.

To shutdown, we transition into deactivating, cleaning up, shutting down and end in the finalized state. The networking interfaces are deactivated and stop processing, deallocate memory, exit cleanly, in those stages, respectively.

The lifecycle node framework is used extensively through out this project and all servers utilize it. It is best convention for all ROS systems to use lifecycle nodes if it is possible.

Within Nav2, we use a wrapper of LifecycleNodes, nav2_util LifecycleNode. This wrapper wraps much of the complexities of LifecycleNodes for typical applications. It also includes a bond connection for the lifecycle manager to ensure that after a server transitions up, it also remains active. If a server crashes, it lets the lifecycle manager know and transition down the system to prevent a critical failure. See Eloquent to Foxy for details.


Behavior Trees

Behavior trees (BT) are becoming increasingly common in complex robotics tasks. They are a tree structure of tasks to be completed. It creates a more scalable and human-understandable framework for defining multi-step or many state applications. This is opposed to a finite state machine (FSM) which may have dozens of states and hundreds of transitions. An example would be a soccer-playing robot. Embedding the logic of soccer game play into a FSM would be challenging and error prone with many possible states and rules. Additionally, modeling choices like to shoot at the goal from the left, right, or center, is particularly unclear. With a BT, basic primitives, like “kick”, “walk”, “go to ball”, can be created and reused for many behaviors. More information can be found in this book. I strongly recommend reading chapters 1-3 to get a good understanding of the nomenclature and workflow. It should only take about 30 minutes.

Behavior Trees provide a formal structure for navigation logic which can be both used to create complex systems but also be verifiable and validated as provenly correct using advanced tools. Having the application logic centralized in the behavior tree and with independent task servers (which only communicate data over the tree) allows for formal analysis.

For this project, we use BehaviorTree CPP V4 as the behavior tree library. We create node plugins which can be constructed into a tree, inside the BT Navigator. The node plugins are loaded into the BT and when the XML file of the tree is parsed, the registered names are associated. At this point, we can march through the behavior tree to navigate.

One reason this library is used is its ability to load subtrees. This means that the Nav2 behavior tree can be loaded into another higher-level BT to use this project as node plugin. An example would be in soccer play, using the Nav2 behavior tree as the “go to ball” node with a ball detection as part of a larger task. Additionally, we supply a NavigateToPoseAction plugin (among others) for BT so the Nav2 stack can be called from a client application through the usual action interface.

Other systems could be used to design complex autonomous behavior, namely Hierarchical FSMs (HFSM). Behavior Trees were selected due to popularity across the robotics and related industries and by largely user demand. However, due to the independent task server nature of Nav2, it is not difficult to offer a nav2_hfsm_navigator package in the future, pending interest and contribution.


Navigation Servers

Planners and controllers are at the heart of a navigation task. Recoveries are used to get the robot out of a bad situation or attempt to deal with various forms of issues to make the system fault-tolerant. Smoothers can be used for additional quality improvements of the planned path. In this section, the general concepts around them and their uses in this project are analyzed.

Planner, Controller, Smoother, Route, and Behavior Servers

Four of the action servers in this project are the planner, behavior, smoother, route, and controller servers.

These action servers are used to host a map of algorithm plugins to complete various tasks. They also host the environmental representation used by the algorithm plugins to compute their outputs.

The planner, smoother and controller servers will be configured at runtime with the names (aliases) and types of algorithms to use. These types are the pluginlib names that have been registered and the names are the aliases for the task. An example would be the DWB controller used with name FollowPath, as it follows a reference path. In this case, then all parameters for DWB would be placed in that namespace, e.g. FollowPath.<param>.

These three servers then expose an action interface corresponding to their task. When the behavior tree ticks the corresponding BT node, it will call the action server to process its task. The action server callback inside the server will call the chosen algorithm by its name (e.g. FollowPath) that maps to a specific algorithm. This allows a user to abstract the algorithm used in the behavior tree to classes of algorithms. For instance, you can have N plugin controllers to follow paths, dock with charger, avoid dynamic obstacles, or interface with a tool. Having all of these plugins in the same server allows the user to make use of a single environmental representation object, which is costly to duplicate.

For the behavior server, each of the behaviors also contains their own name, however, each plugin will also expose its own special action server. This is done because of the wide variety of behavior actions that may be created which cannot have a single simple interface to share. The behavior server also contains a costmap subscriber to the local costmap, receiving real-time updates from the controller server, to compute its tasks. We do this to avoid having multiple instances of the local costmap which are computationally expensive to duplicate.

The route server does not contain multiple “routing algorithms” like the planner or controller servers. Instead, it computes a route using a navigation graph using a set of plugins for scoring edges in the graph, parsing graph files, and performing operations along the route, if necessary. Rather than freespace planning, this computes a route using a graph that can be generated to represent lanes, areas the robot is allowed to navigate, a teach-and-repeat route, urban roadways, and more.

Alternatively, since the BT nodes are trivial plugins calling an action, new BT nodes can be created to call other action servers with other action types. It is advisable to use the provided servers if possible at all times. If, due to the plugin or action interfaces, a new server is needed, that can be sustained with the framework. The new server should use the new type and plugin interface, similar to the provided servers. A new BT node plugin will need to be created to call the new action server – however no forking or modification is required in the Nav2 repo itself by making extensive use of servers and plugins.

If you find that you require a new interface to the pluginlib definition or action type, please file a ticket and see if we can rectify that in the same interfaces.

Planners

The task of a planner is to compute a path to complete some objective function. The path can also be known as a route, depending on the nomenclature and algorithm selected. Two canonical examples are computing a plan to a goal (e.g. from current position to a goal) or complete coverage (e.g. plan to cover all free space). The planner will have access to a global environmental representation and sensor data buffered into it. Planners can be written to:

  • Compute shortest path

  • Compute complete coverage path

  • Compute paths along sparse or predefined routes

The general task in Nav2 for the planner is to compute a valid, and potentially optimal, path from the current pose to a goal pose. However, many classes of plans and routes exist which are supported.

Controllers

Controllers, also known as local planners in ROS 1, are the way we follow the globally computed path or complete a local task. The controller will have access to a local environment representation to attempt to compute feasible control efforts for the base to follow. Many controller will project the robot forward in space and compute a locally feasible path at each update iteration. Controllers can be written to:

  • Follow a path

  • Dock with a charging station using detectors in the odometric frame

  • Board an elevator

  • Interface with a tool

The general task in Nav2 for a controller is to compute a valid control effort to follow the global plan. However, many classes of controllers and local planners exist. It is the goal of this project that all controller algorithms can be plugins in this server for common research and industrial tasks.

Behaviors

Recovery behaviors are a mainstay of fault-tolerant systems. The goal of recoveries are to deal with unknown or failure conditions of the system and autonomously handle them. Examples may include faults in the perception system resulting in the environmental representation being full of fake obstacles. The clear costmap recovery would then be triggered to allow the robot to move.

Another example would be if the robot was stuck due to dynamic obstacles or poor control. Backing up or spinning in place, if permissible, allow the robot to move from a poor location into free space it may navigate successfully.

Finally, in the case of a total failure, a recovery may be implemented to call an operator’s attention for help. This can be done via email, SMS, Slack, Matrix, etc.

It is important to note that the behavior server can hold any behavior to share access to expensive resources like costmaps or TF buffers, not just recovery behaviors. Each may have its own API.

Smoothers

As criteria for optimality of the path searched by a planner are usually reduced compared to reality, additional path refinement is often beneficial. Smoothers have been introduced for this purpose, typically responsible for reducing path raggedness and smoothing abrupt rotations, but also for increasing distance from obstacles and high-cost areas as the smoothers have access to a global environmental representation.

Use of a separate smoother over one that is included as part of a planner is advantageous when combining different planners with different smoothers or when a specific control over smoothing is required, e.g. smoothing only a specific part of the path.

The general task in Nav2 for a smoother is to receive a path and return its improved version. However, for different input paths, criteria of the improvements and methods of acquiring them exist, creating space for a multitude of smoothers that can be registered in this server.

Route

The route server is a specialized planner that computes a route using a navigation graph, rather than the freespace costmap. The route is computed as the optimal way from the start to the goal through the set of nodes and directional edges in the pre-defined navigation graph. This navigation graph can be generated to represent lanes, areas the robot is allowed to navigate, a teach-and-repeat route, urban roadways, and more.

Robot Footprints

It is worth remarking that in the cost maps, we set a robot’s footprint either as a circle of radius robot_radius or as a vector of points footprint representing an arbitrary polygon if the robot is non-circular. This can also be adjusted over time using the costmap’s ~/footprint topic, which will update the polygon over time as needed due to changes in the robot’s state, such as movement of an attached manipulator, picking up a pallet, or other actions that adjust a robot’s shape. That polygon will then automatically be used by the planners and controllers.

Waypoint Following

Waypoint following is a basic feature of a navigation system. It tells our system how to use navigation to get to multiple destinations.

The nav2_waypoint_follower contains a waypoint following program with a plugin interface for specific task executors. This is useful if you need to go to a given location and complete a specific task like take a picture, pick up a box, or wait for user input. It is a nice demo application for how to use Nav2 in a sample application.

However, it could be used for more than just a sample application. There are 2 schools of thoughts for fleet managers / dispatchers:

  • Dumb robot; smart centralized dispatcher

  • Smart robot; dumb centralized dispatcher

In the first, the nav2_waypoint_follower is fully sufficient to create a production-grade on-robot solution. Since the autonomy system / dispatcher is taking into account things like the robot’s pose, battery level, current task, and more when assigning tasks, the application on the robot just needs to worry about the task at hand and not the other complexities of the system to complete the requested task. In this situation, you should think of a request to the waypoint follower as 1 unit of work (e.g. 1 pick in a warehouse, 1 security patrole loop, 1 aisle, etc) to do a task and then return to the dispatcher for the next task or request to recharge. In this school of thought, the waypoint following application is just one step above navigation and below the system autonomy application.

In the second, the nav2_waypoint_follower is a nice sample application / proof of concept, but you really need your waypoint following / autonomy system on the robot to carry more weight in making a robust solution. In this case, you should use the nav2_behavior_tree package to create a custom application-level behavior tree using navigation to complete the task. This can include subtrees like checking for the charge status mid-task for returning to dock or handling more than 1 unit of work in a more complex task. Soon, there will be a nav2_bt_waypoint_follower (name subject to adjustment) that will allow you to create this application more easily. In this school of thought, the waypoint following application is more closely tied to the system autonomy, or in many cases, is the system autonomy.

Neither is better than the other, it highly depends on the tasks your robot(s) are completing, in what type of environment, and with what cloud resources available. Often this distinction is very clear for a given business case.

nav2_waypoint_follower also supports GPS waypoint following when global localization is provided by robot_localization using the navsat_transform node - but also may be provided by Fuse or any number of other sources. There is an action server named /follow_gps_waypoints within nav2_waypoint_follower that can directly take in goals expressed in GPS coordinates, convert them to cartesian goals in the global frame, and execute them as cartesian waypoints.


State Estimation

Within the navigation project, there are 2 major transformations that need to be provided, according to community standards. The map to odom transform is provided by a positioning system (localization, mapping, SLAM) and odom to base_link by an odometry system.

Note

There is no requirement on using a LIDAR on your robot to use the navigation system. There is no requirement to use lidar-based collision avoidance, localization, or SLAM. However, we do provide instructions and support tried and true implementations of these things using lidars. You can be equally as successful using a vision or depth based positioning system and using other sensors for collision avoidance. The only requirement is that you follow the standards below with your choice of implementation.

Standards

REP 105 defines the frames and conventions required for navigation and the larger ROS ecosystem. These conventions should be followed at all times to make use of the rich positioning, odometry, and SLAM projects available in the community.

In a nutshell, REP-105 says that you must, at minimum, build a TF tree that contains a full map -> odom -> base_link -> [sensor frames] for your robot. TF2 is the time-variant transformation library in ROS 2 we use to represent and obtain time synchronized transformations. It is the job of the global positioning system (GPS, SLAM, Motion Capture) to, at minimum, provide the map -> odom transformation. It is then the role of the odometry system to provide the odom -> base_link transformation. The remainder of the transformations relative to base_link should be static and defined in your URDF.

Global Positioning: Localization and SLAM

It is the job of the global positioning system (GPS, SLAM, Motion Capture) to, at minimum, provide the map -> odom transformation. We provide amcl which is an Adaptive Monte-Carlo Localization technique based on a particle filter for localization in a static map. We also provide SLAM Toolbox as the default SLAM algorithm for use to position and generate a static map.

These methods may also produce other output including position topics, maps, or other metadata, but they must provide that transformation to be valid. Multiple positioning methods can be fused together using robot localization, discussed more below.

Odometry

It is the role of the odometry system to provide the odom -> base_link transformation. Odometry can come from many sources including LIDAR, RADAR, wheel encoders, VIO, and IMUs. The goal of the odometry is to provide a smooth and continuous local frame based on robot motion. The global positioning system will update the transformation relative to the global frame to account for the odometric drift.

Robot Localization is typically used for this fusion. It will take in N sensors of various types and provide a continuous and smooth odometry to TF and to a topic. A typical mobile robotics setup may have odometry from wheel encoders, IMUs, and vision fused in this manner.

The smooth output can be used then for dead-reckoning for precise motion and updating the position of the robot accurately between global position updates.


Environmental Representation

The environmental representation is the way the robot perceives its environment. It also acts as the central localization for various algorithms and data sources to combine their information into a single space. This space is then used by the controllers, planners, and recoveries to compute their tasks safely and efficiently.

Costmaps and Layers

The current environmental representation is a costmap. A costmap is a regular 2D grid of cells containing a cost from unknown, free, occupied, or inflated cost. This costmap is then searched to compute a global plan or sampled to compute local control efforts.

Various costmap layers are implemented as pluginlib plugins to buffer information into the costmap. This includes information from LIDAR, RADAR, sonar, depth images, etc. It may be wise to process sensor data before inputting it into the costmap layer, but that is up to the developer.

Costmap layers can be created to detect and track obstacles in the scene for collision avoidance using camera or depth sensors. Additionally, layers can be created to algorithmically change the underlying costmap based on some rule or heuristic. Finally, they may be used to buffer live data into the 2D or 3D world for binary obstacle marking.

Costmap Filters

Imagine, you’re annotating a map file (or any image file) in order to have a specific action occur based on the location in the annotated map. Examples of marking/annotating might be keep out zones to avoid planning inside, or have pixels belong to maximum speeds in marked areas. This annotated map is called “filter mask”. Just like a mask overlaid on a surface, it can or cannot be same size, pose and scale as a main map. The main goal of filter mask - is to provide the ability of marking areas on maps with some additional features or behavioral changes.

Costmap filters are a costmap layer-based approach of applying spatial-dependent behavioral changes, annotated in filter masks, into the Nav2 stack. Costmap filters are implemented as costmap plugins. These plugins are called “filters” as they are filtering a costmap by spatial annotations marked on filter masks. In order to make a filtered costmap and change a robot’s behavior in annotated areas, the filter plugin reads the data coming from the filter mask. This data is being linearly transformed into a feature map in a filter space. Having this transformed feature map along with a map/costmap, any sensor data and current robot coordinate filters can update the underlying costmap and change the behavior of the robot depending on where it is. For example, the following functionality could be made by use of costmap filters:

  • Keep-out/safety zones where robots will never enter.

  • Speed restriction areas. Maximum speed of robots going inside those areas will be limited.

  • Preferred lanes for robots moving in industrial environments and warehouses.

Other Forms

Various other forms of environmental representations exist. These include:

  • gradient maps, which are similar to costmaps but represent surface gradients to check traversibility over

  • 3D costmaps, which represent the space in 3D, but then also requires 3D planning and collision checking

  • Mesh maps, which are similar to gradient maps but with surface meshes at many angles

  • “Vector space”, taking in sensor information and using machine learning to detect individual items and locations to track rather than buffering discrete points.


Nav2 Academic Overview


© Copyright 2023.