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); |
|
} |
|
} |