Compute the actual update period for each controller (ros-controls#1140)

Falkor-Robot · Nov 21, 2023 · 99d2f61 · 99d2f61
1 parent 2569b76
commit 99d2f61
Show file tree

Hide file tree

Showing 4 changed files with 18 additions and 2 deletions.
diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp
@@ -2063,7 +2063,9 @@ controller_interface::return_type ControllerManager::update(
 
       if (controller_go)
       {
-        auto controller_ret = loaded_controller.c->update(time, controller_period);
+        const auto controller_actual_period =
+          (time - *loaded_controller.next_update_cycle_time) + controller_period;
+        auto controller_ret = loaded_controller.c->update(time, controller_actual_period);
 
         if (
           *loaded_controller.next_update_cycle_time ==

diff --git a/controller_manager/test/test_controller/test_controller.cpp b/controller_manager/test/test_controller/test_controller.cpp
@@ -60,8 +60,9 @@ controller_interface::InterfaceConfiguration TestController::state_interface_con
 }
 
 controller_interface::return_type TestController::update(
-  const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/)
+  const rclcpp::Time & /*time*/, const rclcpp::Duration & period)
 {
+  update_period_ = period.seconds();
   ++internal_counter;
 
   // set value to hardware to produce and test different behaviors there

diff --git a/controller_manager/test/test_controller/test_controller.hpp b/controller_manager/test/test_controller/test_controller.hpp
@@ -80,6 +80,7 @@ class TestController : public controller_interface::ControllerInterface
   // enables external setting of values to command interfaces - used for simulation of hardware
   // errors
   double set_first_command_interface_value_to;
+  double update_period_ = 0;
 };
 
 }  // namespace test_controller

diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp
@@ -351,6 +351,7 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd
   ASSERT_EQ(std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100)))
     << "switch_controller should be blocking until next update cycle";
 
+  time_ += rclcpp::Duration::from_seconds(0.01);
   EXPECT_EQ(
     controller_interface::return_type::OK,
     cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
@@ -365,11 +366,17 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd
 
   const auto pre_internal_counter = test_controller->internal_counter;
   rclcpp::Rate loop_rate(cm_->get_update_rate());
+  const auto cm_update_rate = cm_->get_update_rate();
   for (size_t i = 0; i < 2 * cm_->get_update_rate(); i++)
   {
+    time_ += rclcpp::Duration::from_seconds(0.01);
     EXPECT_EQ(
       controller_interface::return_type::OK,
       cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
+    // In case of a non perfect divisor, the update period should respect the rule
+    // [controller_update_rate, 2*controller_update_rate)
+    ASSERT_GE(test_controller->update_period_, 1.0 / cm_update_rate);
+    ASSERT_LT(test_controller->update_period_, 2.0 / cm_update_rate);
     loop_rate.sleep();
   }
   // if we do 2 times of the controller_manager update rate, the internal counter should be
@@ -448,6 +455,7 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate)
   ASSERT_EQ(std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100)))
     << "switch_controller should be blocking until next update cycle";
 
+  time_ += rclcpp::Duration::from_seconds(0.01);
   EXPECT_EQ(
     controller_interface::return_type::OK,
     cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
@@ -470,6 +478,10 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate)
     EXPECT_EQ(
       controller_interface::return_type::OK,
       cm_->update(time, rclcpp::Duration::from_seconds(0.01)));
+    // In case of a non perfect divisor, the update period should respect the rule
+    // [controller_update_rate, 2*controller_update_rate)
+    ASSERT_GE(test_controller->update_period_, 1.0 / controller_update_rate);
+    ASSERT_LT(test_controller->update_period_, 2.0 / controller_update_rate);
 
     time += rclcpp::Duration::from_seconds(0.01);
     if (update_counter % cm_update_rate == 0)