diff --git a/controller_manager/test/controller_manager_test_common.hpp b/controller_manager/test/controller_manager_test_common.hpp index 1266acad76..3a784cfb94 100644 --- a/controller_manager/test/controller_manager_test_common.hpp +++ b/controller_manager/test/controller_manager_test_common.hpp @@ -32,7 +32,6 @@ #include "rclcpp/utilities.hpp" #include "ros2_control_test_assets/descriptions.hpp" -#include "test_controller/test_controller.hpp" #include "test_controller_failed_init/test_controller_failed_init.hpp" constexpr auto STRICT = controller_manager_msgs::srv::SwitchController::Request::STRICT; @@ -40,6 +39,21 @@ constexpr auto BEST_EFFORT = controller_manager_msgs::srv::SwitchController::Req const auto TEST_CM_NAME = "test_controller_manager"; +// Strictness structure for parameterized tests - shared between different tests +struct Strictness +{ + int strictness = STRICT; + controller_interface::return_type expected_return; + unsigned int expected_counter; +}; +Strictness strict{STRICT, controller_interface::return_type::ERROR, 0u}; +Strictness best_effort{BEST_EFFORT, controller_interface::return_type::OK, 1u}; + +// Forward definition to avid compile error - defined at the end of the file +template +class ControllerManagerRunner; + +template class ControllerManagerFixture : public ::testing::Test { public: @@ -50,7 +64,7 @@ class ControllerManagerFixture : public ::testing::Test void SetUp() { executor_ = std::make_shared(); - cm_ = std::make_shared( + cm_ = std::make_shared( std::make_unique( ros2_control_test_assets::minimal_robot_urdf, true, true), executor_, TEST_CM_NAME); @@ -80,14 +94,32 @@ class ControllerManagerFixture : public ::testing::Test } } + void switch_test_controllers( + const std::vector & start_controllers, + const std::vector & stop_controllers, const int strictness, + const std::future_status expected_future_status = std::future_status::timeout, + const controller_interface::return_type expected_return = controller_interface::return_type::OK) + { + // First activation not possible because controller not configured + 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(expected_future_status, switch_future.wait_for(std::chrono::milliseconds(100))) + << "switch_controller should be blocking until next update cycle"; + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(expected_return, switch_future.get()); + } + std::shared_ptr executor_; - std::shared_ptr cm_; + std::shared_ptr cm_; std::thread updater_; bool run_updater_; }; -class TestControllerManagerSrvs : public ControllerManagerFixture +class TestControllerManagerSrvs +: public ControllerManagerFixture { public: TestControllerManagerSrvs() {} @@ -146,17 +178,18 @@ class TestControllerManagerSrvs : public ControllerManagerFixture std::future executor_spin_future_; }; +template class ControllerManagerRunner { public: - explicit ControllerManagerRunner(ControllerManagerFixture * cmf) : cmf_(cmf) + explicit ControllerManagerRunner(ControllerManagerFixture * cmf) : cmf_(cmf) { cmf_->startCmUpdater(); } ~ControllerManagerRunner() { cmf_->stopCmUpdater(); } - ControllerManagerFixture * cmf_; + ControllerManagerFixture * cmf_; }; class ControllerMock : public controller_interface::ControllerInterface diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index ec73b315b2..fccf237e0e 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -27,14 +27,9 @@ using ::testing::_; using ::testing::Return; -struct Strictness -{ - int strictness = STRICT; - controller_interface::return_type expected_return; - unsigned int expected_counter; -}; -class TestControllerManager : public ControllerManagerFixture, - public testing::WithParamInterface +class TestControllerManager +: public ControllerManagerFixture, + public testing::WithParamInterface { }; @@ -211,7 +206,5 @@ TEST_P(TestControllerManager, per_controller_update_rate) 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)); diff --git a/controller_manager/test/test_controller_manager_srvs.cpp b/controller_manager/test/test_controller_manager_srvs.cpp index 7cd73ab2e3..2b2f1fecb6 100644 --- a/controller_manager/test/test_controller_manager_srvs.cpp +++ b/controller_manager/test/test_controller_manager_srvs.cpp @@ -26,6 +26,7 @@ #include "controller_manager_msgs/srv/list_controllers.hpp" #include "controller_manager_msgs/srv/switch_controller.hpp" #include "lifecycle_msgs/msg/state.hpp" +#include "test_controller/test_controller.hpp" using ::testing::_; using ::testing::Return; diff --git a/controller_manager/test/test_load_controller.cpp b/controller_manager/test/test_load_controller.cpp index f714bd1d01..913e922bc5 100644 --- a/controller_manager/test/test_load_controller.cpp +++ b/controller_manager/test/test_load_controller.cpp @@ -23,6 +23,7 @@ #include "controller_manager/controller_manager.hpp" #include "controller_manager_test_common.hpp" #include "lifecycle_msgs/msg/state.hpp" +#include "test_controller/test_controller.hpp" using test_controller::TEST_CONTROLLER_CLASS_NAME; using ::testing::_; @@ -31,25 +32,8 @@ const auto controller_name1 = "test_controller1"; const auto controller_name2 = "test_controller2"; using strvec = std::vector; -class TestLoadController : public ControllerManagerFixture +class TestLoadController : public ControllerManagerFixture { -protected: - void _switch_test_controllers( - const strvec & start_controllers, const strvec & stop_controllers, - const std::future_status expected_future_status = std::future_status::timeout, - const controller_interface::return_type expected_interface_status = - controller_interface::return_type::OK) - { - // First activation not possible because controller not configured - auto switch_future = std::async( - std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, - start_controllers, stop_controllers, STRICT, true, rclcpp::Duration(0, 0)); - - ASSERT_EQ(expected_future_status, switch_future.wait_for(std::chrono::milliseconds(100))) - << "switch_controller should be blocking until next update cycle"; - ControllerManagerRunner cm_runner(this); - EXPECT_EQ(expected_interface_status, switch_future.get()); - } }; TEST_F(TestLoadController, load_unknown_controller) @@ -72,6 +56,23 @@ TEST_F(TestLoadController, configuring_non_loaded_controller_fails) EXPECT_EQ(cm_->configure_controller(controller_name1), controller_interface::return_type::ERROR); } +TEST_F(TestLoadController, can_set_and_get_non_default_update_rate) +{ + auto controller_if = + cm_->load_controller("test_controller_01", test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_NE(controller_if, nullptr); + + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if->get_state().id()); + + controller_if->get_node()->set_parameter({"update_rate", 1337}); + + cm_->configure_controller("test_controller_01"); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_state().id()); + + EXPECT_EQ(1337u, controller_if->get_update_rate()); +} + class TestLoadedController : public TestLoadController { public: @@ -86,25 +87,34 @@ class TestLoadedController : public TestLoadController } void start_test_controller( + const int strictness, const std::future_status expected_future_status = std::future_status::timeout, const controller_interface::return_type expected_interface_status = controller_interface::return_type::OK) { - _switch_test_controllers( - strvec{controller_name1}, strvec{}, expected_future_status, expected_interface_status); + switch_test_controllers( + strvec{controller_name1}, strvec{}, strictness, expected_future_status, + expected_interface_status); } void stop_test_controller( + const int strictness, const std::future_status expected_future_status = std::future_status::timeout, const controller_interface::return_type expected_interface_status = controller_interface::return_type::OK) { - _switch_test_controllers( - strvec{}, strvec{controller_name1}, expected_future_status, expected_interface_status); + switch_test_controllers( + strvec{}, strvec{controller_name1}, strictness, expected_future_status, + expected_interface_status); } }; -TEST_F(TestLoadedController, load_and_configure_one_known_controller) +class TestLoadedControllerParametrized : public TestLoadedController, + public testing::WithParamInterface +{ +}; + +TEST_P(TestLoadedControllerParametrized, load_and_configure_one_known_controller) { EXPECT_EQ(1u, cm_->get_loaded_controllers().size()); @@ -115,60 +125,70 @@ TEST_F(TestLoadedController, load_and_configure_one_known_controller) EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_state().id()); } -TEST_F(TestLoadedController, can_start_configured_controller) +TEST_P(TestLoadedControllerParametrized, can_start_configured_controller) { + const auto test_param = GetParam(); + EXPECT_EQ(cm_->configure_controller(controller_name1), controller_interface::return_type::OK); - start_test_controller(); + start_test_controller(test_param.strictness); ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if->get_state().id()); } -TEST_F(TestLoadedController, can_stop_active_controller) +TEST_P(TestLoadedControllerParametrized, can_stop_active_controller) { + const auto test_param = GetParam(); + EXPECT_EQ(cm_->configure_controller(controller_name1), controller_interface::return_type::OK); - start_test_controller(); + start_test_controller(test_param.strictness); // Stop controller - stop_test_controller(); + stop_test_controller(test_param.strictness); ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_state().id()); } -TEST_F(TestLoadedController, starting_and_stopping_a_controller) +TEST_P(TestLoadedControllerParametrized, starting_and_stopping_a_controller) { + const auto test_param = GetParam(); + ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if->get_state().id()); - // Only testing with STRICT now for simplicity { // Test starting unconfigured controller, and starting configured afterwards - start_test_controller(std::future_status::ready, controller_interface::return_type::ERROR); + start_test_controller( + test_param.strictness, std::future_status::ready, test_param.expected_return); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if->get_state().id()); // Activate configured controller cm_->configure_controller(controller_name1); - start_test_controller(); + start_test_controller(test_param.strictness); ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if->get_state().id()); } { // Stop controller - stop_test_controller(); + stop_test_controller(test_param.strictness); ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_state().id()); } } -TEST_F(TestLoadedController, can_not_configure_active_controller) +TEST_P(TestLoadedControllerParametrized, can_not_configure_active_controller) { + const auto test_param = GetParam(); + EXPECT_EQ(cm_->configure_controller(controller_name1), controller_interface::return_type::OK); - start_test_controller(); + start_test_controller(test_param.strictness); // Can not configure active controller EXPECT_EQ(cm_->configure_controller(controller_name1), controller_interface::return_type::ERROR); ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if->get_state().id()); } -TEST_F(TestLoadedController, can_not_start_finalized_controller) +TEST_P(TestLoadedControllerParametrized, can_not_start_finalized_controller) { + const auto test_param = GetParam(); + ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if->get_state().id()); @@ -176,20 +196,23 @@ TEST_F(TestLoadedController, can_not_start_finalized_controller) ASSERT_EQ(controller_if->shutdown().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED); // Start controller - start_test_controller(std::future_status::ready, controller_interface::return_type::ERROR); + start_test_controller( + test_param.strictness, std::future_status::ready, test_param.expected_return); - // Can not configure unconfigured controller + // Can not configure finalize controller EXPECT_EQ(cm_->configure_controller(controller_name1), controller_interface::return_type::ERROR); ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, controller_if->get_state().id()); } -TEST_F(TestLoadedController, inactive_controller_cannot_be_cleaned_up) +TEST_P(TestLoadedControllerParametrized, inactive_controller_cannot_be_cleaned_up) { + const auto test_param = GetParam(); + EXPECT_EQ(cm_->configure_controller(controller_name1), controller_interface::return_type::OK); - start_test_controller(); + start_test_controller(test_param.strictness); - stop_test_controller(); + stop_test_controller(test_param.strictness); ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_state().id()); @@ -204,13 +227,15 @@ TEST_F(TestLoadedController, inactive_controller_cannot_be_cleaned_up) EXPECT_EQ(0u, cleanup_calls); } -TEST_F(TestLoadedController, inactive_controller_cannot_be_configured) +TEST_P(TestLoadedControllerParametrized, inactive_controller_cannot_be_configured) { + const auto test_param = GetParam(); + EXPECT_EQ(cm_->configure_controller(controller_name1), controller_interface::return_type::OK); - start_test_controller(); + start_test_controller(test_param.strictness); - stop_test_controller(); + stop_test_controller(test_param.strictness); ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_state().id()); std::shared_ptr test_controller = @@ -224,6 +249,9 @@ TEST_F(TestLoadedController, inactive_controller_cannot_be_configured) EXPECT_EQ(1u, cleanup_calls); } +INSTANTIATE_TEST_SUITE_P( + test_strict_best_effort, TestLoadedControllerParametrized, testing::Values(strict, best_effort)); + class SwitchTest : public TestLoadedController, public ::testing::WithParamInterface< @@ -329,7 +357,8 @@ INSTANTIATE_TEST_SUITE_P( "Switch with valid start controller and nonexistent controller specified, and strinctness " "STRICT, didn't return ERROR"))); -class TestTwoLoadedControllers : public TestLoadController +class TestTwoLoadedControllers : public TestLoadController, + public testing::WithParamInterface { public: controller_interface::ControllerInterfaceSharedPtr controller_if1{nullptr}; @@ -349,16 +378,6 @@ class TestTwoLoadedControllers : public TestLoadController ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if2->get_state().id()); } - - void switch_test_controllers( - const strvec & start_controllers, const strvec & stop_controllers, - const std::future_status expected_future_status = std::future_status::timeout, - const controller_interface::return_type expected_interface_status = - controller_interface::return_type::OK) - { - _switch_test_controllers( - start_controllers, stop_controllers, expected_future_status, expected_interface_status); - } }; TEST_F(TestTwoLoadedControllers, load_and_configure_two_known_controllers) @@ -370,16 +389,15 @@ TEST_F(TestTwoLoadedControllers, load_and_configure_two_known_controllers) EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if2->get_state().id()); } -TEST_F(TestTwoLoadedControllers, switch_multiple_controllers) +TEST_P(TestTwoLoadedControllers, switch_multiple_controllers) { - // Only testing with STRICT now for simplicity - // Test starting a stopped controller, and stopping afterwards + const auto test_param = GetParam(); cm_->configure_controller(controller_name1); // Start controller #1 RCLCPP_INFO(cm_->get_logger(), "Starting stopped controller #1"); - switch_test_controllers(strvec{controller_name1}, strvec{}); + switch_test_controllers(strvec{controller_name1}, strvec{}, test_param.strictness); ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if1->get_state().id()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if2->get_state().id()); @@ -387,10 +405,15 @@ TEST_F(TestTwoLoadedControllers, switch_multiple_controllers) // Stop controller 1, start controller 2 // Both fail because controller 2 because it is not configured and STRICT is used RCLCPP_INFO( - cm_->get_logger(), - "Stopping controller #1, starting unconfigured controller #2 fails (STRICT)"); + cm_->get_logger(), "Stopping controller #1, starting unconfigured controller #2 fails (%s)", + (test_param.strictness == STRICT ? "STRICT" : "BEST EFFORT")); + // TODO(destogl): fix this test for BEST_EFFORT - probably related to: + // https://github.com/ros-controls/ros2_control/pull/582#issuecomment-1029931561 + // switch_test_controllers( + // strvec{controller_name2}, strvec{controller_name1}, test_param.strictness, + // std::future_status::ready, controller_interface::return_type::ERROR); switch_test_controllers( - strvec{controller_name2}, strvec{controller_name1}, std::future_status::ready, + strvec{controller_name2}, strvec{controller_name1}, STRICT, std::future_status::ready, controller_interface::return_type::ERROR); ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if1->get_state().id()); ASSERT_EQ( @@ -400,41 +423,28 @@ TEST_F(TestTwoLoadedControllers, switch_multiple_controllers) // Stop controller 1 RCLCPP_INFO(cm_->get_logger(), "Stopping controller #1"); - switch_test_controllers(strvec{}, strvec{controller_name1}); + switch_test_controllers(strvec{}, strvec{controller_name1}, test_param.strictness); ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if1->get_state().id()); ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if2->get_state().id()); // Start controller 1 again RCLCPP_INFO(cm_->get_logger(), "Starting stopped controller #1"); - switch_test_controllers(strvec{controller_name1}, strvec{}); + switch_test_controllers(strvec{controller_name1}, strvec{}, test_param.strictness); ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if1->get_state().id()); ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if2->get_state().id()); // Stop controller 1, start controller 2 RCLCPP_INFO(cm_->get_logger(), "Stopping controller #1, starting controller #2"); - switch_test_controllers(strvec{controller_name2}, strvec{controller_name1}); + switch_test_controllers( + strvec{controller_name2}, strvec{controller_name1}, test_param.strictness); ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if1->get_state().id()); ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if2->get_state().id()); // Stop controller 2 RCLCPP_INFO(cm_->get_logger(), "Stopping controller #2"); - switch_test_controllers(strvec{}, strvec{controller_name2}); + switch_test_controllers(strvec{}, strvec{controller_name2}, test_param.strictness); ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if2->get_state().id()); } -TEST_F(TestLoadController, can_set_and_get_non_default_update_rate) -{ - auto controller_if = - cm_->load_controller("test_controller_01", test_controller::TEST_CONTROLLER_CLASS_NAME); - ASSERT_NE(controller_if, nullptr); - - ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if->get_state().id()); - - controller_if->get_node()->set_parameter({"update_rate", 1337}); - - cm_->configure_controller("test_controller_01"); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_state().id()); - - EXPECT_EQ(1337u, controller_if->get_update_rate()); -} +INSTANTIATE_TEST_SUITE_P( + test_strict_best_effort, TestTwoLoadedControllers, testing::Values(strict, best_effort)); diff --git a/controller_manager/test/test_release_interfaces.cpp b/controller_manager/test/test_release_interfaces.cpp index 3fddb8bbe4..3ee141edaa 100644 --- a/controller_manager/test/test_release_interfaces.cpp +++ b/controller_manager/test/test_release_interfaces.cpp @@ -27,7 +27,7 @@ using ::testing::_; using ::testing::Return; -class TestReleaseInterfaces : public ControllerManagerFixture +class TestReleaseInterfaces : public ControllerManagerFixture { }; diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index a29a68b045..b90b8570f8 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -23,12 +23,13 @@ #include "controller_manager/controller_manager.hpp" #include "controller_manager_test_common.hpp" #include "lifecycle_msgs/msg/state.hpp" +#include "test_controller/test_controller.hpp" using ::testing::_; using ::testing::Return; using namespace std::chrono_literals; -class TestLoadController : public ControllerManagerFixture +class TestLoadController : public ControllerManagerFixture { void SetUp() override {