Log if BT rate is exceeded (#3909) (#3913)

(cherry picked from commit a11cdd8) Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>
ros-navigation · Oct 28, 2023 · 6377c72 · 6377c72
1 parent afe56f7
commit 6377c72
Showing 1 changed file with 6 additions and 1 deletion.
diff --git 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(