diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index cfe7d218c37..e1266f95b5b 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/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); } @@ -361,13 +365,19 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd last_internal_counter = test_controller->internal_counter; } +INSTANTIATE_TEST_SUITE_P( + per_controller_equal_and_higher_update_rate, TestControllerManagerWithUpdateRates, + testing::Values(100, 232, 400)); + class TestControllerUpdateRates -: public ControllerManagerFixture +: public ControllerManagerFixture, + public testing::WithParamInterface { }; -TEST_F(TestControllerUpdateRates, check_the_controller_update_rate) +TEST_P(TestControllerUpdateRates, check_the_controller_update_rate) { + const unsigned int ctrl_update_rate = GetParam(); auto test_controller = std::make_shared(); cm_->add_controller( test_controller, test_controller::TEST_CONTROLLER_NAME, @@ -378,7 +388,7 @@ TEST_F(TestControllerUpdateRates, check_the_controller_update_rate) EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id()); - test_controller->get_node()->set_parameter({"update_rate", 40}); + test_controller->get_node()->set_parameter({"update_rate", static_cast(ctrl_update_rate)}); // configure controller cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME); EXPECT_EQ( @@ -410,11 +420,14 @@ TEST_F(TestControllerUpdateRates, check_the_controller_update_rate) EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_state().id()); - EXPECT_EQ(test_controller->get_update_rate(), 40u); + 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 expected_controller_update_rate = + cm_update_rate / (cm_update_rate / controller_update_rate); - for (size_t update_counter = 0; update_counter <= 10 * cm_update_rate; ++update_counter) + const auto initial_counter = test_controller->internal_counter; + for (size_t update_counter = 2; update_counter <= 10 * cm_update_rate; ++update_counter) { EXPECT_EQ( controller_interface::return_type::OK, @@ -423,11 +436,11 @@ TEST_F(TestControllerUpdateRates, check_the_controller_update_rate) if (update_counter % cm_update_rate == 0) { const auto no_of_secs_passed = update_counter / cm_update_rate; - EXPECT_EQ(test_controller->internal_counter, controller_update_rate * no_of_secs_passed); + EXPECT_EQ( + test_controller->internal_counter, (expected_controller_update_rate * no_of_secs_passed)); } } } INSTANTIATE_TEST_SUITE_P( - per_controller_equal_and_higher_update_rate, TestControllerManagerWithUpdateRates, - testing::Values(100, 232, 400)); + per_controller_update_rate_check, TestControllerUpdateRates, testing::Values(23, 40, 50));