🧩 Goal Navigation Plugins¶

In this section you’ĺl learn about the creation of a plugin for the bt_navigator and how to integrate it into the Behavior Tree used for the Goal Navigation.

How to create your plugin for BT Navigator¶

In this tutorial, you are going to learn how to create a Behavior Tree Action Node that can be used in any BT implementing a Navigation logic. This Action Node will simply print a log with a different level of verbosity for debugging purposes. Apart from developing a Behavior Tree Action Node, you can also create any other type of BT Node such as decorator, condition, and control nodes. Each node type has a unique role in the BT to perform actions like planning, control the flow of the BT, check the status of a condition, or modify the output of other BT nodes.

For this tutorial, we are going to use a new ROS 2 package called goal_navigation_tutorial, in which we are going to create a new C++ source file called log_printer_node.cpp.

#include <memory>
#include <string>

#include "rclcpp/rclcpp.hpp"
#include "behaviortree_cpp_v3/action_node.h"

namespace goal_navigation_tutorial
{
/**
* @brief A BT node that logs a message to the console
* with a specific log level that could be INFO, WARN, ERROR.
*/
class LogPrinter : public BT::AsyncActionNode
{
public:
   /**
   * @brief A constructor for goal_navigation_tutorial::LogPrinter
   *
   * @param xml_tag_name Name for the XML tag for this node
   * @param conf  BT node configuration
   */
   LogPrinter(
      const std::string & xml_tag_name,
      const BT::NodeConfiguration & conf)
   : BT::AsyncActionNode(xml_tag_name, conf)
   {
      node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
      getInput("log_text", log_text_);
      getInput("log_level", log_level_);

      if (log_text_.empty()) {
            RCLCPP_WARN(node_->get_logger(), "LogPrinter: log_text is empty");
      }
      if (log_level_.empty()) {
            RCLCPP_WARN(node_->get_logger(), "LogPrinter: log_level is empty, setting to INFO");
            log_level_ = "INFO";
      }
   }

   /**
   * @brief Creates list of BT ports
   * @return BT::PortsList Containing basic ports along with node-specific ports
   */
   static BT::PortsList providedPorts()
   {
      return
      {
            BT::InputPort<std::string>("log_text", "Text to be logged"),
            BT::InputPort<std::string>("log_level", "Log level (INFO, WARN, ERROR)"),
      };
   }

private:
   /**
   * @brief Function to perform some user-defined operation on tick
   */
   BT::NodeStatus tick() override
   {
      if (log_level_ == "INFO") {
            RCLCPP_INFO(node_->get_logger(), "%s", log_text_.c_str());
            return BT::NodeStatus::SUCCESS;
      } else if (log_level_ == "WARN") {
            RCLCPP_WARN(node_->get_logger(), "%s", log_text_.c_str());
            return BT::NodeStatus::SUCCESS;
      } else if (log_level_ == "ERROR") {
            RCLCPP_ERROR(node_->get_logger(), "%s", log_text_.c_str());
            return BT::NodeStatus::SUCCESS;
      } else {
            RCLCPP_ERROR(node_->get_logger(), "Invalid log level: %s", log_level_.c_str());
            return BT::NodeStatus::FAILURE;
      }
   }

   std::string log_text_;
   std::string log_level_;
   rclcpp::Node::SharedPtr node_;
};

}  // namespace goal_navigation_tutorial

#include "behaviortree_cpp_v3/bt_factory.h"

BT_REGISTER_NODES(factory)
{
   factory.registerNodeType<goal_navigation_tutorial::LogPrinter>("LogPrinter");
}

Compile and Install¶

To compile and install the plugin, the file CMakeLists.txt should declare the following (minimum) dependencies:

find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_components REQUIRED)
find_package(behaviortree_cpp_v3 REQUIRED)

set(dependencies
     rclcpp
     rclcpp_lifecycle
     behaviortree_cpp_v3
)
include_directories(
   ${rclcpp_INCLUDE_DIRS}
   ${rclcpp_components_INCLUDE_DIRS}
   ${behaviortree_cpp_v3_INCLUDE_DIRS}
)

To compile and install the plugin, you need to add and install a new library

add_library(pal_log_printer_bt_node SHARED plugins/action/log_printer_node.cpp)
list(APPEND plugin_libs pal_log_printer_bt_node)

install(TARGETS ${plugin_libs}
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION bin
)

foreach(bt_plugin ${plugin_libs})
ament_target_dependencies(${bt_plugin} ${dependencies})
target_compile_definitions(${bt_plugin} PRIVATE BT_PLUGIN_EXPORT)
endforeach()

Finally, the package.xml file should include the required dependencies:

<depend>rclcpp</depend>
<depend>rclcpp_components</depend>
<depend>behaviortree_cpp_v3</depend>

Add your custom plugin to the Behavior Tree¶

To add your custom plugin to the Behavior Tree used by the bt_navigator, you need to create a new XML file

In order to do this you first need to create a new directory in the .pal folder of your robot.

mkdir -p ~/.pal/pal_navigation_cfg_params/params/nav2_bt_navigator

And then, within this folder, you can create a new XML file for your custom Behavior Tree.

touch ~/.pal/pal_navigation_cfg_params/params/nav2_bt_navigator/my_custom_bt_w_custom_plugin.xml

Now you can generate the XML file with the following content:

my_custom_bt_w_custom_plugin.xml¶

<!--
   This Behavior Tree adds a custom plugin to the default Nav2 BT Navigator Behavior Tree.
   The custom plugin is a LogPrinter node that logs a message with a specific log level.
-->
<root main_tree_to_execute="MainTree">
   <BehaviorTree ID="MainTree">
      <RecoveryNode number_of_retries="6" name="NavigateRecovery">
         <PipelineSequence name="NavigateWithReplanning">
         <RateController hz="1.0">
            <RecoveryNode number_of_retries="1" name="ComputePathToPose">
               <ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
               <ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
            </RecoveryNode>
         </RateController>
         <RecoveryNode number_of_retries="1" name="FollowPath">
            <FollowPath path="{path}" controller_id="FollowPath"/>
            <ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
         </RecoveryNode>
         </PipelineSequence>
         <ReactiveFallback name="RecoveryFallback">
         <GoalUpdated/>
         <RoundRobin name="RecoveryActions">
            <Sequence name="ClearingActions">
               <LogPrinter log_text="executing clear costmaps" log_level="WARN">
               <ClearEntireCostmap name="ClearLocalCostmap-Subtree" service_name="local_costmap/clear_entirely_local_costmap"/>
               <ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/>
            </Sequence>
            <LogPrinter log_text="executing spin wait and backup beahviors" log_level="WARN">
            <Spin spin_dist="1.57"/>
            <Wait wait_duration="5"/>
            <BackUp backup_dist="0.30" backup_speed="0.05"/>
         </RoundRobin>
         </ReactiveFallback>
      </RecoveryNode>
   </BehaviorTree>
</root>

In this Behavior Tree it has been added a custom plugin, the LogPrinter node, it has two input ports, log_text and log_level, that allow to specify the text to be logged and the log level (INFO, WARN, ERROR).

LogPrinter node¶

<LogPrinter log_text="text to print" log_level="INFO">

Configuration¶

Now you can configure the bt_navigator, to use the custom plugin and custom Behavior Tree you created. To do so, you first need to create a new directory in the .pal folder of your robot.

mkdir -p ~/.pal/pal_navigation_cfg_params/params/nav2_bt_navigator

And then, within this folder, you can create a new BT Navigator configuration file.

touch ~/.pal/pal_navigation_cfg_params/params/nav2_bt_navigator/my_bt_navigator_config.yaml

Within the newly created my_bt_navigator_config.yaml file, you can insert your custom bt_navigator parameters. For the list of available parameters, you can refer to the Nav2 BT Navigator configuration guide. When creating a new Navigation configuration file, apart from the node parameters, you also need to specify the robot to which they refer.

pal_navigation_cfg:
   ros__parameters:
      supported_robots:
      - some_pal_robot # e.g. tiago, omni_base, tiago_pro, etc.

bt_navigator:
   ros__parameters:
      # Your BT Navigator Parameters
      default_nav_to_pose_bt_xml: /home/pal/.pal/pal_navigation_cfg_params/params/nav2_bt_navigator/my_custom_bt_w_custom_plugin.xml # Full path to your custom BT XML file

      # Other parameters

      plugin_lib_names:
      # List of plugin libraries to load
      - pal_log_printer_bt_node

Once created, to start using your custom BT Navigator configuration, you need to set it in the navigation pipeline by modifying the <robot>_nav.yaml file in the <robot>_2dnav package.

sudo vi /opt/pal/$PAL_DISTRO/share/<robot>_2dnav/params/<robot>_nav.yaml

And then, change the params field of the bt_navigator to the name of your custom BT Navigator configuration file.

nodes:
   # Other nodes
   bt_navigator:
      app: bt_navigator
      params: [my_bt_navigator_config]

Attention

Make sure that the name of the custom BT Navigator configuration file you use in the params field (my_bt_navigator_config) is the same as the name of the file you created in the .pal folder (my_bt_navigator_config.yaml).

Finally, to apply the changes and start using the new BT Navigator configuration, you need to restart the navigation pipeline with the command:

pal module restart navigation