Add user defined functions for Tree creation and completion (#1)

Co-authored-by: Davide Faconti <davide.faconti@gmail.com>

action_server_bt/include/action_server_bt/action_server_bt.hpp

@@ -14,6 +14,7 @@
 #include <functional>
 #include <memory>
 #include <thread>
+#include <optional>
 
 #include "action_server_bt_msgs/action/action_tree.hpp"
 #include "action_server_bt_parameters.hpp"
@@ -42,6 +43,7 @@ class ActionServerBT
    * starts an Action Server that takes requests to execute BehaviorTrees.
    *
    * @param options rclcpp::NodeOptions to pass to node_ when initializing it.
+   * after the tree is created, while its running and after it finishes.
    */
   explicit ActionServerBT(const rclcpp::NodeOptions& options);
 
@@ -51,7 +53,46 @@ class ActionServerBT
    *
    * @return A shared_ptr to the NodeBaseInterface of node_.
    */
-  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr getNodeBaseInterface();
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr nodeBaseInterface();
+
+  // name of the tree being executed
+  const std::string& currentTreeName() const;
+
+  // tree being executed, nullptr if it doesn't exist yet.
+  BT::Tree* currentTree();
+
+  // pointer to the global blackboard
+  BT::Blackboard::Ptr globalBlackboard();
+
+protected:
+  // To be overridden by the user.
+  // Callback invoked when the tree is created and before it is executed,
+  // Can be used to update the blackboard or to attach loggers.
+  virtual void onTreeCreated(BT::Tree& tree)
+  {
+  }
+
+  // To be overridden by the user.
+  // In addition to the built in mechanism to register nodes from plugins,
+  // you can use this method to register custom nodes into the factory.
+  virtual void registerNodesIntoFactory(BT::BehaviorTreeFactory& factory)
+  {
+  }
+
+  // To be overridden by the user.
+  // Callback invoked after the tickOnce.
+  // If it returns something different than std::nullopt, the tree execution will
+  // be halted and the returned value will be the optional NodeStatus.
+  virtual std::optional<BT::NodeStatus> onLoopAfterTick(BT::NodeStatus status)
+  {
+    return std::nullopt;
+  }
+
+  // To be overridden by the user.
+  // Callback invoked when the tree execution is completed
+  virtual void onTreeExecutionCompleted(BT::NodeStatus status, bool was_cancelled)
+  {
+  }
 
 private:
   rclcpp::Node::SharedPtr node_;
@@ -66,6 +107,10 @@ class ActionServerBT
   BT::BehaviorTreeFactory factory_;
   std::shared_ptr<BT::Groot2Publisher> groot_publisher_;
 
+  std::string current_tree_name_;
+  std::shared_ptr<BT::Tree> tree_;
+  BT::Blackboard::Ptr global_blackboard_;
+
   /**
    * @brief handle the goal requested: accept or reject. This implementation always accepts.
    * @param uuid Goal ID

action_server_bt/src/action_server_bt.cpp

@@ -24,12 +24,13 @@ namespace action_server_bt
 ActionServerBT::ActionServerBT(const rclcpp::NodeOptions& options)
   : node_{ std::make_shared<rclcpp::Node>("action_server_bt", options) }
 {
+  // parameter setup
   param_listener_ = std::make_shared<action_server_bt::ParamListener>(node_);
   params_ = param_listener_->get_params();
 
+  // create the action server
   const auto action_name = params_.action_name;
   RCLCPP_INFO(kLogger, "Starting Action Server: %s", action_name.c_str());
-
   action_server_ = rclcpp_action::create_server<ActionTree>(
       node_, action_name,
       [this](const rclcpp_action::GoalUUID& uuid, std::shared_ptr<const ActionTree::Goal> goal) {
@@ -40,9 +41,13 @@ ActionServerBT::ActionServerBT(const rclcpp::NodeOptions& options)
 
   // register the users Plugins and BehaviorTree.xml files into the factory
   register_behavior_trees(params_, factory_, node_);
+  registerNodesIntoFactory(factory_);
+
+  // No one "own" this blackboard
+  global_blackboard_ = BT::Blackboard::create();
 }
 
-rclcpp::node_interfaces::NodeBaseInterface::SharedPtr ActionServerBT::getNodeBaseInterface()
+rclcpp::node_interfaces::NodeBaseInterface::SharedPtr ActionServerBT::nodeBaseInterface()
 {
   return node_->get_node_base_interface();
 }
@@ -83,68 +88,116 @@ void ActionServerBT::execute(const std::shared_ptr<GoalHandleActionTree> goal_ha
   const auto goal = goal_handle->get_goal();
   BT::NodeStatus status = BT::NodeStatus::RUNNING;
   auto action_result = std::make_shared<ActionTree::Result>();
-  rclcpp::WallRate loopRate(std::chrono::milliseconds(static_cast<int>(1000.0 / params_.behavior_tick_frequency)));
 
   // Before executing check if we have new Behaviors or Subtrees to reload
   if (param_listener_->is_old(params_))
   {
     params_ = param_listener_->get_params();
     register_behavior_trees(params_, factory_, node_);
+    registerNodesIntoFactory(factory_);
   }
 
   // Loop until something happens with ROS or the node completes
   try
   {
-    auto tree = factory_.createTree(goal->target_tree);
+    // This blackboard will be owned by "MainTree". It parent is global_blackboard_
+    auto root_blackboard = BT::Blackboard::create(global_blackboard_);
+
+    tree_ = std::make_shared<BT::Tree>();
+    *tree_ = factory_.createTree(goal->target_tree, root_blackboard);
+    current_tree_name_ = goal->target_tree;
 
-    // log to console and Groot
-    BT::StdCoutLogger logger_cout(tree);
+    // call user defined function after the tree has been created
+    onTreeCreated(*tree_);
     groot_publisher_.reset();
-    groot_publisher_ = std::make_shared<BT::Groot2Publisher>(tree, params_.groot2_port);
+    groot_publisher_ = std::make_shared<BT::Groot2Publisher>(*tree_, params_.groot2_port);
+
+    // Loop until the tree is done or a cancel is requested
+    const auto period = std::chrono::milliseconds(static_cast<int>(1000.0 / params_.behavior_tick_frequency));
+    auto loop_deadline = std::chrono::steady_clock::now() + period;
+
+    // operations to be done if the tree execution is aborted, either by
+    // cancel_requested_ or by onLoopAfterTick()
+    auto stop_action = [this, &action_result](BT::NodeStatus status, const std::string& message) {
+      action_result->node_status = convert_node_status(status);
+      action_result->error_message = message;
+      RCLCPP_WARN(kLogger, action_result->error_message.c_str());
+      tree_->haltTree();
+      onTreeExecutionCompleted(status, true);
+    };
 
     while (rclcpp::ok() && status == BT::NodeStatus::RUNNING)
     {
       if (cancel_requested_)
       {
-        action_result->error_message = "Action Server canceling and halting Behavior Tree";
-        RCLCPP_ERROR(kLogger, action_result->error_message.c_str());
-        tree.haltTree();
+        stop_action(status, "Action Server canceling and halting Behavior Tree");
         goal_handle->canceled(action_result);
         return;
       }
 
       // tick the tree once and publish the action feedback
-      status = tree.tickExactlyOnce();
+      status = tree_->tickExactlyOnce();
+
+      if (const auto res = onLoopAfterTick(status); res.has_value())
+      {
+        stop_action(res.value(), "Action Server aborted by onLoopAfterTick()");
+        goal_handle->abort(action_result);
+        return;
+      }
+
       auto feedback = std::make_shared<ActionTree::Feedback>();
       feedback->node_status = convert_node_status(status);
       goal_handle->publish_feedback(feedback);
 
-      if (!loopRate.sleep())
+      const auto now = std::chrono::steady_clock::now();
+      if (now < loop_deadline)
       {
-        RCLCPP_WARN(kLogger, "Behavior Tree tick frequency %d Hz was exceeded!",
-                    static_cast<int>(1.0 / (loopRate.period().count() * 1.0e-9)));
+        tree_->sleep(loop_deadline - now);
       }
+      loop_deadline += period;
     }
   }
   catch (const std::exception& ex)
   {
-    action_result->error_message = std::string("Behavior Tree exception: ") + ex.what();
+    action_result->error_message = "Behavior Tree exception: %s", ex.what();
     RCLCPP_ERROR(kLogger, action_result->error_message.c_str());
     goal_handle->abort(action_result);
     return;
   }
 
-  // The only options for NodeStatus here are SUCCESS or FAILURE
+  // call user defined execution complete function
+  onTreeExecutionCompleted(status, false);
+
+  // set the node_status result to the action
+  action_result->node_status = convert_node_status(status);
+
+  // return success or aborted for the action result
   if (status == BT::NodeStatus::SUCCESS)
   {
-    RCLCPP_INFO(kLogger, "BT finished with status: SUCCESS");
+    RCLCPP_INFO(kLogger, "BT finished with status: %s", BT::toStr(status).c_str());
     goal_handle->succeed(action_result);
   }
   else
   {
-    action_result->error_message = "Behavior Tree failed during execution with status: FAILURE";
+    action_result->error_message = "Behavior Tree failed during execution with status: %s", BT::toStr(status);
     RCLCPP_ERROR(kLogger, action_result->error_message.c_str());
     goal_handle->abort(action_result);
   }
 }
+
+const std::string& ActionServerBT::currentTreeName() const
+{
+  return current_tree_name_;
+}
+
+BT::Tree* ActionServerBT::currentTree()
+{
+  return tree_ ? tree_.get() : nullptr;
+}
+
+BT::Blackboard::Ptr ActionServerBT::globalBlackboard()
+{
+  return global_blackboard_;
+}
+
 }  // namespace action_server_bt

action_server_bt/src/action_server_bt_node.cpp

@@ -13,19 +13,44 @@
 
 #include <action_server_bt/action_server_bt.hpp>
 
+// Example that shows how to customize ActionServerBT.
+// Here, we simply add an extra logger
+class MyActionServer : public action_server_bt::ActionServerBT
+{
+public:
+  MyActionServer(const rclcpp::NodeOptions& options) : ActionServerBT(options)
+  {
+  }
+
+  void onTreeCreated(BT::Tree& tree) override
+  {
+    logger_cout_ = std::make_shared<BT::StdCoutLogger>(tree);
+  }
+
+  void onTreeExecutionCompleted(BT::NodeStatus status, bool was_cancelled) override
+  {
+    // NOT really needed, even if logger_cout_ may contain a dangling pointer of the tree
+    // at this point
+    logger_cout_.reset();
+  }
+
+private:
+  std::shared_ptr<BT::StdCoutLogger> logger_cout_;
+};
+
 int main(int argc, char* argv[])
 {
   rclcpp::init(argc, argv);
 
   rclcpp::NodeOptions options;
-  auto node = std::make_shared<action_server_bt::ActionServerBT>(options);
+  auto action_server = std::make_shared<MyActionServer>(options);
 
   // 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.
   rclcpp::executors::MultiThreadedExecutor exec(rclcpp::ExecutorOptions(), 0, false, std::chrono::milliseconds(250));
-  exec.add_node(node->getNodeBaseInterface());
+  exec.add_node(action_server->nodeBaseInterface());
   exec.spin();
-  exec.remove_node(node->getNodeBaseInterface());
+  exec.remove_node(action_server->nodeBaseInterface());
 
   rclcpp::shutdown();
 }

action_server_bt/src/bt_utils.cpp

@@ -34,6 +34,8 @@ action_server_bt_msgs::msg::NodeStatus convert_node_status(BT::NodeStatus& statu
       action_status.status = action_server_bt_msgs::msg::NodeStatus::FAILURE;
     case BT::NodeStatus::IDLE:
       action_status.status = action_server_bt_msgs::msg::NodeStatus::IDLE;
+    case BT::NodeStatus::SKIPPED:
+      action_status.status = action_server_bt_msgs::msg::NodeStatus::SKIPPED;
   }
 
   return action_status;

action_server_bt_msgs/action/ActionTree.action

@@ -3,6 +3,7 @@ string target_tree
 ---
 # Result
 string error_message
+NodeStatus node_status
 ---
 # Feedback
 NodeStatus node_status

action_server_bt_msgs/msgs/NodeStatus.msg

@@ -3,5 +3,6 @@ uint8 IDLE = 0
 uint8 RUNNING = 1
 uint8 SUCCESS = 2
 uint8 FAILURE = 3
+uint8 SKIPPED = 4
 
 uint8 status