check that the controller update rate is real

ros-controls · Oct 22, 2023 · bc6c158 · bc6c158
1 parent bd6911d
commit bc6c158
Show file tree

Hide file tree

Showing 3 changed files with 15 additions and 1 deletion.
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
@@ -77,6 +77,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
@@ -314,6 +314,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)));
@@ -328,11 +329,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
@@ -408,6 +415,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)));
@@ -430,6 +438,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)