diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index 69418398e9..ea2d7300be 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -15,6 +15,7 @@ #include #include #include +#include #include #include "./test_controller/test_controller.hpp" @@ -26,17 +27,29 @@ using ::testing::_; using ::testing::Return; -class TestControllerManager : public ControllerManagerFixture +struct Strictness +{ + int strictness = STRICT; + controller_interface::return_type expected_return; + unsigned int expected_counter; +}; +class TestControllerManager : public ControllerManagerFixture, + public testing::WithParamInterface { }; -TEST_F(TestControllerManager, controller_lifecycle) +TEST_P(TestControllerManager, controller_lifecycle) { + const auto test_param = GetParam(); auto test_controller = std::make_shared(); + auto test_controller2 = std::make_shared(); + constexpr char TEST_CONTROLLER2_NAME[] = "test_controller2_name"; cm_->add_controller( test_controller, test_controller::TEST_CONTROLLER_NAME, test_controller::TEST_CONTROLLER_CLASS_NAME); - EXPECT_EQ(1u, cm_->get_loaded_controllers().size()); + cm_->add_controller( + test_controller2, TEST_CONTROLLER2_NAME, test_controller::TEST_CONTROLLER_CLASS_NAME); + EXPECT_EQ(2u, cm_->get_loaded_controllers().size()); EXPECT_EQ(2, test_controller.use_count()); EXPECT_EQ( @@ -50,19 +63,42 @@ TEST_F(TestControllerManager, controller_lifecycle) // configure controller cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME); + cm_->configure_controller(TEST_CONTROLLER2_NAME); EXPECT_EQ( controller_interface::return_type::OK, cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is not started"; + EXPECT_EQ(0u, test_controller2->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 start_controllers = {"fake_controller", TEST_CONTROLLER2_NAME}; std::vector stop_controllers = {}; auto switch_future = std::async( std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, - start_controllers, stop_controllers, STRICT, true, rclcpp::Duration(0, 0)); + start_controllers, stop_controllers, test_param.strictness, true, rclcpp::Duration(0, 0)); + + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + EXPECT_EQ(0u, test_controller2->internal_counter) << "Controller is started at the end of update"; + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(test_param.expected_return, switch_future.get()); + } + + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + EXPECT_GE(test_controller2->internal_counter, test_param.expected_counter); + + // Start the real test controller, will take effect at the end of the update function + start_controllers = {test_controller::TEST_CONTROLLER_NAME}; + stop_controllers = {}; + switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, test_param.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"; @@ -88,7 +124,7 @@ TEST_F(TestControllerManager, controller_lifecycle) stop_controllers = {test_controller::TEST_CONTROLLER_NAME}; switch_future = std::async( std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, - start_controllers, stop_controllers, STRICT, true, rclcpp::Duration(0, 0)); + start_controllers, stop_controllers, test_param.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"; @@ -118,8 +154,9 @@ TEST_F(TestControllerManager, controller_lifecycle) EXPECT_EQ(1, test_controller.use_count()); } -TEST_F(TestControllerManager, per_controller_update_rate) +TEST_P(TestControllerManager, per_controller_update_rate) { + auto strictness = GetParam().strictness; auto test_controller = std::make_shared(); cm_->add_controller( test_controller, test_controller::TEST_CONTROLLER_NAME, @@ -151,7 +188,7 @@ TEST_F(TestControllerManager, per_controller_update_rate) std::vector stop_controllers = {}; auto switch_future = std::async( std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, - start_controllers, stop_controllers, STRICT, true, rclcpp::Duration(0, 0)); + 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"; @@ -173,3 +210,8 @@ TEST_F(TestControllerManager, per_controller_update_rate) EXPECT_GE(test_controller->internal_counter, 1u); EXPECT_EQ(test_controller->get_update_rate(), 4u); } + +Strictness strict{STRICT, controller_interface::return_type::ERROR, 0u}; +Strictness best_effort{BEST_EFFORT, controller_interface::return_type::OK, 1u}; +INSTANTIATE_TEST_SUITE_P( + test_strict_best_effort, TestControllerManager, testing::Values(strict, best_effort));