From 2f7c176c5606ddd25f7378d148ca96d3a28744b0 Mon Sep 17 00:00:00 2001 From: ARK3r Date: Thu, 28 Mar 2024 14:13:09 -0400 Subject: [PATCH 1/2] added current loop rate printout Signed-off-by: ARK3r --- nav2_controller/src/controller_server.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index fa25e0b137..e96ba78cda 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -452,6 +452,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; @@ -479,10 +481,13 @@ 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) { From a269a7388133a932089151c49cea0fe081b0b541 Mon Sep 17 00:00:00 2001 From: ARK3r Date: Thu, 28 Mar 2024 14:21:06 -0400 Subject: [PATCH 2/2] remove empty line Signed-off-by: ARK3r --- nav2_controller/src/controller_server.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index e96ba78cda..17c3b510dc 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -482,7 +482,6 @@ void ControllerServer::computeControl() } auto cycle_duration = this->now() - start_time; - if (!loop_rate.sleep()) { RCLCPP_WARN( get_logger(),