Proper controller update rate (#1105)

controller_manager/include/controller_manager/controller_manager.hpp

@@ -122,6 +122,7 @@ class ControllerManager : public rclcpp::Node
     controller_spec.c = controller;
     controller_spec.info.name = controller_name;
     controller_spec.info.type = controller_type;
+    controller_spec.next_update_cycle_time = std::make_shared<rclcpp::Time>(0);
     return add_controller_impl(controller_spec);
   }
 

controller_manager/include/controller_manager/controller_spec.hpp

@@ -20,6 +20,7 @@
 #define CONTROLLER_MANAGER__CONTROLLER_SPEC_HPP_
 
 #include <map>
+#include <memory>
 #include <string>
 #include <vector>
 #include "controller_interface/controller_interface.hpp"
@@ -37,6 +38,7 @@ struct ControllerSpec
 {
   hardware_interface::ControllerInfo info;
   controller_interface::ControllerInterfaceBaseSharedPtr c;
+  std::shared_ptr<rclcpp::Time> next_update_cycle_time;
 };
 
 }  // namespace controller_manager

controller_manager/src/controller_manager.cpp

@@ -565,6 +565,7 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c
   controller_spec.c = controller;
   controller_spec.info.name = controller_name;
   controller_spec.info.type = controller_type;
+  controller_spec.next_update_cycle_time = std::make_shared<rclcpp::Time>(0);
 
   return add_controller_impl(controller_spec);
 }
@@ -740,16 +741,13 @@ controller_interface::return_type ControllerManager::configure_controller(
   }
   else if (controller_update_rate != 0 && cm_update_rate % controller_update_rate != 0)
   {
-    // NOTE: The following computation is done to compute the approx controller update that can be
-    // achieved w.r.t to the CM's update rate. This is done this way to take into account the
-    // unsigned integer division.
-    const auto act_ctrl_update_rate = cm_update_rate / (cm_update_rate / controller_update_rate);
     RCLCPP_WARN(
       get_logger(),
-      "The controller : %s update rate : %d Hz is not a perfect divisor of the controller "
-      "manager's update rate : %d Hz!. The controller will be updated with nearest divisor's "
-      "update rate which is : %d Hz.",
-      controller_name.c_str(), controller_update_rate, cm_update_rate, act_ctrl_update_rate);
+      "The controller : %s update cycles won't be triggered at a constant period : %f sec, as the "
+      "controller's update rate : %d Hz is not a perfect divisor of the controller manager's "
+      "update rate : %d Hz!.",
+      controller_name.c_str(), 1.0 / controller_update_rate, controller_update_rate,
+      cm_update_rate);
   }
 
   // CHAINABLE CONTROLLERS: get reference interfaces from chainable controllers
@@ -1458,6 +1456,8 @@ void ControllerManager::activate_controllers(
     }
     auto controller = found_it->c;
     auto controller_name = found_it->info.name;
+    // reset the next update cycle time for newly activated controllers
+    *found_it->next_update_cycle_time = rclcpp::Time(0);
 
     bool assignment_successful = true;
     // assign command interfaces to the controller
@@ -2030,30 +2030,36 @@ controller_interface::return_type ControllerManager::update(
   ++update_loop_counter_;
   update_loop_counter_ %= update_rate_;
 
-  for (auto loaded_controller : rt_controller_list)
+  for (const auto & loaded_controller : rt_controller_list)
   {
     // TODO(v-lopez) we could cache this information
     // https://github.com/ros-controls/ros2_control/issues/153
     if (!loaded_controller.c->is_async() && is_controller_active(*loaded_controller.c))
     {
       const auto controller_update_rate = loaded_controller.c->get_update_rate();
-      const auto controller_update_factor =
-        (controller_update_rate == 0) || (controller_update_rate >= update_rate_)
-          ? 1u
-          : update_rate_ / controller_update_rate;
+      const bool run_controller_at_cm_rate =
+        (controller_update_rate == 0) || (controller_update_rate >= update_rate_);
+      const auto controller_period =
+        run_controller_at_cm_rate ? period
+                                  : rclcpp::Duration::from_seconds((1.0 / controller_update_rate));
+
+      bool controller_go = (time == rclcpp::Time(0)) ||
+                           (time.seconds() >= loaded_controller.next_update_cycle_time->seconds());
 
-      bool controller_go = ((update_loop_counter_ % controller_update_factor) == 0);
       RCLCPP_DEBUG(
         get_logger(), "update_loop_counter: '%d ' controller_go: '%s ' controller_name: '%s '",
         update_loop_counter_, controller_go ? "True" : "False",
         loaded_controller.info.name.c_str());
 
       if (controller_go)
       {
-        auto controller_ret = loaded_controller.c->update(
-          time, (controller_update_factor != 1u)
-                  ? rclcpp::Duration::from_seconds(1.0 / controller_update_rate)
-                  : period);
+        auto controller_ret = loaded_controller.c->update(time, controller_period);
+
+        if (*loaded_controller.next_update_cycle_time == rclcpp::Time(0))
+        {
+          *loaded_controller.next_update_cycle_time = time;
+        }
+        *loaded_controller.next_update_cycle_time += controller_period;
 
         if (controller_ret != controller_interface::return_type::OK)
         {

controller_manager/test/test_controller_manager.cpp

@@ -423,23 +423,28 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate)
   EXPECT_EQ(test_controller->get_update_rate(), ctrl_update_rate);
   const auto cm_update_rate = cm_->get_update_rate();
   const auto controller_update_rate = test_controller->get_update_rate();
-  const auto controller_factor = (cm_update_rate / controller_update_rate);
-  const auto expected_controller_update_rate =
-    static_cast<unsigned int>(std::round(cm_update_rate / static_cast<double>(controller_factor)));
 
   const auto initial_counter = test_controller->internal_counter;
-  for (size_t update_counter = 1; update_counter <= 10 * cm_update_rate; ++update_counter)
+  rclcpp::Time time = rclcpp::Time(0);
+  for (size_t update_counter = 0; update_counter <= 10 * cm_update_rate; ++update_counter)
   {
     EXPECT_EQ(
       controller_interface::return_type::OK,
-      cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)));
+      cm_->update(time, rclcpp::Duration::from_seconds(0.01)));
 
+    time += rclcpp::Duration::from_seconds(0.01);
     if (update_counter % cm_update_rate == 0)
     {
       const auto no_of_secs_passed = update_counter / cm_update_rate;
-      EXPECT_EQ(
+      // NOTE: here EXPECT_NEAR is used because it is observed that in the first iteration of whole
+      // cycle of cm_update_rate counts, there is one count missing, but in rest of the 9 cycles it
+      // is clearly tracking, so adding 1 here won't affect the final count.
+      // For instance, a controller with update rate 37 Hz, seems to have 36 in the first update
+      // cycle and then on accumulating 37 on every other update cycle so at the end of the 10
+      // cycles it will have 369 instead of 370.
+      EXPECT_NEAR(
         test_controller->internal_counter - initial_counter,
-        (expected_controller_update_rate * no_of_secs_passed));
+        (controller_update_rate * no_of_secs_passed), 1);
     }
   }
 }