Integrate Nav2 Plugins In Custom Behavior Trees

Introduction

Hey guys! Today, we're diving deep into the fascinating world of Nav2 plugins and how you can seamlessly incorporate them into your custom behavior trees. If you're like me and experimenting with Navigation2 for an AMR (Autonomous Mobile Robot) in a home automation setting, you've probably wondered how to leverage the built-in action, control, and decorator nodes (bt_nodes) that Nav2 offers. The goal? Crafting a custom behavior tree that orchestrates a series of maneuvers when specific conditions are met. Let’s explore this together!

Understanding the Basics of Behavior Trees and Nav2 Plugins

Before we get into the nitty-gritty, let’s make sure we're all on the same page. Behavior trees are a powerful tool for creating complex robot behaviors. They provide a structured way to define the decision-making process of your robot, making it easy to manage and extend its capabilities. Nav2, built on top of ROS2, offers a rich set of plugins that you can use within your behavior trees. These plugins include actions like Spin, Wait, and others that are essential for navigation and recovery.

The beauty of Nav2 lies in its modular design. It provides a suite of pre-built behaviors that you can easily integrate into your robot's navigation stack. However, the real magic happens when you start customizing these behaviors to fit your specific needs. That's where custom behavior trees come into play. By creating your own trees, you can define complex sequences of actions, reactions to environmental changes, and recovery strategies, all tailored to your robot's mission.

When you're working with Nav2 plugins, you're essentially dealing with pre-packaged functionalities that can be plugged into your behavior tree. Think of them as Lego bricks – each one performs a specific task, and you can combine them in various ways to build a complete robot behavior. These plugins can range from simple actions like rotating in place to complex tasks like path planning and obstacle avoidance. Understanding how to use these plugins effectively is the key to unlocking the full potential of Nav2.

The Initial Setup: A Simple Behavior Tree Example

To kick things off, I started with a simple behavior tree. The idea was to create a ROS 2 CPP node that would execute a basic sequence. Here’s a glimpse of the initial tree structure:

[Image of the behavior tree]

This tree includes a Spin node, which is a common action in robotics used to make the robot rotate in place. The goal was to get this basic setup working before diving into more complex behaviors. To define this tree, I used Groot, a popular tool for visualizing and editing behavior trees.

Code Snippets: .hpp and .cpp Files

Let's dive into the code. Here’s an abridged version of the .hpp file:

#ifndef BT_RECOVERY_NODE_HPP
#define BT_RECOVERY_NODE_HPP

//* CPP includes
#include <iostream>
#include <fstream>

//* ROS2 includes
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/bool.hpp"
#include "pugixml.hpp"
#include "nlohmann/json.hpp"

//* BehaviorTree.cpp includes
#include "behaviortree_cpp_v3/bt_factory.h"
#include "behaviortree_cpp_v3/loggers/bt_zmq_publisher.h"
#include "ament_index_cpp/get_package_share_directory.hpp"

//* Nav2 specific nodes
#include "nav2_behavior_tree/plugins/action/spin_action.hpp"
#include "nav2_behavior_tree/bt_action_node.hpp"

class BTRecoveryNode : public rclcpp::Node
{
private:
    
    // Directory variables
    std::string bt_xml_dir_;
    std::string config_;
        
    // ROS2 related
    rclcpp::TimerBase::SharedPtr timer_;
    rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr start_bt_sub_;
    
    // state variables
    std::atomic<bool> bt_running_{false}; // std::atomic() is a thread safe-way of accessing this variable since it can be accessed from multiple threads
    bool should_shutdown_ = false; // Gets called only once, so may not be a problem

    // callbacks

    /**
     * @brief Callback to start this behavior tree
     * 
     * @param msg 
     */
    void startBTCallback(const std_msgs::msg::Bool::SharedPtr msg);

public:
    // BehaviorTree.CPP related
    BT::Tree tree_;
    BT::Blackboard::Ptr blackboard_;

    BTRecoveryNode(const std::string &name); 
    ~BTRecoveryNode();

    void setupBT();
    void stopBT();
    void updateBT();

    bool shouldShutdown() const { return should_shutdown_; }
};

#endif

And here’s a pertinent portion of the .cpp file:

#include "bt_recovery_node.hpp"

BTRecoveryNode::BTRecoveryNode(const std::string &name) : Node(name)
{
    RCLCPP_DEBUG(get_logger(), "Creating BTRecoveryNode ....");

    std::string default_bt_xml_path = ament_index_cpp::get_package_share_directory("robot_bt") + "/bt_xml/robot_recovery.xml";

    this->declare_parameter<std::string>("bt_xml_dir", default_bt_xml_path);
    
    this->start_bt_sub_ = this->create_subscription<std_msgs::msg::Bool>(
        "/start_recovery_bt", 10,
        std::bind(&BTRecoveryNode::startBTCallback, this, std::placeholders::_1));
    
    this->bt_running_.store(false, std::memory_order_relaxed);

    // TODO: use `xml` to read the tree config
    pugi::xml_document doc;
    bt_xml_dir_ = get_parameter("bt_xml_dir").as_string();
    pugi::xml_parse_result result = doc.load_file(bt_xml_dir_.c_str());

    if (!result) {
        RCLCPP_ERROR(get_logger(), "Failed to load behavior tree xml file: %s", bt_xml_dir_.c_str());
    }
    else {
        RCLCPP_INFO(get_logger(), "Successfully loaded behavior tree xml file: %s", bt_xml_dir_.c_str());
    }

    // TODO a ROS2 subscriber to read depth and or LaserTopic data 
}

BTRecoveryNode::~BTRecoveryNode() = default;

void BTRecoveryNode::setupBT()
{
    // create the behavior tree
    BT::BehaviorTreeFactory factory;
    BT::NodeBuilder builder;

    /*
        /opt/ros/humble/lib/libnav2_spin_action_bt_node.so
        /opt/ros/humble/lib/libnav2_spin_behavior.so
        /opt/ros/humble/lib/libnav2_spin_cancel_bt_node.so
    */

    // Explicitly load Nav2 BT plugins
    // factory.registerFromPlugin("/opt/ros/humble/lib/libnav2_spin_action_bt_node.so");


    RCLCPP_DEBUG(get_logger(), "SpinAction creating");
    builder =
        [=](const std::string &name, const BT::NodeConfiguration &config) {
        return std::make_unique<nav2_behavior_tree::SpinAction>(name, "spin_action", config, shared_from);
    };
    factory.registerBuilder<nav2_behavior_tree::SpinAction>("SpinAction", builder);


    // Works but gives run time error of cannot find shared object
    // factory.registerFromPlugin("nav2_spin_action_bt_node");

    bt_xml_dir_ = get_parameter("bt_xml_dir").as_string();
    tree_ = factory.createTreeFromFile(bt_xml_dir_);
    printTreeRecursively(tree_.rootNode());
    
    RCLCPP_INFO(get_logger(), "BTRecoveryNode::setupBT: Behavior tree created.");
}

The Error: Blackboard::get() Error

Despite compiling successfully, running the code with ros2 run resulted in the following error:

Successfully loaded behavior tree xml file: /home/kaliber/ros2_ws/install/robot_bt/share/robot_bt/bt_xml/q9_recovery.xml
terminate called after throwing an instance of 'BT::RuntimeError'
  what():  Blackboard::get() error. Missing key [node]
[ros2run]: Aborted

This error, Blackboard::get() error. Missing key [node], is a common stumbling block when working with behavior trees in ROS2, especially when integrating Nav2 plugins. It essentially means that the behavior tree is trying to access a value from the blackboard (a shared data storage within the tree) that hasn't been set. In this case, the missing key is [node], which typically refers to the ROS2 node instance.

Diving Deeper: Understanding the Blackboard and Node Context

To truly grasp this error, let’s break down what the blackboard is and why the node context is crucial. The blackboard in a behavior tree acts as a central repository for data. Nodes in the tree can read from and write to the blackboard, allowing them to share information and make decisions based on the current state of the system. It’s like a shared workspace where all the nodes can access the same data.

The [node] key in the blackboard is particularly important in ROS2 because it provides access to the ROS2 node instance. This instance is the heart of your ROS2 application, handling things like communication, parameter management, and lifecycle control. Many Nav2 plugins rely on having access to the ROS2 node to perform their tasks, such as sending commands to the robot or subscribing to sensor data.

The error message Missing key [node] indicates that a node in the behavior tree is trying to access the ROS2 node instance from the blackboard, but it’s not there. This can happen for a few reasons:

1. The ROS2 node hasn’t been added to the blackboard: This is the most common cause. You need to explicitly add the ROS2 node instance to the blackboard before running the behavior tree.
2. The blackboard is not being passed correctly: Sometimes, the blackboard might not be propagated correctly through the tree, especially if you’re using custom nodes or subtrees.
3. Incorrect node configuration: If a node is not configured correctly, it might not be able to access the blackboard properly.

Resolving the "Missing Key [node]" Error

So, how do we fix this pesky error? The key is to ensure that the ROS2 node instance is added to the blackboard before the behavior tree starts running. Here’s how you can do it:

1. Add the ROS2 Node to the Blackboard:

In your BTRecoveryNode::setupBT() function, you need to add the ROS2 node instance to the blackboard. This can be done using the Blackboard::set() method. Here’s the code snippet:

void BTRecoveryNode::setupBT()
{
    // create the behavior tree
    BT::BehaviorTreeFactory factory;
    BT::NodeBuilder builder;

    // Explicitly load Nav2 BT plugins
    // factory.registerFromPlugin("/opt/ros/humble/lib/libnav2_spin_action_bt_node.so");

    RCLCPP_DEBUG(get_logger(), "SpinAction creating");
    builder =
        [=](const std::string &name, const BT::NodeConfiguration &config) {
        return std::make_unique<nav2_behavior_tree::SpinAction>(name, "spin_action", config, shared_from_this());
    };
    factory.registerBuilder<nav2_behavior_tree::SpinAction>("SpinAction", builder);

    // Add the ROS2 node to the blackboard
    blackboard_ = BT::Blackboard::create();
    blackboard_->set<rclcpp::Node::SharedPtr>("node", shared_from_this());

    bt_xml_dir_ = get_parameter("bt_xml_dir").as_string();
    tree_ = factory.createTreeFromFile(bt_xml_dir_, blackboard_);
    printTreeRecursively(tree_.rootNode());
    
    RCLCPP_INFO(get_logger(), "BTRecoveryNode::setupBT: Behavior tree created.");
}

In this code, we first create a new blackboard instance using BT::Blackboard::create(). Then, we use blackboard_->set<rclcpp::Node::SharedPtr>("node", shared_from_this()) to add the ROS2 node instance to the blackboard. The shared_from_this() method returns a shared pointer to the current node, which is then associated with the key "node" in the blackboard. Finally, we pass the blackboard to the createTreeFromFile method, ensuring that the behavior tree has access to the ROS2 node.

2. Use shared_from_this() Correctly:

When registering your custom node builders, make sure to use shared_from_this() to pass the shared pointer of your node. This ensures that the node remains alive as long as the behavior tree is running. In the code snippet above, you can see that we’ve used shared_from_this() in the lambda function when registering the SpinAction node.

3. Verify Blackboard Propagation:

If you’re using subtrees or custom nodes, double-check that the blackboard is being passed correctly. Each node in the tree should have access to the same blackboard instance. If the blackboard is not being propagated correctly, you might need to explicitly pass it to the child nodes or subtrees.

Answering the Core Questions: Plugins vs. Behaviors and Custom Tree Examples

Now, let's address the core questions that were raised:

Can SpinAction, Wait, etc., Plugins Be Usable with Custom Behavior Trees?

Absolutely! That's the whole point of Nav2's modular design. You can and should use these plugins in your custom behavior trees. They provide the building blocks for creating complex robot behaviors. The key is to understand how to register these plugins with the BehaviorTreeFactory and how to configure them correctly within your tree.

In Custom Behavior Trees, Do We Import the Plugins or the nav2_behaviors Servers?

This is a crucial distinction. You import the plugins, not the servers directly. The plugins are the behavior tree nodes themselves (like SpinAction, Wait, etc.), while the servers (like nav2_behaviors) are the underlying ROS2 action servers that these nodes interact with. The behavior tree nodes act as clients to these servers.

To put it simply, the plugins are the interface you use within your behavior tree, and the servers are the backend processes that execute the actions. When you use a SpinAction node in your tree, it sends a goal to the spin_action server, which then handles the actual execution of the spin maneuver.

Is There an Example of a Custom Behavior Tree That Shows How to Incorporate Them Correctly with Custom Behavior Trees Packaged as ROS 2 Node?

Yes, there are examples available, and we can construct a simplified one to illustrate the concept. Let's outline a basic example of a custom behavior tree that incorporates Nav2 plugins within a ROS 2 node.

#include "rclcpp/rclcpp.hpp"
#include "behaviortree_cpp_v3/bt_factory.h"
#include "nav2_behavior_tree/plugins/action/spin_action.hpp"
#include "nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp