From 0087d82b8e5f5d78616f15905153a780d62f18cb Mon Sep 17 00:00:00 2001
From: Boone Adkins <boone.adkins@gmail.com>
Date: Tue, 3 Dec 2024 20:21:11 -0800
Subject: [PATCH] Adds onLoopBeforeTick callback to BT::TreeExecutionServer.

---
 .../tree_execution_server.hpp                 | 15 +++++++++
 .../src/tree_execution_server.cpp             | 31 ++++++++++---------
 2 files changed, 32 insertions(+), 14 deletions(-)

diff --git a/behaviortree_ros2/include/behaviortree_ros2/tree_execution_server.hpp b/behaviortree_ros2/include/behaviortree_ros2/tree_execution_server.hpp
index f472cfd..dc82877 100644
--- a/behaviortree_ros2/include/behaviortree_ros2/tree_execution_server.hpp
+++ b/behaviortree_ros2/include/behaviortree_ros2/tree_execution_server.hpp
@@ -104,6 +104,21 @@ class TreeExecutionServer
   virtual void registerNodesIntoFactory(BT::BehaviorTreeFactory& factory)
   {}
 
+  /**
+   * @brief onLoopBeforeTick invoked at each loop, before tree.tickOnce().
+   * It can be overridden to avoid ticking the tree without blocking the
+   * rest of the execute loop.
+   *
+   * Consider whether it's better to inject a preTickCallback to the root node
+   * or another node of the behavior tree.
+   *
+   * @return False to skip the tick.
+   */
+  virtual bool onLoopBeforeTick()
+  {
+    return true;
+  }
+
   /**
    * @brief onLoopAfterTick invoked at each loop, after tree.tickOnce().
    * If it returns a valid NodeStatus, the tree will stop and return that status.
diff --git a/behaviortree_ros2/src/tree_execution_server.cpp b/behaviortree_ros2/src/tree_execution_server.cpp
index f712a52..34bbaff 100644
--- a/behaviortree_ros2/src/tree_execution_server.cpp
+++ b/behaviortree_ros2/src/tree_execution_server.cpp
@@ -222,21 +222,24 @@ void TreeExecutionServer::execute(
         return;
       }
 
-      // tick the tree once and publish the action feedback
-      status = p_->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;
-      }
-
-      if(const auto res = onLoopFeedback(); res.has_value())
+      if(onLoopBeforeTick())
       {
-        auto feedback = std::make_shared<ExecuteTree::Feedback>();
-        feedback->message = res.value();
-        goal_handle->publish_feedback(feedback);
+        // tick the tree once and publish the action feedback
+        status = p_->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;
+        }
+
+        if(const auto res = onLoopFeedback(); res.has_value())
+        {
+          auto feedback = std::make_shared<ExecuteTree::Feedback>();
+          feedback->message = res.value();
+          goal_handle->publish_feedback(feedback);
+        }
       }
 
       const auto now = std::chrono::steady_clock::now();