[CM] Fixes the issue with individual controller's update rate (#1082)

* added parameterized test for the controller's update rate check
* added warning for the update rate to be a perfect factor
* fix the issue with the period when the controller update rate is higher than the controller manager

(cherry picked from commit ad70c9a)

controller_manager/src/controller_manager.cpp

@@ -481,6 +481,19 @@ controller_interface::return_type ControllerManager::configure_controller(
       "update rate.",
       controller_name.c_str(), controller_update_rate, cm_update_rate);
   }
+  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);
+  }
 
   // CHAINABLE CONTROLLERS: get reference interfaces from chainable controllers
   if (controller->is_chainable())
@@ -1695,10 +1708,12 @@ controller_interface::return_type ControllerManager::update(
     if (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;
 
-      bool controller_go = controller_update_rate == 0 ||
-                           ((update_loop_counter_ % controller_update_rate) == 0) ||
-                           (controller_update_rate >= update_rate_);
+      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",
@@ -1707,7 +1722,7 @@ controller_interface::return_type ControllerManager::update(
       if (controller_go)
       {
         auto controller_ret = loaded_controller.c->update(
-          time, (controller_update_rate != update_rate_ && controller_update_rate != 0)
+          time, (controller_update_factor != 1u)
                   ? rclcpp::Duration::from_seconds(1.0 / controller_update_rate)
                   : period);
 

controller_manager/test/test_controller_manager.cpp

@@ -246,9 +246,13 @@ TEST_P(TestControllerManagerWithStrictness, per_controller_update_rate)
 
   EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_state().id());
 
-  EXPECT_EQ(
-    controller_interface::return_type::OK,
-    cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)));
+  // As the controller frequency is 4Hz, it needs to pass 25 iterations for 1 update cycle
+  for (size_t i = 0; i < 25; i++)
+  {
+    EXPECT_EQ(
+      controller_interface::return_type::OK,
+      cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)));
+  }
   EXPECT_GE(test_controller->internal_counter, 1u);
   EXPECT_EQ(test_controller->get_update_rate(), 4u);
 }
@@ -364,3 +368,82 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd
 INSTANTIATE_TEST_SUITE_P(
   per_controller_equal_and_higher_update_rate, TestControllerManagerWithUpdateRates,
   testing::Values(100, 232, 400));
+
+class TestControllerUpdateRates
+: public ControllerManagerFixture<controller_manager::ControllerManager>,
+  public testing::WithParamInterface<unsigned int>
+{
+};
+
+TEST_P(TestControllerUpdateRates, check_the_controller_update_rate)
+{
+  const unsigned int ctrl_update_rate = GetParam();
+  auto test_controller = std::make_shared<test_controller::TestController>();
+  cm_->add_controller(
+    test_controller, test_controller::TEST_CONTROLLER_NAME,
+    test_controller::TEST_CONTROLLER_CLASS_NAME);
+  EXPECT_EQ(1u, cm_->get_loaded_controllers().size());
+  EXPECT_EQ(2, test_controller.use_count());
+
+  EXPECT_EQ(
+    lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id());
+
+  test_controller->get_node()->set_parameter({"update_rate", static_cast<int>(ctrl_update_rate)});
+  // configure controller
+  cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME);
+  EXPECT_EQ(
+    controller_interface::return_type::OK,
+    cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)));
+  EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is not started";
+
+  EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_state().id());
+
+  // Start controller, will take effect at the end of the update function
+  const auto strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT;
+  std::vector<std::string> start_controllers = {test_controller::TEST_CONTROLLER_NAME};
+  std::vector<std::string> stop_controllers = {};
+  auto switch_future = std::async(
+    std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_,
+    start_controllers, stop_controllers, strictness, true, rclcpp::Duration(0, 0));
+
+  ASSERT_EQ(std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100)))
+    << "switch_controller should be blocking until next update cycle";
+
+  EXPECT_EQ(
+    controller_interface::return_type::OK,
+    cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)));
+  EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is started at the end of update";
+  {
+    ControllerManagerRunner cm_runner(this);
+    EXPECT_EQ(controller_interface::return_type::OK, switch_future.get());
+  }
+
+  EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_state().id());
+
+  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)
+  {
+    EXPECT_EQ(
+      controller_interface::return_type::OK,
+      cm_->update(rclcpp::Time(0), 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(
+        test_controller->internal_counter - initial_counter,
+        (expected_controller_update_rate * no_of_secs_passed));
+    }
+  }
+}
+
+INSTANTIATE_TEST_SUITE_P(
+  per_controller_update_rate_check, TestControllerUpdateRates,
+  testing::Values(10, 12, 16, 23, 37, 40, 50, 63, 71, 85, 98));