From 3f44b698d0e88215e6de2641fcfd5a6b2ff5ae2b Mon Sep 17 00:00:00 2001 From: marqrazz Date: Sat, 27 Apr 2024 08:55:48 -0600 Subject: [PATCH] switch to UserCB struct --- .../action_server_bt/action_server_bt.hpp | 68 ++++++------------- action_server_bt/src/action_server_bt.cpp | 19 +++--- .../src/action_server_bt_node.cpp | 5 +- 3 files changed, 33 insertions(+), 59 deletions(-) diff --git a/action_server_bt/include/action_server_bt/action_server_bt.hpp b/action_server_bt/include/action_server_bt/action_server_bt.hpp index 4311852..01d3da6 100644 --- a/action_server_bt/include/action_server_bt/action_server_bt.hpp +++ b/action_server_bt/include/action_server_bt/action_server_bt.hpp @@ -26,6 +26,21 @@ namespace action_server_bt { +typedef std::function OnTreeCreatedCallback; +typedef std::function OnLoopCallback; +typedef std::function OnTreeExecutionCompletedCallback; + +/** + * @brief UserCallbacks is a collection of functions that get called + * after the tree is created, while its running and after it finishes. + */ +struct UserCallbacks +{ + OnTreeCreatedCallback on_create = [](BT::Tree&) {}; + OnLoopCallback on_loop = [](BT::Blackboard&) {}; + OnTreeExecutionCompletedCallback execution_complete = [](BT::NodeStatus&) {}; +}; + /** * @brief ActionServerBT class hosts a ROS Action Server that is able * to load Behavior plugins, BehaviorTree.xml files and execute them. @@ -36,60 +51,16 @@ class ActionServerBT using ActionTree = action_server_bt_msgs::action::ActionTree; using GoalHandleActionTree = rclcpp_action::ServerGoalHandle; - typedef std::function OnTreeCreatedCallback; - typedef std::function OnTreeExecutionCompletedCallback; - - /** - * @brief Constructor for ActionServerBT. - * @details This initializes a ParameterListener to read configurable options from the user and - * starts an Action Server that takes requests to execute BehaviorTrees. - * - * @param options rclcpp::NodeOptions to pass to node_ when initializing it. - * @param tree_created user defined function that is called after the tree is created. - * @param execution_complete user defined function that is called after the tree has finished. - */ - explicit ActionServerBT(const rclcpp::NodeOptions& options, OnTreeCreatedCallback tree_created, - OnTreeExecutionCompletedCallback execution_complete); - - /** - * @brief Constructor for ActionServerBT. - * @details This initializes a ParameterListener to read configurable options from the user and - * starts an Action Server that takes requests to execute BehaviorTrees with no user defined callbacks. - * - * @param options rclcpp::NodeOptions to pass to node_ when initializing it. - */ - explicit ActionServerBT(const rclcpp::NodeOptions& options) - : ActionServerBT( - options, [](BT::Tree&) {}, [](BT::NodeStatus&) {}) - { - } - - /** - * @brief Constructor for ActionServerBT. - * @details This initializes a ParameterListener to read configurable options from the user and - * starts an Action Server that takes requests to execute BehaviorTrees. - * - * @param options rclcpp::NodeOptions to pass to node_ when initializing it. - * @param tree_created user defined function that is called after the tree is created. - */ - explicit ActionServerBT(const rclcpp::NodeOptions& options, OnTreeCreatedCallback tree_created) - : ActionServerBT(options, tree_created, [](BT::NodeStatus&) {}) - { - } - /** * @brief Constructor for ActionServerBT. * @details This initializes a ParameterListener to read configurable options from the user and * starts an Action Server that takes requests to execute BehaviorTrees. * * @param options rclcpp::NodeOptions to pass to node_ when initializing it. - * @param execution_complete user defined function that is called after the tree has finished. + * @param user_callbacks optional struct of user defined function that are called + * after the tree is created, while its running and after it finishes. */ - explicit ActionServerBT(const rclcpp::NodeOptions& options, OnTreeExecutionCompletedCallback execution_complete) - : ActionServerBT( - options, [](BT::Tree&) {}, execution_complete) - { - } + explicit ActionServerBT(const rclcpp::NodeOptions& options, const UserCallbacks& user_callbacks = UserCallbacks()); /** * @brief Gets the NodeBaseInterface of node_. @@ -110,8 +81,7 @@ class ActionServerBT action_server_bt::Params params_; BT::BehaviorTreeFactory factory_; - OnTreeCreatedCallback on_tree_created_; - OnTreeExecutionCompletedCallback on_execution_complete_; + UserCallbacks user_cbs_; std::shared_ptr groot_publisher_; /** diff --git a/action_server_bt/src/action_server_bt.cpp b/action_server_bt/src/action_server_bt.cpp index c22bafb..5fb80c5 100644 --- a/action_server_bt/src/action_server_bt.cpp +++ b/action_server_bt/src/action_server_bt.cpp @@ -21,13 +21,11 @@ static const auto kLogger = rclcpp::get_logger("action_server_bt"); namespace action_server_bt { -ActionServerBT::ActionServerBT(const rclcpp::NodeOptions& options, OnTreeCreatedCallback tree_created, - OnTreeExecutionCompletedCallback execution_complete) +ActionServerBT::ActionServerBT(const rclcpp::NodeOptions& options, const UserCallbacks& user_callbacks) : node_{ std::make_shared("action_server_bt", options) } { - // initialize user callbacks for tree creation and execution complete - on_tree_created_ = tree_created; - on_execution_complete_ = execution_complete; + // initialize user callbacks + user_cbs_ = user_callbacks; // parameter setup param_listener_ = std::make_shared(node_); @@ -100,10 +98,14 @@ void ActionServerBT::execute(const std::shared_ptr goal_ha // Loop until something happens with ROS or the node completes try { - auto tree = factory_.createTree(goal->target_tree); + // No one "own" this blackboard + auto global_blackboard = BT::Blackboard::create(); + // This blackboard will be owned by "MainTree". It parent is global_blackboard + auto root_blackboard = BT::Blackboard::create(global_blackboard); + auto tree = factory_.createTree(goal->target_tree, root_blackboard); // call user defined function after the tree has been created - on_tree_created_(tree); + user_cbs_.on_create(tree); groot_publisher_.reset(); groot_publisher_ = std::make_shared(tree, params_.groot2_port); @@ -122,6 +124,7 @@ void ActionServerBT::execute(const std::shared_ptr goal_ha return; } + user_cbs_.on_loop(*global_blackboard); // tick the tree once and publish the action feedback status = tree.tickExactlyOnce(); auto feedback = std::make_shared(); @@ -145,7 +148,7 @@ void ActionServerBT::execute(const std::shared_ptr goal_ha } // call user defined execution complete function - on_execution_complete_(status); + user_cbs_.execution_complete(status); // set the node_status result to the action action_result->node_status = convert_node_status(status); diff --git a/action_server_bt/src/action_server_bt_node.cpp b/action_server_bt/src/action_server_bt_node.cpp index ac8af1e..16db338 100644 --- a/action_server_bt/src/action_server_bt_node.cpp +++ b/action_server_bt/src/action_server_bt_node.cpp @@ -20,8 +20,9 @@ int main(int argc, char* argv[]) rclcpp::NodeOptions options; // create a persistent logger and pass it into the function that will be called on Tree creation std::shared_ptr logger_cout; - auto on_tree_creation = [&logger_cout](BT::Tree& tree) { logger_cout = std::make_shared(tree); }; - auto node = std::make_shared(options, on_tree_creation); + action_server_bt::UserCallbacks user_cb; + user_cb.on_create = [&logger_cout](BT::Tree& tree) { logger_cout = std::make_shared(tree); }; + auto node = std::make_shared(options, user_cb); // TODO: This workaround is for a bug in MultiThreadedExecutor where it can deadlock when spinning without a timeout. // Deadlock is caused when Publishers or Subscribers are dynamically removed as the node is spinning.