From e54e565ff406b729705ee8455721e467ff852b5b Mon Sep 17 00:00:00 2001 From: Guillaume Doisy Date: Fri, 14 Jun 2024 03:15:39 +0200 Subject: [PATCH] Throttle and switch to DEBUG bt loop rate warning (#4430) Signed-off-by: Guillaume Doisy Co-authored-by: Guillaume Doisy --- .../include/nav2_behavior_tree/behavior_tree_engine.hpp | 6 +++++- .../include/nav2_behavior_tree/bt_action_server_impl.hpp | 2 +- nav2_behavior_tree/src/behavior_tree_engine.cpp | 8 ++++++-- 3 files changed, 12 insertions(+), 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 index 9ee903fd78..a0d86649c0 100644 --- 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 & plugin_libraries); + const std::vector & 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 index 75dfff5597..d2b120f44c 100644 --- 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::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(plugin_lib_names_); + bt_ = std::make_unique(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 index 8ed1cd4e71..70a192d719 100644 --- 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 & plugin_libraries) + const std::vector & 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)); }