Add support for custom sleep override function in Tree class

Signed-off-by: Tony Najjar <tony.najjar@dexory.com>

Author: tonynajjar

include/behaviortree_cpp/bt_factory.h

@@ -122,6 +122,13 @@ class Tree
 
   [[nodiscard]] TreeNode* rootNode() const;
 
+  /// Function signature for a custom sleep override.
+  /// The function receives the desired sleep duration and a reference to the
+  /// WakeUpSignal so it can check for early wake-up.
+  /// Return true if woken up before the timeout, false if the full duration elapsed.
+  using SleepOverrideFunc =
+    std::function<bool(std::chrono::system_clock::duration, WakeUpSignal&)>;
+
   /**
     * @brief Sleep for a certain amount of time. This sleep could be interrupted by the methods
     * TreeNode::emitWakeUpSignal() or Tree::emitWakeUpSignal()
@@ -137,6 +144,14 @@ class Tree
    */
   void emitWakeUpSignal();
 
+  /**
+   * @brief Set a custom sleep override function. When set, Tree::sleep()
+   * will delegate to this function instead of using the default
+   * WakeUpSignal::waitFor(). This allows integrating with external clock
+   * sources (e.g. ROS sim time) while preserving wake-up signal support.
+   */
+  void setSleepOverride(SleepOverrideFunc func);
+
   ~Tree();
 
   /// Tick the root of the tree once, even if a node invoked
@@ -196,6 +211,7 @@ class Tree
   friend class BehaviorTreeFactory;
 
   std::shared_ptr<WakeUpSignal> wake_up_;
+  SleepOverrideFunc sleep_override_;
 
   enum TickOption
   {

src/bt_factory.cpp

@@ -655,6 +655,10 @@ TreeNode* Tree::rootNode() const
 
 bool Tree::sleep(std::chrono::system_clock::duration timeout)
 {
+  if(sleep_override_)
+  {
+    return sleep_override_(timeout, *wake_up_);
+  }
   return wake_up_->waitFor(
       std::chrono::duration_cast<std::chrono::milliseconds>(timeout));
 }
@@ -664,6 +668,11 @@ void Tree::emitWakeUpSignal()
   wake_up_->emitSignal();
 }
 
+void Tree::setSleepOverride(SleepOverrideFunc func)
+{
+  sleep_override_ = std::move(func);
+}
+
 Tree::~Tree()
 {
   haltTree();