In robotics, efficient navigation is paramount for successful task completion, whether it's guiding a robot through a cluttered environment or navigating complex terrain. The Navigation2 (NAV2) framework within the Robot Operating System (ROS) provides a powerful suite of tools for robot navigation. One key component in orchestrating robot behavior is the Behavior Tree (BT), a hierarchical control structure widely used for decision-making in autonomous systems which in NAV2 is responsible for navigation decisions from point A to point B.

This tutorial focuses on integrating custom Behavior Trees leaf into NAV2 BT, leveraging the flexibility and modularity of the BT architecture to enhance robot navigation strategies. By combining the structured decision-making of BTs with the robust navigation capabilities of NAV2, robots can exhibit more intelligent and adaptive behavior in diverse environments.


Understanding Behavior Trees (BTs) in Robotics

Behavior Trees are the successors of state machines and provide a hierarchical representation of robot behavior, organizing actions, conditions, and control flow nodes into a tree structure. At the root of the tree is the main decision-making node, which delegates tasks to child nodes based on predefined conditions and priorities. This hierarchical organization facilitates clear, modular, and scalable behavior design, making BTs a popular choice in robotics.

In the context of NAV2, Behavior Trees serve as the control mechanism for guiding robot navigation. By defining navigation-related tasks and conditions within the BT structure, robots can autonomously plan and execute navigation behaviors while dynamically adapting to changes in the environment.


Integrating Behavior Trees with NAV2

To integrate Behavior Trees with NAV2, we utilize tools such as Groot. Groot, a graphical tool for editing BTs, simplifies the design and visualization of complex behavior hierarchies. Custom nodes, tailored to specific navigation tasks or conditions, extend the functionality of NAV2 and enhance the robot's decision-making capabilities.

This tutorial provides step-by-step instructions for:

  1. Setting up Groot for editing NAV2 Behavior Trees.
  2. Adding custom nodes to the NAV2 Behavior Tree framework.
  3. Modifying Humble configuration files to incorporate custom nodes into the navigation pipeline.


By following these steps, developers can extend the capabilities of NAV2 and tailor robot behavior to specific navigation challenges, ultimately improving the overall autonomy and performance of robotic systems.

Now, let's dive into the practical implementation of integrating Behavior Trees with NAV2, starting with setting up the necessary tools and environment.


Using groot


Adding a custom node to NAV2 BT in Humble

In Humble, modifying a NAV2 BT has not yet been made obvious, so here's what we're interested in: adding a custom node. In the Navigation2 stack, we will have to act on the nav2_behavior_tree and nav2_bt_navigator packages.

Let's consider adding a condition node called GoalDistanceCondition:

  • in the nav2_behavior_tree package we will add these files:
nav2_behavior_tree
├── include
│   └── nav2_behavior_tree
│       └── plugins
│           └── condition
│               └── goal_distance_condition.hpp
└── plugins
    └── condition
        └── goal_distance_condition.cpp
  • still in the nav2_behavior_tree package, we are going to modify these files:
nav2_behavior_tree
├── CMakeLists.txt
└── nav2_tree_nodes.xml
  • by adding these two lines to the CMakeLists.txt:
add_library(nav2_goal_distance_condition_bt_node SHARED plugins/condition/goal_distance_condition.cpp)
list(APPEND plugin_libs nav2_goal_distance_condition_bt_node)
  • and in nav2_tree_nodes.xml by adding your custom node ID. For this exemple:
<Condition ID="GoalDistance">
    <input_port name="goal">Destination</input_port>
    <input_port name="global_frame">Reference frame</input_port>
    <input_port name="robot_base_frame">Robot base frame</input_port>
    <input_port name="threshold">threshold</input_port>
</Condition>
  • we then need to modify this file in the nav2_bt_navigator package
nav2_bt_navigator
└── src
    └── bt_navigator.cpp
  • and add the following line in const std::vector<std::string> plugin_libs:
"nav2_goal_distance_condition_bt_node"

The customised node is now added and practically ready to use. The only thing left to do is to add it to the .yaml navigation configuration file generally called nav2_params.yaml the following line to the list of plugins used by the bt_navigator:

- nav2_goal_distance_condition_bt_node

Below you will find the hpp and cpp code mentioned and you can use it as a basis for the development of your own nodes: goal_distance_condition.hpp content:

#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__GOAL_DISTANCE_CONDITION_HPP_
#define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__GOAL_DISTANCE_CONDITION_HPP_

#include <string>
#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "behaviortree_cpp_v3/condition_node.h"
#include "tf2_ros/buffer.h"
#include "geometry_msgs/msg/pose_stamped.hpp"

namespace nav2_behavior_tree
{

class GoalDistanceCondition : public BT::ConditionNode
{
public:
GoalDistanceCondition(
    const std::string & condition_name,
    const BT::NodeConfiguration & conf);

GoalDistanceCondition() = delete;

~GoalDistanceCondition() override;

BT::NodeStatus tick() override;

void initialize();

bool isGoalReached();

static BT::PortsList providedPorts()
{
    return {
    BT::InputPort<geometry_msgs::msg::PoseStamped>("goal", "Destination"),
    BT::InputPort<std::string>("global_frame", std::string("map"), "Global frame"),
    BT::InputPort<std::string>("robot_base_frame", std::string("base_link"), "Robot base frame"),
    BT::InputPort<double>("threshold", 0.5, "Threshold distance")
    };
}

protected:
void cleanup()
{}

private:
rclcpp::Node::SharedPtr node_;
std::shared_ptr<tf2_ros::Buffer> tf_;

bool initialized_;
double threshold_;
double goal_reached_tol_;
std::string global_frame_;
std::string robot_base_frame_;
double transform_tolerance_;
};

}  // namespace nav2_behavior_tree

#endif  // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__GOAL_DISTANCE_CONDITION_HPP_

goal_distance_condition.cpp content:

#include <string>
#include <memory>

#include "nav2_util/robot_utils.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "nav2_util/node_utils.hpp"
#include "nav2_util/geometry_utils.hpp"

#include "nav2_behavior_tree/plugins/condition/goal_distance_condition.hpp"

namespace nav2_behavior_tree
{

GoalDistanceCondition::GoalDistanceCondition(
  const std::string & condition_name,
  const BT::NodeConfiguration & conf)
: BT::ConditionNode(condition_name, conf),
  initialized_(false),  
  global_frame_("map"),
  robot_base_frame_("base_link")
{
  getInput("threshold", threshold_);
  getInput("global_frame", global_frame_);
  getInput("robot_base_frame", robot_base_frame_);
}


GoalDistanceCondition::~GoalDistanceCondition()
{
  cleanup();
}

BT::NodeStatus GoalDistanceCondition::tick()
{
  if (!initialized_) {
    initialize();
  }

  BT::NodeStatus status = isGoalReached() ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE;

  // Display node status in console
  // std::string status_str = (status == BT::NodeStatus::SUCCESS) ? "SUCCESS" : "FAILURE";
  // std::cout << "GoalDistanceCondition tick() status: " << status_str << std::endl;

  return status;
}
void GoalDistanceCondition::initialize()
{
  node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");

  node_->get_parameter("threshold", threshold_);

  nav2_util::declare_parameter_if_not_declared(
    node_, "goal_reached_tol",
    rclcpp::ParameterValue(0.25));
  node_->get_parameter_or<double>("goal_reached_tol", goal_reached_tol_, 0.25);
  tf_ = config().blackboard->get<std::shared_ptr<tf2_ros::Buffer>>("tf_buffer");

  node_->get_parameter("transform_tolerance", transform_tolerance_);

  initialized_ = true;
}

bool GoalDistanceCondition::isGoalReached()
{
  geometry_msgs::msg::PoseStamped current_pose;

  if (!nav2_util::getCurrentPose(
      current_pose, *tf_, global_frame_, robot_base_frame_, transform_tolerance_))
  {
    RCLCPP_DEBUG(node_->get_logger(), "Current robot pose is not available.");
    return false;
  }

  geometry_msgs::msg::PoseStamped goal;
  getInput("goal", goal);

  auto travelle = nav2_util::geometry_utils::euclidean_distance(
    goal.pose, current_pose.pose);
  // std::cout << "travelle : " << travelle << std::endl;
  
  return travelle <= threshold_ ;
}

}  // namespace nav2_behavior_tree

#include "behaviortree_cpp_v3/bt_factory.h"

BT_REGISTER_NODES(factory)
{
  BT::NodeBuilder builder =
    [](const std::string & name, const BT::NodeConfiguration & config)
    {
      return std::make_unique<nav2_behavior_tree::GoalDistanceCondition>(name, config);
    };

  factory.registerBuilder<nav2_behavior_tree::GoalDistanceCondition>("GoalDistance", builder);
}