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

ARK3r · Marc-Morcos · commit f505d3061726 · 2024-07-04T12:04:16.000-04:00
* added current loop rate printout

Signed-off-by: ARK3r &lt;kermani.areza@gmail.com&gt;

* remove empty line

Signed-off-by: ARK3r &lt;kermani.areza@gmail.com&gt;

---------

Signed-off-by: ARK3r &lt;kermani.areza@gmail.com&gt;
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) {

Original file line number	Diff line number	Diff line change
`@@ -491,10 +491,12 @@ void ControllerServer::computeControl()`
`491`	`491`	`break;`
`492`	`492`	`}`
`493`	`493`
	`494`	`+ auto cycle_duration = this->now() - start_time;`
`494`	`495`	`if (!loop_rate.sleep()) {`
`495`	`496`	`RCLCPP_WARN(`
`496`		`- get_logger(), "Control loop missed its desired rate of %.4fHz",`
`497`		`- controller_frequency_);`
	`497`	`+ get_logger(),`
	`498`	`+ "Control loop missed its desired rate of %.4f Hz. Current loop rate is %.4f Hz.",`
	`499`	`+ controller_frequency_, 1 / cycle_duration.seconds());`
`498`	`500`	`}`
`499`	`501`	`}`
`500`	`502`	`} catch (nav2_core::InvalidController & e) {`