Throttle and switch to DEBUG bt loop rate warning (#4430)

Signed-off-by: Guillaume Doisy <[email protected]> Co-authored-by: Guillaume Doisy <[email protected]>
ros-navigation · Jun 14, 2024 · 635880d · 635880d
1 parent 29ba4b1
commit 635880d
Show file tree

Hide file tree

Showing 3 changed files with 12 additions and 4 deletions.
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp
@@ -47,7 +47,8 @@ class BehaviorTreeEngine
    * @param plugin_libraries vector of BT plugin library names to load
    */
   explicit BehaviorTreeEngine(
-    const std::vector<std::string> & plugin_libraries);
+    const std::vector<std::string> & plugin_libraries,
+    rclcpp::Node::SharedPtr node);
   virtual ~BehaviorTreeEngine() {}
 
   /**
@@ -93,6 +94,9 @@ class BehaviorTreeEngine
 protected:
   // The factory that will be used to dynamically construct the behavior tree
   BT::BehaviorTreeFactory factory_;
+
+  // Clock
+  rclcpp::Clock::SharedPtr clock_;
 };
 
 }  // namespace nav2_behavior_tree

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp
@@ -173,7 +173,7 @@ bool BtActionServer<ActionT>::on_configure()
   error_code_names_ = node->get_parameter("error_code_names").as_string_array();
 
   // Create the class that registers our custom nodes and executes the BT
-  bt_ = std::make_unique<nav2_behavior_tree::BehaviorTreeEngine>(plugin_lib_names_);
+  bt_ = std::make_unique<nav2_behavior_tree::BehaviorTreeEngine>(plugin_lib_names_, client_node_);
 
   // Create the blackboard that will be shared by all of the nodes in the tree
   blackboard_ = BT::Blackboard::create();

diff --git a/nav2_behavior_tree/src/behavior_tree_engine.cpp b/nav2_behavior_tree/src/behavior_tree_engine.cpp
@@ -26,13 +26,16 @@ namespace nav2_behavior_tree
 {
 
 BehaviorTreeEngine::BehaviorTreeEngine(
-  const std::vector<std::string> & plugin_libraries)
+  const std::vector<std::string> & plugin_libraries, rclcpp::Node::SharedPtr node)
 {
   BT::SharedLibrary loader;
   for (const auto & p : plugin_libraries) {
     factory_.registerFromPlugin(loader.getOSName(p));
   }
 
+  // clock for throttled debug log
+  clock_ = node->get_clock();
+
   // FIXME: the next two line are needed for back-compatibility with BT.CPP 3.8.x
   // Note that the can be removed, once we migrate from BT.CPP 4.5.x to 4.6+
   BT::ReactiveSequence::EnableException(false);
@@ -62,8 +65,9 @@ BehaviorTreeEngine::run(
       onLoop();
 
       if (!loopRate.sleep()) {
-        RCLCPP_WARN(
+        RCLCPP_DEBUG_THROTTLE(
           rclcpp::get_logger("BehaviorTreeEngine"),
+          *clock_, 1000,
           "Behavior Tree tick rate %0.2f was exceeded!",
           1.0 / (loopRate.period().count() * 1.0e-9));
       }