Fix the skipped cycles of controllers running at different rate (#2557)

Author: saikishor

controller_manager/src/controller_manager.cpp

@@ -2964,16 +2964,16 @@ controller_interface::return_type ControllerManager::update(
       const auto controller_actual_period =
         (current_time - *loaded_controller.last_update_cycle_time);
 
-      /// @note The factor 0.99 is used to avoid the controllers skipping update cycles due to the
-      /// jitter in the system sleep cycles.
-      // For instance, A controller running at 50 Hz and the CM running at 100Hz, then when we have
-      // an update cycle at 0.019s (ideally, the controller should only trigger >= 0.02s), if we
-      // wait for next cycle, then trigger will happen at ~0.029 sec and this is creating an issue
-      // to keep up with the controller update rate (see issue #1769).
+      const double error_now =
+        std::abs((controller_actual_period.seconds() * controller_update_rate) - 1.0);
+      const double error_if_skipped = std::abs(
+        ((controller_actual_period.seconds() + (1.0 / static_cast<double>(update_rate_))) *
+         controller_update_rate) -
+        1.0);
       const bool controller_go =
         run_controller_at_cm_rate ||
         (time == rclcpp::Time(0, 0, this->get_trigger_clock()->get_clock_type())) ||
-        (controller_actual_period.seconds() * controller_update_rate >= 0.99) || first_update_cycle;
+        (error_now <= error_if_skipped) || first_update_cycle;
 
       RCLCPP_DEBUG(
         get_logger(), "update_loop_counter: '%d ' controller_go: '%s ' controller_name: '%s '",

controller_manager/test/test_controller_manager.cpp

@@ -920,11 +920,11 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd
     EXPECT_THAT(
       cm_->get_loaded_controllers()[0].periodicity_statistics->get_average(),
       testing::AllOf(
-        testing::Ge(0.90 * cm_->get_update_rate()), testing::Lt((1.05 * cm_->get_update_rate()))));
+        testing::Ge(0.9 * cm_->get_update_rate()), testing::Lt((1.05 * cm_->get_update_rate()))));
     EXPECT_THAT(
       cm_->get_loaded_controllers()[0].periodicity_statistics->get_min(),
       testing::AllOf(
-        testing::Ge(0.70 * cm_->get_update_rate()), testing::Lt((1.2 * cm_->get_update_rate()))));
+        testing::Ge(0.5 * cm_->get_update_rate()), testing::Lt((1.2 * cm_->get_update_rate()))));
     EXPECT_THAT(
       cm_->get_loaded_controllers()[0].periodicity_statistics->get_max(),
       testing::AllOf(
@@ -1031,12 +1031,14 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate)
   const auto cm_update_rate = cm_->get_update_rate();
   const auto controller_update_rate = test_controller->get_update_rate();
   const double controller_period = 1.0 / controller_update_rate;
+  const double exp_controller_period =
+    std::round((static_cast<double>(cm_update_rate) / controller_update_rate) - 0.01) /
+    cm_update_rate;
 
   const auto initial_counter = test_controller->internal_counter;
   // don't start with zero to check if the period is correct if controller is activated anytime
   rclcpp::Time time = time_;
-  const auto exp_periodicity =
-    cm_update_rate / std::ceil(static_cast<double>(cm_update_rate) / controller_update_rate);
+  const auto exp_periodicity = 1.0 / exp_controller_period;
   for (size_t update_counter = 0; update_counter <= 10 * cm_update_rate; ++update_counter)
   {
     rclcpp::Time old_time = time;
@@ -1053,10 +1055,11 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate)
       EXPECT_THAT(
         test_controller->update_period_.seconds(),
         testing::AllOf(
-          testing::Gt(0.99 * controller_period),
-          testing::Lt((1.2 * controller_period) + PERIOD.seconds())))
+          testing::Gt(0.99 * exp_controller_period),
+          testing::Lt((1.2 * exp_controller_period) + PERIOD.seconds())))
         << "update_counter: " << update_counter
         << " desired controller period: " << controller_period
+        << " expected controller period: " << exp_controller_period
         << " actual controller period: " << test_controller->update_period_.seconds();
     }
     else
@@ -1092,10 +1095,10 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate)
         << "The first update is not counted in periodicity statistics";
       EXPECT_THAT(
         cm_->get_loaded_controllers()[0].periodicity_statistics->get_average(),
-        testing::AllOf(testing::Ge(0.92 * exp_periodicity), testing::Lt((1.05 * exp_periodicity))));
+        testing::AllOf(testing::Ge(0.9 * exp_periodicity), testing::Lt((1.05 * exp_periodicity))));
       EXPECT_THAT(
         cm_->get_loaded_controllers()[0].periodicity_statistics->get_min(),
-        testing::AllOf(testing::Ge(0.75 * exp_periodicity), testing::Lt((1.2 * exp_periodicity))));
+        testing::AllOf(testing::Ge(0.5 * exp_periodicity), testing::Lt((1.2 * exp_periodicity))));
       EXPECT_THAT(
         cm_->get_loaded_controllers()[0].periodicity_statistics->get_max(),
         testing::AllOf(testing::Ge(0.75 * exp_periodicity), testing::Lt((2.0 * exp_periodicity))));
@@ -1178,12 +1181,14 @@ TEST_F(TestAsyncControllerUpdateRates, check_the_async_controller_update_rate_an
   const auto cm_update_rate = cm_->get_update_rate();
   const auto controller_update_rate = test_controller->get_update_rate();
   const double controller_period = 1.0 / controller_update_rate;
+  const double exp_controller_period =
+    std::round((static_cast<double>(cm_update_rate) / controller_update_rate) - 0.01) /
+    cm_update_rate;
 
   const auto initial_counter = test_controller->internal_counter;
   // don't start with zero to check if the period is correct if controller is activated anytime
   rclcpp::Time time = time_;
-  const auto exp_periodicity =
-    cm_update_rate / std::ceil(static_cast<double>(cm_update_rate) / controller_update_rate);
+  const auto exp_periodicity = 1.0 / exp_controller_period;
   for (size_t update_counter = 0; update_counter <= 10 * cm_update_rate; ++update_counter)
   {
     rclcpp::Time old_time = time;
@@ -1200,10 +1205,11 @@ TEST_F(TestAsyncControllerUpdateRates, check_the_async_controller_update_rate_an
       EXPECT_THAT(
         test_controller->update_period_.seconds(),
         testing::AllOf(
-          testing::Gt(0.99 * controller_period),
-          testing::Lt((1.05 * controller_period) + PERIOD.seconds())))
+          testing::Gt(0.99 * exp_controller_period),
+          testing::Lt((1.05 * exp_controller_period) + PERIOD.seconds())))
         << "update_counter: " << update_counter
         << " desired controller period: " << controller_period
+        << " expected controller period: " << exp_controller_period
         << " actual controller period: " << test_controller->update_period_.seconds();
     }
     // else