nav2_controller: add loop rate log (#4171)

* update smac_planner README Signed-off-by: ARK3r <kermani.areza@gmail.com> * added current controller loop rate logging Signed-off-by: ARK3r <kermani.areza@gmail.com> * linting Signed-off-by: ARK3r <kermani.areza@gmail.com> * uncrustify lint Signed-off-by: ARK3r <kermani.areza@gmail.com> * Update nav2_controller/src/controller_server.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update nav2_controller/src/controller_server.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update nav2_controller/src/controller_server.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: ARK3r <kermani.areza@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

Author: ARK3r

nav2_controller/src/controller_server.cpp

@@ -419,6 +419,7 @@ void ControllerServer::computeControl()
 {
  std::lock_guard<std::mutex> 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) {