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

* update smac_planner README

Signed-off-by: ARK3r <[email protected]>

* added current controller loop rate logging

Signed-off-by: ARK3r <[email protected]>

* linting

Signed-off-by: ARK3r <[email protected]>

* uncrustify lint

Signed-off-by: ARK3r <[email protected]>

* Update nav2_controller/src/controller_server.cpp

Signed-off-by: Steve Macenski <[email protected]>

* Update nav2_controller/src/controller_server.cpp

Signed-off-by: Steve Macenski <[email protected]>

* Update nav2_controller/src/controller_server.cpp

Signed-off-by: Steve Macenski <[email protected]>

---------

Signed-off-by: ARK3r <[email protected]>
Signed-off-by: Steve Macenski <[email protected]>
Co-authored-by: Steve Macenski <[email protected]>

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) {