#Premise
The version of ROS is Humble. I'm trying to implement a behavior tree plugin for an action that moves an omnidirectional robot in 2D mantaining a fixed direction. I'd like to extend a navigation2 move_to_pose_with_replanning_and_recovery behaviour tree in such a way that the last approach is executed using my custom action. The action works normally when called by an action client or with a goal action message from CLI.
I'm fairly new with C++ but I think I understand most of the nav2_behavior_tree source code.
I created the two files of the wait action example in a separate library, one contains the .hpp code for the plugin, one the .cpp code. The library has the same setup as the nav2_behavior_tree:
- planar_move
-planar_move
- ... (action definition code)
- planar_node (where the bt plugin is defined)
-include/plugins
-planar_move_action.hpp
-plugins
-planar_move_action.hpp
-cmake
-plugin_palette.xml (defines the custom action plugin)
-custom_bt.xml (defines the custom behavior tree to use with nav to pose)
-package.xml
this is the planar_move_aciton.cpp code that implements the behavior tree plugin:
#include <memory>
#include <string>
#include "planar_move_interfaces/plugins/action/planar_move.hpp"
namespace its_behaviour_tree
{
PlanarMoveActionNode::PlanarMoveActionNode(
const std::string & xml_tag_name,
const std::string & action_name,
const BT::NodeConfiguration & conf)
: BtActionNode<Action>(xml_tag_name, action_name, conf)
{
}
void PlanarMoveActionNode::on_tick()
{
if (!getInput("goal", goal_.pose)) {
RCLCPP_ERROR(
node_->get_logger(),
"NavigateToPoseAction: goal not provided");
return;
}
}
BT::NodeStatus PlanarMoveActionNode::on_success()
{
// Set empty error code, action was successful
setOutput("error_code_id", ActionGoal::NONE);
return BT::NodeStatus::SUCCESS;
}
BT::NodeStatus PlanarMoveActionNode::on_aborted()
{
setOutput("error_code_id", result_.result->error_code);
return BT::NodeStatus::FAILURE;
}
BT::NodeStatus PlanarMoveActionNode::on_cancelled()
{
setOutput("error_code_id", ActionGoal::NONE);
return BT::NodeStatus::SUCCESS;
}
} // namespace its_behaviour_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<its_behaviour_tree::PlanarMoveActionNode>(
name, "planar_move", config);
};
factory.registerBuilder<its_behaviour_tree::PlanarMoveActionNode>(
"PlanarMove", builder);
}
This it the planar_move_action.hpp code:
#ifndef PLANAR_MOVE_ACTION_HPP_
#define PLANAR_MOVE_ACTION_HPP_
#include <string>
#include "planar_move_interfaces/action/planar_move.hpp"
#include "nav_msgs/msg/path.h"
#include "nav2_behavior_tree/bt_action_node.hpp"
class PlanarMoveActionNode : public BtActionNode<planar_move_interfaces::action::PlanarMove>
{
using Action = nav2_msgs::action::PlanarMove;
using ActionResult = Action::Result;
using ActionGoal = Action::Goal;
public:
/**
* @brief A constructor for nav2_behavior_tree::PlanarMoveActionNode
* @param xml_tag_name Name for the XML tag for this node
* @param action_name Action name this node creates a client for
* @param conf BT node configuration
*/
PlanarMoveActionNode(
const std::string & xml_tag_name,
const std::string & action_name,
const BT::NodeConfiguration & conf);
/**
* @brief Function to perform some user-defined operation on tick
*/
void on_tick() override;
/**
* @brief Function to perform some user-defined operation upon successful completion of the action
*/
BT::NodeStatus on_success() override;
/**
* @brief Function to perform some user-defined operation upon abortion of the action
*/
BT::NodeStatus on_aborted() override;
/**
* @brief Function to perform some user-defined operation upon cancellation of the action
*/
BT::NodeStatus on_cancelled() override;
/**
* @brief Creates list of BT ports
* @return BT::PortsList Containing basic ports along with node-specific ports
*/
static BT::PortsList providedPorts()
{
return providedBasicPorts(
{
BT::InputPort<geometry_msgs::msg::PoseStamped>("goal", "Destination to plan to"),
});
}
};
} // namespace nav2_behavior_tree
#endif // PLANAR_MOVE_ACTION_HPP_
Problem
I'm unsure on how to utilize the custom behavior tree with the navigation2 stack and how to include the plugin into nav2. From what I have seen from the tutorials, I should add the default behavior tree to use with the nav to pose action and the custom plugin into the configuration .yaml file. This is what I did:
default_nav_to_pose_bt_xml: "home/user/project/src/planar_move/planar_node/first_try_tree.xml"
plugin_lib_names:
- nav2_compute_path_to_pose_action_bt_node
- nav2_compute_path_through_poses_action_bt_node
- nav2_follow_path_action_bt_node
- ...
- nav2_planar_move_action_bt_node
The current naming pattern does not work since the plugin does not get found by nav2 stack and this leads to a malfunction of the code. Also, despite using the absolute path, the code can't find the custom behaviour tree. My suspicion is that the custom bt uses the custom action which is also not found by the code, not sure though.
I wanted to know how to correctly name the plugin in order for it to be found at runtime by the nav2 stack, I can't seem to understand the naming pattern used for the other plugins as it is not specified anywhere in the documentation. I also couldn't find where in the code the plugin name is specified.
I supposed this was done in the factory builder but I don't see any part relevant to the naming.
For example: all the current plugins come from nav2 and are action_bt_nodes so it makes sense that the naming pattern would be nav2_<plugin_name>_action_bt_node. But my plugin comes from a different library (planar_node) and naming it planar_node_planar_move_action_bt_node does not seem to work. I tried using the namespace as well its_behavior_tree_planar_move_bt_action but it lead to nothing
I could just git clone the whole repo, modify the navigation behavior tree by adding my plugin and build nav2 from there but I think it would not be optimal.
Thanks in advance for any kind of help or direction :)
Originally posted by filippo.guarda on ROS Answers with karma: 13 on 2023-04-30
Post score: 0
