Add the overrun detection and sleep to use complete steady clock appr…

…oach
ros-controls · Feb 10, 2025 · 6c6cd79 · 6c6cd79
1 parent 032715f
commit 6c6cd79
Showing 1 changed file with 22 additions and 5 deletions.
diff --git a/controller_manager/src/ros2_control_node.cpp b/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->get_trigger_clock()->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->get_trigger_clock()->now() - period;
+      rclcpp::Time previous_time = cm->get_trigger_clock()->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->get_trigger_clock()->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);
         }
       }