From 505743a14309eb5f0606152026265cc8a184aebb Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 4 Jul 2023 19:51:56 +0100 Subject: [PATCH] Refactor update rate tests to be parametrized on update rate --- .../test/test_controller_manager.cpp | 184 +++++++++--------- 1 file changed, 94 insertions(+), 90 deletions(-) diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index 103b3af80a..fe605243de 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include #include #include @@ -27,13 +27,13 @@ using ::testing::_; using ::testing::Return; -class TestControllerManager +class TestControllerManagerWithStrictness : public ControllerManagerFixture, public testing::WithParamInterface { }; -TEST_P(TestControllerManager, controller_lifecycle) +TEST_P(TestControllerManagerWithStrictness, controller_lifecycle) { const auto test_param = GetParam(); auto test_controller = std::make_shared(); @@ -159,7 +159,7 @@ TEST_P(TestControllerManager, controller_lifecycle) controller_interface::return_type::OK, cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); EXPECT_GE(test_controller->internal_counter, 1u); - auto last_internal_counter = test_controller->internal_counter; + size_t last_internal_counter = test_controller->internal_counter; // Stop controller, will take effect at the end of the update function start_controllers = {}; @@ -196,7 +196,7 @@ TEST_P(TestControllerManager, controller_lifecycle) EXPECT_EQ(1, test_controller.use_count()); } -TEST_P(TestControllerManager, per_controller_update_rate) +TEST_P(TestControllerManagerWithStrictness, per_controller_update_rate) { auto strictness = GetParam().strictness; auto test_controller = std::make_shared(); @@ -253,109 +253,113 @@ TEST_P(TestControllerManager, per_controller_update_rate) EXPECT_EQ(test_controller->get_update_rate(), 4u); } -TEST_P(TestControllerManager, per_controller_equal_and_higher_update_rate) +INSTANTIATE_TEST_SUITE_P( + test_strict_best_effort, TestControllerManagerWithStrictness, + testing::Values(strict, best_effort)); + +class TestControllerManagerWithUpdateRates +: public ControllerManagerFixture, + public testing::WithParamInterface { - auto strictness = GetParam().strictness; +}; + +TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_update_rate) +{ + const auto strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT; + const unsigned int ctrl_update_rate = GetParam(); auto test_controller = std::make_shared(); auto last_internal_counter = 0u; - for (unsigned int ctrl_update_rate : {100, 232, 400}) + RCLCPP_INFO( + rclcpp::get_logger("test_controller_manager"), "Testing update rate : %u Hz", ctrl_update_rate); { - RCLCPP_INFO( - rclcpp::get_logger("test_controller_manager"), "Testing update rate : %d Hz", - ctrl_update_rate); - { - ControllerManagerRunner cm_runner(this); - 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( - controller_interface::return_type::OK, - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); - EXPECT_EQ(last_internal_counter, test_controller->internal_counter) - << "Update should not reach an unconfigured controller"; + ControllerManagerRunner cm_runner(this); + 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( + controller_interface::return_type::OK, + cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + EXPECT_EQ(last_internal_counter, test_controller->internal_counter) + << "Update should not reach an unconfigured controller"; - EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id()); - rclcpp::Parameter update_rate_parameter("update_rate", static_cast(ctrl_update_rate)); - test_controller->get_node()->set_parameter(update_rate_parameter); - // 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(last_internal_counter, test_controller->internal_counter) - << "Controller is not started"; - EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_state().id()); + rclcpp::Parameter update_rate_parameter("update_rate", static_cast(ctrl_update_rate)); + test_controller->get_node()->set_parameter(update_rate_parameter); + // 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(last_internal_counter, 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 + std::vector start_controllers = {test_controller::TEST_CONTROLLER_NAME}; + std::vector 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)); - // Start controller, will take effect at the end of the update function - std::vector start_controllers = {test_controller::TEST_CONTROLLER_NAME}; - std::vector 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"; - 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(last_internal_counter, 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()); + const auto pre_internal_counter = test_controller->internal_counter; + rclcpp::Rate loop_rate(cm_->get_update_rate()); + for (size_t i = 0; i < 2 * cm_->get_update_rate(); i++) + { EXPECT_EQ( controller_interface::return_type::OK, cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); - EXPECT_EQ(last_internal_counter, 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()); - - const auto pre_internal_counter = test_controller->internal_counter; - rclcpp::Rate loop_rate(cm_->get_update_rate()); - for (size_t i = 0; i < 2 * cm_->get_update_rate(); i++) - { - EXPECT_EQ( - controller_interface::return_type::OK, - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); - loop_rate.sleep(); - } - // if we do 2 times of the controller_manager update rate, the internal counter should be - // similarly incremented - EXPECT_EQ( - test_controller->internal_counter, pre_internal_counter + (2 * cm_->get_update_rate())); - EXPECT_EQ(test_controller->get_update_rate(), ctrl_update_rate); + loop_rate.sleep(); + } + // if we do 2 times of the controller_manager update rate, the internal counter should be + // similarly incremented + EXPECT_EQ(test_controller->internal_counter, pre_internal_counter + (2 * cm_->get_update_rate())); + EXPECT_EQ(test_controller->get_update_rate(), ctrl_update_rate); - auto deactivate_future = std::async( - std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, - stop_controllers, start_controllers, strictness, true, rclcpp::Duration(0, 0)); + auto deactivate_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + stop_controllers, start_controllers, strictness, true, rclcpp::Duration(0, 0)); - ASSERT_EQ( - std::future_status::timeout, deactivate_future.wait_for(std::chrono::milliseconds(100))) - << "switch_controller should be blocking until next update cycle"; + ASSERT_EQ(std::future_status::timeout, deactivate_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))) - << "Controller is stopped at the end of update, so it should have done one more update"; - { - ControllerManagerRunner cm_runner(this); - EXPECT_EQ(controller_interface::return_type::OK, deactivate_future.get()); - } - auto unload_future = std::async( - std::launch::async, &controller_manager::ControllerManager::unload_controller, cm_, - test_controller::TEST_CONTROLLER_NAME); - ASSERT_EQ(std::future_status::timeout, unload_future.wait_for(std::chrono::milliseconds(100))) - << "unload_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))) + << "Controller is stopped at the end of update, so it should have done one more update"; + { ControllerManagerRunner cm_runner(this); - EXPECT_EQ(controller_interface::return_type::OK, unload_future.get()); - last_internal_counter = test_controller->internal_counter; + EXPECT_EQ(controller_interface::return_type::OK, deactivate_future.get()); } + auto unload_future = std::async( + std::launch::async, &controller_manager::ControllerManager::unload_controller, cm_, + test_controller::TEST_CONTROLLER_NAME); + ASSERT_EQ(std::future_status::timeout, unload_future.wait_for(std::chrono::milliseconds(100))) + << "unload_controller should be blocking until next update cycle"; + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(controller_interface::return_type::OK, unload_future.get()); + last_internal_counter = test_controller->internal_counter; } INSTANTIATE_TEST_SUITE_P( - test_strict_best_effort, TestControllerManager, testing::Values(strict, best_effort)); + per_controller_equal_and_higher_update_rate, TestControllerManagerWithUpdateRates, testing::Values(100, 232, 400));