Log if BT rate is exceeded (ros-navigation#3909) (ros-navigation#3913)

(cherry picked from commit a11cdd8) Co-authored-by: Steve Macenski <[email protected]>
EnjoyRobotics · Nov 16, 2023 · fee5e7b · fee5e7b
1 parent 7af5c96
commit fee5e7b
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(