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