🧩 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