From f8413bd630b32ed9e5d74a9d09f903f8c60cd565 Mon Sep 17 00:00:00 2001
From: Steve Macenski <stevenmacenski@gmail.com>
Date: Sat, 28 Oct 2023 15:24:38 -0700
Subject: [PATCH] Log if BT rate is exceeded (#3909)

(cherry picked from commit a11cdd80665238628be9eef4b439067d4b675b7b)
---
 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 7bfc003cf1..a5ee96af5e 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(