BehaviorTree/BehaviorTree.ROS2

Overrided registerNodesIntoFactory doesn't work

Closed this issue · 1 comments

Thank you for adding the new and exciting feature, BT::TreeExecutionServer.

I attempted to create my own class by inheriting from BT::TreeExecutionServer, but it seems that the registerNodesIntoFactory I inherited is not functioning properly. When attempting to load a behavior tree containing the Node added within registerNodesIntoFactory, the following error occurs:

[ERROR] [1715003079.255998664] [bt_action_server]: Failed to load BehaviorTree: tree.xml 
 Error at line 7: -> Node not recognized: MyNode

This issue seems to be caused by two reasons:

  • The registerNodesIntoFactory called within the constructor of BT::TreeExecutionServer is its own rather than that of the inheriting class.
  • RegisterBehaviorTrees() is called before registerNodesIntoFactory() is invoked.

TreeExecutionServer::TreeExecutionServer(const rclcpp::NodeOptions& options)
: p_(new Pimpl(options))
{
// create the action server
const auto action_name = p_->params.action_name;
RCLCPP_INFO(kLogger, "Starting Action Server: %s", action_name.c_str());
p_->action_server = rclcpp_action::create_server<ExecuteTree>(
p_->node, action_name,
[this](const rclcpp_action::GoalUUID& uuid,
std::shared_ptr<const ExecuteTree::Goal> goal) {
return handle_goal(uuid, std::move(goal));
},
[this](const std::shared_ptr<GoalHandleExecuteTree> goal_handle) {
return handle_cancel(std::move(goal_handle));
},
[this](const std::shared_ptr<GoalHandleExecuteTree> goal_handle) {
handle_accepted(std::move(goal_handle));
});
// register the users Plugins and BehaviorTree.xml files into the factory
RegisterBehaviorTrees(p_->params, p_->factory, p_->node);
registerNodesIntoFactory(p_->factory);
}

void RegisterBehaviorTrees(bt_server::Params& params, BT::BehaviorTreeFactory& factory,
rclcpp::Node::SharedPtr node)
{
// clear the factory and load/reload it with the Behaviors and Trees specified by the user in their [bt_action_server] config yaml
factory.clearRegisteredBehaviorTrees();
BT::RosNodeParams ros_params;
ros_params.nh = node;
ros_params.server_timeout = std::chrono::milliseconds(params.ros_plugins_timeout);
ros_params.wait_for_server_timeout = ros_params.server_timeout;
for(const auto& plugin : params.plugins)
{
const auto plugin_directory = GetDirectoryPath(plugin);
// skip invalid plugins directories
if(plugin_directory.empty())
{
continue;
}
LoadRosPlugins(factory, plugin_directory, ros_params);
}
for(const auto& tree_dir : params.behavior_trees)
{
const auto tree_directory = GetDirectoryPath(tree_dir);
// skip invalid subtree directories
if(tree_directory.empty())
continue;
LoadBehaviorTrees(factory, tree_directory);
}
}

yiu are right!!!! I will fix it