nav2_controller: add loop rate log (ros-navigation#4228)

* added current loop rate printout Signed-off-by: ARK3r <kermani.areza@gmail.com> * remove empty line Signed-off-by: ARK3r <kermani.areza@gmail.com> --------- Signed-off-by: ARK3r <kermani.areza@gmail.com>
Marc-Morcos · Jul 4, 2024 · f505d30 · f505d30
1 parent bb4f48f
commit f505d30
Showing 1 changed file with 4 additions and 2 deletions.
diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp
@@ -491,10 +491,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) {