From e9f21c4f8669da1a84ad0cafcf9700ae67956430 Mon Sep 17 00:00:00 2001
From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com>
Date: Sat, 28 Oct 2023 15:29:05 -0700
Subject: [PATCH] Log if BT rate is exceeded (#3909) (#3913)

(cherry picked from commit a11cdd80665238628be9eef4b439067d4b675b7b)

Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>
---
 nav2_behavior_tree/src/behavior_tree_engine.cpp | 7 ++++++-
 1 file changed, 6 insertions(+), 1 deletion(-)

diff --git a/nav2_behavior_tree/src/behavior_tree_engine.cpp b/nav2_behavior_tree/src/behavior_tree_engine.cpp
index 7bfc003cf1c..a5ee96af5e1 100644
--- a/nav2_behavior_tree/src/behavior_tree_engine.cpp
+++ b/nav2_behavior_tree/src/behavior_tree_engine.cpp
@@ -55,7 +55,12 @@ BehaviorTreeEngine::run(
 
       onLoop();
 
-      loopRate.sleep();
+      if (!loopRate.sleep()) {
+        RCLCPP_WARN(
+          rclcpp::get_logger("BehaviorTreeEngine"),
+          "Behavior Tree tick rate %0.2f was exceeded!",
+          1.0 / (loopRate.period().count() * 1.0e-9));
+      }
     }
   } catch (const std::exception & ex) {
     RCLCPP_ERROR(