From 596dfb60f511fa69b0e3f2b9218b017062d39bdf Mon Sep 17 00:00:00 2001 From: Reza Kermani Date: Fri, 8 Mar 2024 13:25:25 -0500 Subject: [PATCH] nav2_controller: add loop rate log (#4171) * update smac_planner README Signed-off-by: ARK3r * added current controller loop rate logging Signed-off-by: ARK3r * linting Signed-off-by: ARK3r * uncrustify lint Signed-off-by: ARK3r * Update nav2_controller/src/controller_server.cpp Signed-off-by: Steve Macenski * Update nav2_controller/src/controller_server.cpp Signed-off-by: Steve Macenski * Update nav2_controller/src/controller_server.cpp Signed-off-by: Steve Macenski --------- Signed-off-by: ARK3r Signed-off-by: Steve Macenski Co-authored-by: Steve Macenski Signed-off-by: enricosutera --- nav2_controller/src/controller_server.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index fa25e0b137..85aae171cc 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -419,6 +419,7 @@ void ControllerServer::computeControl() { std::lock_guard lock(dynamic_params_lock_); + auto start_time = this->now(); RCLCPP_INFO(get_logger(), "Received a goal, begin computing control effort."); try { @@ -479,10 +480,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) {