switch to UserCB struct

action_server_bt/include/action_server_bt/action_server_bt.hpp

@@ -26,6 +26,21 @@
 namespace action_server_bt
 {
 
+typedef std::function<void(BT::Tree&)> OnTreeCreatedCallback;
+typedef std::function<void(BT::Blackboard&)> OnLoopCallback;
+typedef std::function<void(BT::NodeStatus&)> 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<ActionTree>;
 
-  typedef std::function<void(BT::Tree&)> OnTreeCreatedCallback;
-  typedef std::function<void(BT::NodeStatus&)> 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<BT::Groot2Publisher> groot_publisher_;
 
   /**

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<rclcpp::Node>("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<action_server_bt::ParamListener>(node_);
@@ -100,10 +98,14 @@ void ActionServerBT::execute(const std::shared_ptr<GoalHandleActionTree> 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<BT::Groot2Publisher>(tree, params_.groot2_port);
 
@@ -122,6 +124,7 @@ void ActionServerBT::execute(const std::shared_ptr<GoalHandleActionTree> 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<ActionTree::Feedback>();
@@ -145,7 +148,7 @@ void ActionServerBT::execute(const std::shared_ptr<GoalHandleActionTree> 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);

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<BT::StdCoutLogger> logger_cout;
-  auto on_tree_creation = [&logger_cout](BT::Tree& tree) { logger_cout = std::make_shared<BT::StdCoutLogger>(tree); };
-  auto node = std::make_shared<action_server_bt::ActionServerBT>(options, on_tree_creation);
+  action_server_bt::UserCallbacks user_cb;
+  user_cb.on_create = [&logger_cout](BT::Tree& tree) { logger_cout = std::make_shared<BT::StdCoutLogger>(tree); };
+  auto node = std::make_shared<action_server_bt::ActionServerBT>(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.