Nav2 Behavior Trees: Customising Robot Behaviour Without Forking the Stack

Tom Cosgrove, 09.08.2026

TL;DR: A nav2 behavior tree custom configuration lets you change how your robot plans, recovers, and reacts to sensor failures by editing an XML file and, when needed, registering a small C++ plugin. You do not need to fork Nav2. This article shows what a behavior tree is in Nav2, how to swap the default XML, how to write a custom BT node in C++, and how to build a recovery for GNSS-fix loss on a field UGV such as the Leo Rover.

Why a nav2 behavior tree custom setup beats forking the stack

Nav2 orchestrates navigation through a Behavior Tree (BT) executed by bt_navigator. The tree defines the control flow: compute a path, follow it, react to failure, retry, or abort. BT leaves call Nav2 action and service interfaces, including planner, controller, smoother, and behavior-server functionality.

This matters because most behavioural changes people want, such as “wait longer before recovery”, “back up 40 cm instead of spinning”, or “pause navigation when RTK fix drops to float”, are BT edits, not Nav2 source changes. A nav2 behavior tree custom file plus optional plugins keeps you on upstream releases, which is important when you track ROS 2 Humble or Jazzy for long-term field deployments.

What a behavior tree is, in one sentence: a hierarchy of control nodes (Sequence, Fallback, Parallel, Decorators) and leaf nodes (Actions, Conditions) that are “ticked” at a fixed rate and return SUCCESS, FAILURE, or RUNNING. Nav2 uses BehaviorTree.CPP v3 or v4, depending on the ROS 2 distribution.

What ships in the default Nav2 tree

The default navigate_to_pose_w_replanning_and_recovery.xml contains two main branches wrapped in a RecoveryNode: a navigation branch (compute path on a rate, then follow path) and a recovery branch (clear costmaps, spin, back up, wait). If any leaf fails and the retry counter is exhausted, the whole navigation fails.

Before writing anything custom, inspect the shipped trees:

On a sourced Nav2 install: ros2 pkg prefix nav2_bt_navigator, then look under share/nav2_bt_navigator/behavior_trees/.
The directory includes default trees for NavigateToPose and NavigateThroughPoses, along with variants for different recovery and replanning strategies.

How to swap the default XML with your own tree

You do not need to rebuild Nav2 to use a different tree. Two options exist.

Option A – global default. Point bt_navigator at your file through the parameter default_nav_to_pose_bt_xml. Example fragment for nav2_params.yaml:

bt_navigator: ros__parameters: default_nav_to_pose_bt_xml: "/opt/leo_ws/src/leo_nav2/bt/nav_to_pose_rtk.xml" plugin_lib_names: - nav2_compute_path_to_pose_action_bt_node - nav2_follow_path_action_bt_node - nav2_spin_action_bt_node - nav2_back_up_action_bt_node - nav2_wait_action_bt_node - nav2_clear_costmap_service_bt_node - leo_rtk_check_condition_bt_node - leo_wait_for_rtk_fix_bt_node

Option B – per-goal override. The NavigateToPose action has a behavior_tree field. You can send a different XML per goal, useful when a mission planner chooses between “highway” and “off-road cautious” trees.

A minimal custom tree you can start from

The tree below adds two things to the stock behaviour: a time-based replanning cadence and an RTK-aware branch that waits for a fix instead of spinning blindly.

The Inverter makes loss of the required fix fail the navigation branch, which halts the active controller action and enters recovery. Two nodes here, RTKFixLost and WaitForRTKFix, are not shipped with Nav2. You register them as plugins.

How to write a custom BT node in C++

Nav2 BT nodes are plugin classes derived from BehaviorTree.CPP base classes. For a check that reads the current GNSS fix status, a ConditionNode is appropriate. It returns SUCCESS if the fix is lost (so the recovery fallback proceeds) and FAILURE if the fix is healthy.

#include <behaviortree_cpp_v3/condition_node.h> #include <rclcpp/rclcpp.hpp> #include <sensor_msgs/msg/nav_sat_fix.hpp> #include <mutex>

class RTKFixLost : public BT::ConditionNode {
public:
  RTKFixLost(const std::string & name, const BT::NodeConfiguration & cfg)
  : BT::ConditionNode(name, cfg) {
    config().blackboard->get("node", node_);
    std::string topic;
    getInput("topic", topic);
    getInput("min_status", min_status_);
    sub_ = node_->create_subscription<sensor_msgs::msg::NavSatFix>(
      topic, rclcpp::SensorDataQoS(),
      [this](sensor_msgs::msg::NavSatFix::SharedPtr msg) {
        std::lock_guard<std::mutex> lock(mutex_);
        last_status_ = msg->status.status;
        received_ = true;
      });
  }
  static BT::PortsList providedPorts() {
    return { BT::InputPort<std::string>("topic"),
           BT::InputPort<int>("min_status") };
  }
  BT::NodeStatus tick() override {
    std::lock_guard<std::mutex> lock(mutex_);
    if (!received_ || last_status_ < min_status_) {
      return BT::NodeStatus::SUCCESS;
    }
    return BT::NodeStatus::FAILURE;
  }
private:
  rclcpp::Node::SharedPtr node_;
  rclcpp::Subscription<sensor_msgs::msg::NavSatFix>::SharedPtr sub_;
  std::mutex mutex_;
  int last_status_{sensor_msgs::msg::NavSatStatus::STATUS_NO_FIX};
  int min_status_{2};
  bool received_{false};
};

#include "behaviortree_cpp_v3/bt_factory.h"
BT_REGISTER_NODES(factory) {
factory.registerNodeType<RTKFixLost>("RTKFixLost");
}

min_status=2 maps to NavSatStatus::STATUS_GBAS_FIX. The standard sensor_msgs/NavSatFix status values do not distinguish RTK float from RTK fixed. Use this threshold only if the GNSS driver explicitly documents a compatible mapping; otherwise, read the receiver’s RTK state from its vendor-specific message or expose that state through a dedicated topic.

Building and registering the plugin

The plugin is a shared library. In CMakeLists.txt:

add_library(leo_rtk_check_condition_bt_node SHARED src/rtk_fix_lost.cpp) ament_target_dependencies(leo_rtk_check_condition_bt_node rclcpp behaviortree_cpp_v3 sensor_msgs) target_compile_definitions(leo_rtk_check_condition_bt_node PRIVATE BT_PLUGIN_EXPORT) install(TARGETS leo_rtk_check_condition_bt_node LIBRARY DESTINATION lib)

Then add the library name to plugin_lib_names in bt_navigator‘s parameters, as shown earlier. Nav2 loads it at startup and matches the XML tag RTKFixLost to the registered type.

The WaitForRTKFix action follows the same pattern but derives from BT::StatefulActionNode and returns RUNNING until the fix status recovers or the timeout expires. It should use the Nav2 node supplied on the BT blackboard, rather than creating and spinning a separate ROS node. Structuring it as a stateful action lets the tree continue ticking other reactive branches, such as GoalUpdated, while waiting.

How to design recovery behaviors for terrain

Default Nav2 recovery, spin plus back up, assumes indoor conditions. On outdoor UGVs this can be counterproductive: spinning on loose gravel digs wheels in, and backing up blindly on a slope is worse than stopping. A field-oriented tree usually replaces or reorders these.

Practical patterns Fictionlab’s team has seen work on Leo Rover deployments and similar four-wheel skid-steer platforms:

Slope-aware recovery. Add a condition node that reads IMU pitch/roll and skips BackUp if the absolute pitch exceeds a threshold (for example 15 degrees).
Costmap-first recovery. Clear only the local costmap first; a full clear can erase valid static obstacles the global planner needs.
Sensor-health gating. Before any motion recovery, check that odometry, LiDAR, and GNSS are publishing at expected rates. If not, fail fast to the mission layer rather than executing motion.
RTK-aware pause. As shown above, waiting a bounded time for the GNSS solution to reconverge is often safer than driving on dead reckoning.

These are all XML changes with, at most, one or two custom condition plugins. The Nav2 source tree stays untouched, which is the point.

A realistic RTK-loss scenario

Consider a Leo Rover mapping a solar farm. The robot follows a global path generated in a UTM-based navigation frame with waypoints spaced every 5 m. When it passes under a row of panels, RTK drops from fixed to float. The EKF fusing GNSS and wheel odometry can start drifting laterally by tens of centimeters, and the controller can begin commanding corrections that are actually chasing the drift.

With the tree above, RTKFixLost causes the inverted navigation check to fail when the configured fix criterion is no longer met. The active navigation branch is halted and RTKAwareRecovery runs before the TerrainRecovery branch, so the robot waits up to 30 seconds for the required fix state. If RTK recovers, WaitForRTKFix returns SUCCESS, the recovery succeeds, and the main pipeline resumes. If not, the tree falls through to costmap clearing and controlled back-up, or, if you prefer, to a mission-level abort.

If you want to reproduce this on your hardware, see the platform documentation at docs.fictionlab.pl, the Leo Rover product page, and Fictionlab’s research applications reference. For fully bespoke autonomy stacks, the custom robotics service covers integration work beyond stock Nav2.

How to test a custom tree before deploying it

A behaviour change that looks correct in XML can still misbehave at runtime. A minimal test loop:

Run bt_navigator with ros2 launch nav2_bringup navigation_launch.py using your parameters.
Set bt_loop_duration: 10 to use a 10 ms tree tick period. For live Groot monitoring, enable enable_groot_monitoring where supported by your Nav2 distribution and use the compatible Groot client; you can also inspect the /behavior_tree_log topic when it is available.
Inject failures: rosbag-replay a GNSS stream with degraded status, or use ros2 topic pub to publish a NavSatFix message whose nested status.status value is -1.
Confirm the tree enters the expected branch by inspecting the log stream, not just robot motion.

Frequently asked questions

Do you need to recompile Nav2 to use a custom behavior tree?

No. Changing the XML file referenced by bt_navigator is enough. Only custom C++ plugin nodes require compilation, and they build in a separate package.

What is the difference between a BT node and a Nav2 plugin?

A BT node is a leaf or control element executed inside the behavior tree. A Nav2 plugin, such as a planner or controller, is an algorithm loaded by a Nav2 server. BT action leaves often call Nav2 action servers, which in turn use those plugins.

Can you use BehaviorTree.CPP v4 with Nav2?

It depends on the Nav2 release. Humble tracks v3, while newer distributions use v4. Check the package.xml of nav2_behavior_tree in your installed distribution before writing plugins.

How do you pass runtime data between BT nodes?

Through the blackboard. Ports declared with InputPort and OutputPort read and write keys such as {path} or {goal}. This is how ComputePathToPose hands a path to FollowPath.

Should you use one tree for everything or many?

Use as many as your operational modes require. The behavior_tree field on the NavigateToPose goal can select a tree per mission phase, for example a cautious tree for inspection and a faster one for transit legs.

How do you debug a tree that always falls into recovery?

Log every tick with a compatible Groot monitor or the /behavior_tree_log topic, then check which leaf returns FAILURE first. A common cause is ComputePathToPose failing because the goal lies in an inflated obstacle, rather than a genuine control issue.

Does a custom tree affect Nav2 upgrades?

Rarely for XML-only changes. Plugin nodes may need minor updates if the BehaviorTree.CPP major version changes between distributions. Keeping plugins in a separate package makes such upgrades tractable.

Start building your own navigation behaviour

Ready to try a nav2 behavior tree custom setup on real hardware? Full setup guides, parameter references, and example trees for Leo Rover are available at docs.fictionlab.pl.