From a73607ddb939fd67ef41dd99aee67062ec047284 Mon Sep 17 00:00:00 2001 From: Reza Kermani Date: Thu, 28 Mar 2024 14:23:29 -0400 Subject: [PATCH] nav2_controller: add loop rate log (#4228) * added current loop rate printout Signed-off-by: ARK3r * remove empty line Signed-off-by: ARK3r --------- Signed-off-by: ARK3r --- nav2_controller/src/controller_server.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index c6908bb5115..447d8534819 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -472,6 +472,8 @@ void ControllerServer::computeControl() last_valid_cmd_time_ = now(); rclcpp::WallRate loop_rate(controller_frequency_); while (rclcpp::ok()) { + auto start_time = this->now(); + if (action_server_ == nullptr || !action_server_->is_server_active()) { RCLCPP_DEBUG(get_logger(), "Action server unavailable or inactive. Stopping."); return; @@ -499,10 +501,12 @@ void ControllerServer::computeControl() break; } + auto cycle_duration = this->now() - start_time; if (!loop_rate.sleep()) { RCLCPP_WARN( - get_logger(), "Control loop missed its desired rate of %.4fHz", - controller_frequency_); + get_logger(), + "Control loop missed its desired rate of %.4f Hz. Current loop rate is %.4f Hz.", + controller_frequency_, 1 / cycle_duration.seconds()); } } } catch (nav2_core::InvalidController & e) {