Calculate achievable update rate of controllers (#2828)

Author: saikishor

controller_interface/include/controller_interface/controller_interface_params.hpp

@@ -47,6 +47,7 @@ struct ControllerInterfaceParams
   std::string controller_name = "";
   std::string robot_description = "";
   unsigned int update_rate = 0;
+  unsigned int controller_manager_update_rate = 0;
 
   std::string node_namespace = "";
   rclcpp::NodeOptions node_options = rclcpp::NodeOptions();

controller_interface/src/controller_interface_base.cpp

@@ -47,6 +47,7 @@ return_type ControllerInterfaceBase::init(
   params.controller_name = controller_name;
   params.robot_description = urdf;
   params.update_rate = cm_update_rate;
+  params.controller_manager_update_rate = cm_update_rate;
   params.node_namespace = node_namespace;
   params.node_options = node_options;
 
@@ -62,11 +63,26 @@ return_type ControllerInterfaceBase::init(
     ctrl_itf_params_.node_options,
     false);  // disable LifecycleNode service interfaces
 
+  if (ctrl_itf_params_.controller_manager_update_rate == 0 && ctrl_itf_params_.update_rate != 0)
+  {
+    RCLCPP_WARN(
+      node_->get_logger(), "%s",
+      fmt::format(
+        "The 'controller_manager_update_rate' variable of the ControllerInterfaceParams is unset "
+        "or set to 0 Hz while the 'update_rate' variable is set to a non-zero value of '{} Hz'. "
+        "Using the controller's update rate as the controller manager update rate. Please fix in "
+        "the tests by initializing the 'controller_manager_update_rate' instead of the "
+        "'update_rate' variable within the ControllerInterfaceParams struct",
+        ctrl_itf_params_.update_rate)
+        .c_str());
+    ctrl_itf_params_.controller_manager_update_rate = ctrl_itf_params_.update_rate;
+  }
+
   try
   {
     // no rclcpp::ParameterValue unsigned int specialization
-    auto_declare<int>("update_rate", static_cast<int>(ctrl_itf_params_.update_rate));
-
+    auto_declare<int>(
+      "update_rate", static_cast<int>(ctrl_itf_params_.controller_manager_update_rate));
     auto_declare<bool>("is_async", false);
     auto_declare<int>("thread_priority", -100);
   }
@@ -168,14 +184,33 @@ const rclcpp_lifecycle::State & ControllerInterfaceBase::configure()
         get_node()->get_logger(), "%s",
         fmt::format(
           "The update rate of the controller : '{} Hz' cannot be higher than the update rate of "
-          "the "
-          "controller manager : '{} Hz'. Setting it to the update rate of the controller manager.",
+          "the controller manager : '{} Hz'. Setting it to the update rate of the controller "
+          "manager.",
           update_rate, ctrl_itf_params_.update_rate)
           .c_str());
     }
     else
     {
-      ctrl_itf_params_.update_rate = static_cast<unsigned int>(update_rate);
+      if (update_rate > 0 && ctrl_itf_params_.controller_manager_update_rate > 0)
+      {
+        // Calculate the update rate corresponding the periodicity of the controller manager
+        const bool is_frequency_achievable = (ctrl_itf_params_.controller_manager_update_rate %
+                                              static_cast<unsigned int>(update_rate)) == 0;
+        const unsigned int ticks_per_controller_per_second = static_cast<unsigned int>(std::round(
+          static_cast<double>(ctrl_itf_params_.controller_manager_update_rate) /
+          static_cast<double>(update_rate)));
+        const unsigned int achievable_hz =
+          is_frequency_achievable
+            ? static_cast<unsigned int>(update_rate)
+            : ctrl_itf_params_.controller_manager_update_rate / ticks_per_controller_per_second;
+
+        RCLCPP_WARN_EXPRESSION(
+          get_node()->get_logger(), !is_frequency_achievable,
+          "The requested update rate of '%ld Hz' is not achievable with the controller manager "
+          "update rate of '%d Hz'. Setting it to the closest achievable frequency '%d Hz'.",
+          update_rate, ctrl_itf_params_.controller_manager_update_rate, achievable_hz);
+        ctrl_itf_params_.update_rate = achievable_hz;
+      }
     }
     is_async_ = get_node()->get_parameter("is_async").as_bool();
   }

controller_interface/test/test_controller_interface.cpp

@@ -115,6 +115,7 @@ TEST(TestableControllerInterface, setting_update_rate_in_configure)
   params.controller_name = TEST_CONTROLLER_NAME;
   params.robot_description = "";
   params.update_rate = 5000;  // set a different update rate than the one in the node options
+  params.controller_manager_update_rate = 5000;
   params.node_namespace = "";
   params.node_options = node_options;
   joint_limits::JointLimits joint_limits;
@@ -159,36 +160,37 @@ TEST(TestableControllerInterface, setting_update_rate_in_configure)
 
   // Even after configure is 0
   controller.configure();
-  ASSERT_EQ(controller.get_update_rate(), 2812u);
+  ASSERT_EQ(controller.get_update_rate(), 2500u) << "Needs to be achievable rate based on cm rate";
 
   // Test updating of update_rate parameter
   auto res = controller.get_node()->set_parameter(rclcpp::Parameter("update_rate", 623));
   EXPECT_EQ(res.successful, true);
   // Keep the same update rate until transition from 'UNCONFIGURED' TO 'INACTIVE' does not happen
   controller.configure();  // No transition so the update rate should stay intact
   ASSERT_NE(controller.get_update_rate(), 623u);
-  ASSERT_EQ(controller.get_update_rate(), 2812u);
+  ASSERT_EQ(controller.get_update_rate(), 2500u);
 
   controller.get_node()->activate();
   controller.configure();  // No transition so the update rate should stay intact
   ASSERT_NE(controller.get_update_rate(), 623u);
-  ASSERT_EQ(controller.get_update_rate(), 2812u);
+  ASSERT_EQ(controller.get_update_rate(), 2500u);
 
   controller.update(controller.get_node()->now(), rclcpp::Duration::from_seconds(0.1));
   controller.configure();  // No transition so the update rate should stay intact
   ASSERT_NE(controller.get_update_rate(), 623u);
-  ASSERT_EQ(controller.get_update_rate(), 2812u);
+  ASSERT_EQ(controller.get_update_rate(), 2500u);
 
   controller.get_node()->deactivate();
   controller.configure();  // No transition so the update rate should stay intact
   ASSERT_NE(controller.get_update_rate(), 623u);
-  ASSERT_EQ(controller.get_update_rate(), 2812u);
+  ASSERT_EQ(controller.get_update_rate(), 2500u);
 
   controller.get_node()->cleanup();
-  ASSERT_EQ(controller.get_update_rate(), 2812u);
+  ASSERT_EQ(controller.get_update_rate(), 2500u);
   // It is first changed after controller is configured again.
   controller.configure();
-  ASSERT_EQ(controller.get_update_rate(), 623u);
+  ASSERT_EQ(controller.get_update_rate(), 625u)
+    << "It needs to be 625 as it is closest achievable rate wrt to the CM rate";
 
   // Should stay same after multiple cleanups as it is set during initialization
   const auto hard_limits_final = controller.get_hard_joint_limits();

controller_manager/src/controller_manager.cpp

@@ -2114,6 +2114,7 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::add_co
     controller_params.controller_name = controller.info.name;
     controller_params.robot_description = robot_description_;
     controller_params.update_rate = get_update_rate();
+    controller_params.controller_manager_update_rate = get_update_rate();
     controller_params.node_namespace = get_namespace();
     controller_params.node_options = controller_node_options;
     controller_params.hard_joint_limits = resource_manager_->get_hard_joint_limits();

controller_manager/test/test_controller_manager.cpp

@@ -1030,18 +1030,18 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate)
   EXPECT_EQ(
     lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_lifecycle_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 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 exp_periodicity = 1.0 / exp_controller_period;
+  EXPECT_EQ(test_controller->get_update_rate(), std::floor(exp_periodicity));
 
   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 = 1.0 / exp_controller_period;
   for (size_t update_counter = 0; update_counter <= 10 * cm_update_rate; ++update_counter)
   {
     rclcpp::Time old_time = time;
@@ -1183,18 +1183,18 @@ TEST_F(TestAsyncControllerUpdateRates, check_the_async_controller_update_rate_an
   EXPECT_EQ(
     lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_lifecycle_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 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 exp_periodicity = 1.0 / exp_controller_period;
+  EXPECT_EQ(test_controller->get_update_rate(), std::round(exp_periodicity));
 
   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 = 1.0 / exp_controller_period;
   for (size_t update_counter = 0; update_counter <= 10 * cm_update_rate; ++update_counter)
   {
     rclcpp::Time old_time = time;

doc/release_notes.rst

@@ -9,6 +9,7 @@ controller_interface
 ********************
 * The new ``MagneticFieldSensor`` semantic component provides an interface for reading data from magnetometers. (`#2627 <https://github.com/ros-controls/ros2_control/pull/2627>`__)
 * The controller_manager will now deactivate the entire controller chain if a controller in the chain fails during the update cycle. `(#2681 <https://github.com/ros-controls/ros2_control/pull/2681>`__)
+* The update rate of the controller will now be approximated to a closer achievable frequency, when its frequency is not achievable with the current controller manager update rate. (`#2828 <https://github.com/ros-controls/ros2_control/pull/2828>`__)
 
 controller_manager
 ******************