🧩 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:
<!--
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 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
See also¶
To continue learning about the creation of Behavior Trees nodes used in Nav2, you can check Writing a New Behavior Tree Plugin.