Fix wrong CM statistics + add fix for overruns

controller_manager/src/ros2_control_node.cpp

@@ -124,12 +124,12 @@ int main(int argc, char ** argv)
 
       // for calculating sleep time
       auto const period = std::chrono::nanoseconds(1'000'000'000 / cm->get_update_rate());
-      auto const cm_now = std::chrono::nanoseconds(cm->now().nanoseconds());
-      std::chrono::time_point<std::chrono::system_clock, std::chrono::nanoseconds>
-        next_iteration_time{cm_now - period};
 
       // for calculating the measured period of the loop
-      rclcpp::Time previous_time = cm->now() - period;
+      rclcpp::Time previous_time = cm->now();
+      std::this_thread::sleep_for(period);
+
+      std::chrono::steady_clock::time_point next_iteration_time{std::chrono::steady_clock::now()};
 
       while (rclcpp::ok())
       {
@@ -144,13 +144,30 @@ int main(int argc, char ** argv)
         cm->write(cm->now(), measured_period);
 
         // wait until we hit the end of the period
-        next_iteration_time += period;
         if (use_sim_time)
         {
           cm->get_clock()->sleep_until(current_time + period);
         }
         else
         {
+          next_iteration_time += period;
+          const auto time_now = std::chrono::steady_clock::now();
+          if (next_iteration_time < time_now)
+          {
+            const double time_diff =
+              static_cast<double>(
+                std::chrono::duration_cast<std::chrono::nanoseconds>(time_now - next_iteration_time)
+                  .count()) /
+              1.e6;
+            const double cm_period = 1.e3 / static_cast<double>(cm->get_update_rate());
+            const int overrun_count = static_cast<int>(std::ceil(time_diff / cm_period));
+            RCLCPP_WARN(
+              cm->get_logger(),
+              "Overrun detected! The controller manager missed its desired rate of %d Hz. The loop "
+              "took %f ms (missed cycles : %d).",
+              cm->get_update_rate(), time_diff + cm_period, overrun_count + 1);
+            next_iteration_time += (overrun_count * period);
+          }
           std::this_thread::sleep_until(next_iteration_time);
         }
       }