From 12d4dda10c1e6c15bebc4293207820a017430485 Mon Sep 17 00:00:00 2001 From: Xi Huang Date: Thu, 25 Nov 2021 13:53:00 +0100 Subject: [PATCH 1/9] Add BEST_EFFORT in the controller switch tests. --- .../test/test_controller_manager.cpp | 92 ++++++++++++++++++- 1 file changed, 90 insertions(+), 2 deletions(-) diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index 69418398e9..f9533e8862 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -110,8 +110,70 @@ TEST_F(TestControllerManager, controller_lifecycle) 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()); + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(controller_interface::return_type::OK, unload_future.get()); + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id()); + EXPECT_EQ(1, test_controller.use_count()); + + // Start controller, use BEST_EFFORT instead of STRICT + switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, BEST_EFFORT, 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"; + + 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 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()); + + 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); + last_internal_counter = test_controller->internal_counter; + + // Stop controller, use BEST_EFFORT instead of STRICT + start_controllers = {}; + stop_controllers = {test_controller::TEST_CONTROLLER_NAME}; + switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, BEST_EFFORT, 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"; + + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + EXPECT_EQ(last_internal_counter + 1u, test_controller->internal_counter) + << "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, switch_future.get()); + } + + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_state().id()); + 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()); + } EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id()); @@ -167,6 +229,32 @@ TEST_F(TestControllerManager, per_controller_update_rate) EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_state().id()); + // Start controller with BEST_EFFORT, will take effect at the end of the update function + // All the controller should be running. With the tag BEST_EFFORT, it should return OK. + 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); + + switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, BEST_EFFORT, 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"; + + 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 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()); + EXPECT_EQ( controller_interface::return_type::OK, cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); From 75f3c0ac69d25e05280dbac1e612e097cc4bf2be Mon Sep 17 00:00:00 2001 From: Xi Huang Date: Thu, 25 Nov 2021 15:03:25 +0100 Subject: [PATCH 2/9] Write the tests in seperate test functions. --- .../test/test_controller_manager.cpp | 132 +++++++++++++----- 1 file changed, 94 insertions(+), 38 deletions(-) diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index f9533e8862..a1af605604 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -110,17 +110,101 @@ TEST_F(TestControllerManager, controller_lifecycle) 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()); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id()); + EXPECT_EQ(1, test_controller.use_count()); +} + +TEST_F(TestControllerManager, per_controller_update_rate) +{ + auto test_controller = std::make_shared(); + 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(0u, 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()); + + test_controller->get_node()->set_parameter({"update_rate", 4}); + // 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(0u, 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, STRICT, 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"; + + 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 started at the end of update"; { ControllerManagerRunner cm_runner(this); - EXPECT_EQ(controller_interface::return_type::OK, unload_future.get()); + EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); } + 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))); + EXPECT_GE(test_controller->internal_counter, 1u); + EXPECT_EQ(test_controller->get_update_rate(), 4u); +} + +TEST_F(TestControllerManager, controller_lifecycle_best_effort) +{ + auto test_controller = std::make_shared(); + 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(0u, 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(1, test_controller.use_count()); - // Start controller, use BEST_EFFORT instead of STRICT - switch_future = std::async( + // 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(0u, 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, BEST_EFFORT, true, rclcpp::Duration(0, 0)); @@ -141,9 +225,9 @@ TEST_F(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); - last_internal_counter = test_controller->internal_counter; + auto last_internal_counter = test_controller->internal_counter; - // Stop controller, use BEST_EFFORT instead of STRICT + // Stop controller, will take effect at the end of the update function start_controllers = {}; stop_controllers = {test_controller::TEST_CONTROLLER_NAME}; switch_future = std::async( @@ -164,23 +248,21 @@ TEST_F(TestControllerManager, controller_lifecycle) } EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_state().id()); - unload_future = std::async( + 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()); - } + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(controller_interface::return_type::OK, unload_future.get()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id()); EXPECT_EQ(1, test_controller.use_count()); } -TEST_F(TestControllerManager, per_controller_update_rate) +TEST_F(TestControllerManager, per_controller_update_rate_best_effort) { auto test_controller = std::make_shared(); cm_->add_controller( @@ -212,32 +294,6 @@ TEST_F(TestControllerManager, per_controller_update_rate) 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, STRICT, 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"; - - 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 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()); - - // Start controller with BEST_EFFORT, will take effect at the end of the update function - // All the controller should be running. With the tag BEST_EFFORT, it should return OK. - 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); - - switch_future = std::async( std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, start_controllers, stop_controllers, BEST_EFFORT, true, rclcpp::Duration(0, 0)); From a385c7e514713587f0943556431ed680469e886c Mon Sep 17 00:00:00 2001 From: Xi Huang Date: Thu, 2 Dec 2021 14:59:23 +0100 Subject: [PATCH 3/9] Use parameterized test for STRICT and BEST_EFFORT. --- .../test/test_controller_manager.cpp | 158 +----------------- 1 file changed, 9 insertions(+), 149 deletions(-) diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index a1af605604..e05cb5a83d 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -26,11 +26,12 @@ using ::testing::_; using ::testing::Return; -class TestControllerManager : public ControllerManagerFixture +class TestControllerManager : public ControllerManagerFixture, + public testing::WithParamInterface { }; -TEST_F(TestControllerManager, controller_lifecycle) +TEST_P(TestControllerManager, controller_lifecycle) { auto test_controller = std::make_shared(); cm_->add_controller( @@ -62,7 +63,7 @@ TEST_F(TestControllerManager, controller_lifecycle) 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, GetParam(), 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 +89,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, GetParam(), 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,7 +119,7 @@ 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 test_controller = std::make_shared(); cm_->add_controller( @@ -151,7 +152,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, GetParam(), 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"; @@ -174,146 +175,5 @@ TEST_F(TestControllerManager, per_controller_update_rate) EXPECT_EQ(test_controller->get_update_rate(), 4u); } -TEST_F(TestControllerManager, controller_lifecycle_best_effort) -{ - auto test_controller = std::make_shared(); - 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(0u, 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()); - - // 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(0u, 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, BEST_EFFORT, 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"; - - 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 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()); - - 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); - auto last_internal_counter = test_controller->internal_counter; - - // Stop controller, will take effect at the end of the update function - start_controllers = {}; - stop_controllers = {test_controller::TEST_CONTROLLER_NAME}; - switch_future = std::async( - std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, - start_controllers, stop_controllers, BEST_EFFORT, 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"; - - EXPECT_EQ( - controller_interface::return_type::OK, - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); - EXPECT_EQ(last_internal_counter + 1u, test_controller->internal_counter) - << "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, switch_future.get()); - } - - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_state().id()); - 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()); - - EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id()); - EXPECT_EQ(1, test_controller.use_count()); -} - -TEST_F(TestControllerManager, per_controller_update_rate_best_effort) -{ - auto test_controller = std::make_shared(); - 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(0u, 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()); - - test_controller->get_node()->set_parameter({"update_rate", 4}); - // 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(0u, 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, BEST_EFFORT, 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"; - - 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 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()); - - 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); -} +INSTANTIATE_TEST_SUITE_P( + test_strick_best_effort, TestControllerManager, testing::Values(STRICT, BEST_EFFORT)); From 08dc71af9b0bf3b4c11a5612589ebaddea5a3e1c Mon Sep 17 00:00:00 2001 From: Xi Huang Date: Thu, 9 Dec 2021 17:47:47 +0100 Subject: [PATCH 4/9] Add multiple controllers. --- .vscode/settings.json | 68 ++++++++++++++++++ .../test/test_controller_manager.cpp | 43 ++++++++--- .../__pycache__/__init__.cpython-38.pyc | Bin 0 -> 188 bytes .../api/__pycache__/__init__.cpython-38.pyc | Bin 0 -> 4254 bytes .../__pycache__/__init__.cpython-38.pyc | Bin 0 -> 196 bytes .../verb/__pycache__/__init__.cpython-38.pyc | Bin 0 -> 193 bytes 6 files changed, 103 insertions(+), 8 deletions(-) create mode 100644 .vscode/settings.json create mode 100644 ros2controlcli/ros2controlcli/__pycache__/__init__.cpython-38.pyc create mode 100644 ros2controlcli/ros2controlcli/api/__pycache__/__init__.cpython-38.pyc create mode 100644 ros2controlcli/ros2controlcli/command/__pycache__/__init__.cpython-38.pyc create mode 100644 ros2controlcli/ros2controlcli/verb/__pycache__/__init__.cpython-38.pyc diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000000..32a0042511 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,68 @@ +{ + "files.associations": { + "array": "cpp", + "*.tcc": "cpp", + "functional": "cpp", + "istream": "cpp", + "tuple": "cpp", + "utility": "cpp", + "memory": "cpp", + "variant": "cpp", + "cctype": "cpp", + "clocale": "cpp", + "cmath": "cpp", + "cstdarg": "cpp", + "cstddef": "cpp", + "cstdio": "cpp", + "cstdlib": "cpp", + "cstring": "cpp", + "ctime": "cpp", + "cwchar": "cpp", + "cwctype": "cpp", + "atomic": "cpp", + "strstream": "cpp", + "bitset": "cpp", + "chrono": "cpp", + "complex": "cpp", + "condition_variable": "cpp", + "cstdint": "cpp", + "deque": "cpp", + "list": "cpp", + "unordered_map": "cpp", + "vector": "cpp", + "exception": "cpp", + "fstream": "cpp", + "initializer_list": "cpp", + "iomanip": "cpp", + "iosfwd": "cpp", + "iostream": "cpp", + "limits": "cpp", + "mutex": "cpp", + "new": "cpp", + "ostream": "cpp", + "numeric": "cpp", + "ratio": "cpp", + "sstream": "cpp", + "stdexcept": "cpp", + "streambuf": "cpp", + "system_error": "cpp", + "thread": "cpp", + "cfenv": "cpp", + "cinttypes": "cpp", + "regex": "cpp", + "type_traits": "cpp", + "typeindex": "cpp", + "typeinfo": "cpp", + "algorithm": "cpp", + "iterator": "cpp", + "map": "cpp", + "memory_resource": "cpp", + "optional": "cpp", + "random": "cpp", + "set": "cpp", + "string": "cpp", + "string_view": "cpp", + "shared_mutex": "cpp", + "bit": "cpp" + } +} diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index e05cb5a83d..9dc6f904d4 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,18 +27,26 @@ using ::testing::_; using ::testing::Return; -class TestControllerManager : public ControllerManagerFixture, - public testing::WithParamInterface +class TestControllerManager +: public ControllerManagerFixture, + public testing::WithParamInterface> { }; TEST_P(TestControllerManager, controller_lifecycle) { + const auto param = GetParam(); + auto strictness = param.first; + auto expected_return = param.second; 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( @@ -51,19 +60,34 @@ TEST_P(TestControllerManager, controller_lifecycle) // configure controller cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME); + // Comment out this line on purpose. Otherwise, the test will crash. + // 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, GetParam(), true, rclcpp::Duration(0, 0)); + start_controllers, stop_controllers, strictness, true, rclcpp::Duration(0, 0)); + + EXPECT_EQ(expected_return, switch_future.get()); + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + + // Start 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, 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"; @@ -89,7 +113,7 @@ TEST_P(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, GetParam(), 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"; @@ -121,6 +145,7 @@ TEST_P(TestControllerManager, controller_lifecycle) TEST_P(TestControllerManager, per_controller_update_rate) { + auto strictness = GetParam().first; auto test_controller = std::make_shared(); cm_->add_controller( test_controller, test_controller::TEST_CONTROLLER_NAME, @@ -152,7 +177,7 @@ TEST_P(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, GetParam(), 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"; @@ -175,5 +200,7 @@ TEST_P(TestControllerManager, per_controller_update_rate) EXPECT_EQ(test_controller->get_update_rate(), 4u); } +auto strict_pair = std::make_pair(STRICT, controller_interface::return_type::ERROR); +auto best_effort_pair = std::make_pair(BEST_EFFORT, controller_interface::return_type::OK); INSTANTIATE_TEST_SUITE_P( - test_strick_best_effort, TestControllerManager, testing::Values(STRICT, BEST_EFFORT)); + test_strict_best_effort, TestControllerManager, testing::Values(strict_pair, best_effort_pair)); diff --git a/ros2controlcli/ros2controlcli/__pycache__/__init__.cpython-38.pyc b/ros2controlcli/ros2controlcli/__pycache__/__init__.cpython-38.pyc new file mode 100644 index 0000000000000000000000000000000000000000..bccec83eec61d9f24ef09614f40fdd4a0e69c09c GIT binary patch literal 188 zcmWIL<>g`kg34{xi6Hthh(HF6K#l_t7qb9~6oz01O-8?!3`HPe1o10TKO;XkRX?LN zF)v-eD8JY!K0PrfF}Wl&S--d_86={coS#=xl%G=!k%scX94I?EClj3;AD@|*SrQ+w XS5SG2!zMRBr8Fni4rKLbAZ7pnE#flj literal 0 HcmV?d00001 diff --git a/ros2controlcli/ros2controlcli/api/__pycache__/__init__.cpython-38.pyc b/ros2controlcli/ros2controlcli/api/__pycache__/__init__.cpython-38.pyc new file mode 100644 index 0000000000000000000000000000000000000000..f4ae5ec065c5b655402db2ed321bf127ffe2475f GIT binary patch literal 4254 zcmbtY&5ztj6|b`0ZjYzuBbnKJOvtWxg;-kb^n|cMY!sqMlU**Tfkcp%5waXtb&n@* zds231cAB0G31T@!2#FhtG!3U*IPypE2XOEaPW%gt(DHj_clXTXYo&F|<*N6}@4b58 z?w>C$xeV9A-+vwcbCt1w)8hDLVsQ(1{wEBA37)V4_c^Dzkr)HhHwTt)4H|w!!_36? zZQ!h=IdFVO!y3uL!1Y}{x0A&|%Wq-c6wVXoFNp==;<+pqMGMaru_Tu9T>TLfD`NEt z6RVF6{|r`V#96G)Vs%!miFNE*6Xspke(OJwjJ0{Wn#6e#L|Iy>EJ>uw+h%!TJ4<_U zf2ic$B2)!dHL^Va<17?2>@ma|Ax`b?g1mb>Rx&EOnUGkWp=UQ7$n9*fm&igY0N0~1 zNdhg#UZ{WsIFCq$)2oSzgL@ly{x1xpe-!-4oG{Mtv;-fsqlPeqIWhhWB|bAJ{Lmbm zNA?()iE(I+t)u3I7nZU#k7S!LW`91a*?=HbtC+yG$^+iy!iBiSk9=pcC zh1Dl9==muqz(B38zoN4IYE&sdO5)%PzEv{c4U~L1lsU9CTHg=jB2N2WFH>GF)uT9) z-srqnWZr!lWdrGjkHRGYvFx^9g&wzFX(yfiC3q8~=Z{OwCK^4GmV_<*R_ zkn3K_-O;-re-_ETBF<88e$T=m@lgDj< zhx!H$muVm^tG8(&w>T#KTUz?l1U|v0lae!o8DA<7@UJiVJ6m&Yk;P7PhqcHFzKZx8 zb4sVFbYD0_rG0YF2=|GL9Pk$I`~n8ad@h)82=1Gd@KMh!VZqTF<@xPdnL1HnMjvg% z^C<-)k0H=g%2lC9^~TG3I{(#`&zqGWk6bG6oG3yz*i+SCBw21_&`!?(aP`nC_}Cg7 zyO8Rsafm|nd-gll^-4R`)+41G3czw!?Gm>;pv*@P==49yh$b8uT^zcnthKN=6AndclF zN=K$5-;ax((oAFufYG^9^7J_`?R?nl#h)GXJNE|ZhO*(!khl?B8}hqs^x-RX+zA4m zTZ7JxB*QGfiOG~m+f+nCQ3Ws0!Yk@#Vj9qGyR>qd^h$eANr+M|!VbAa%255hv>)u# zQ|riInOR6HDzBt&6LlT%&Qy+H0#rwdOTNXdFKyG}BkxTI=)#jgh3p>$r5gl;Obip6 zw}Rl|FidJpGYCW$1%di5F`q}So{=o-Jq(|c7?j=|ZW$W#f_oG87H#_g!)f;xmT2@X zxRNcJPdHrN5esnnhH&BXi{)i%x+L{uwMtJq{*~7{K4ljvj{AS7w5k3)lD-qC^~{C6 zs+=wS)-au~PUIT$PP(`0AQU9jcm=wVQQyQ8U0&-^n8Z4v6f%d;Zvw1-K*L;lQyW&* zi`tc(s4_nK)@x4+_p4rcb->;NnL1!Q^MHM9?3#xS9jsg!Zwywm`$9~T`E*QD2G=op zj$(2bF}ceh*c6q!Miq(rpy;j}NQY?N^x3HbKpF4jQS#_HuhcSkqVE7PzX78_lE zlP+EpidGehq*X-@hn{HWJZ-z;-XY``4OeKOM=Lvc3sZ zKW!ZHg3|}pQy45TG#;N-^hx#9#HZF0Vr+Tzi=R?5QXZ1faDaw1r{)?R=&`^-o2)w% z^wUD>((!>OWG_SysdrONJ(cr1BV#ift#3|SJoGo4HI>o(6vb4UiXAl9vsY(a?)>Dg z<}_M+3E@xRVAhUI_R3~0Lg`R>Numgcd5j;bwMxZDeKZ(k+}YK_h+S*m^rD|8`WD-=4p zYkev*Qn@f~z7_4)`CpYLbbdNJY|y)Nr38b%3aah-9th=>LYQGu&aZy|wVN%DHS!a> zq)&ituWQ#=6a@aIqdrr&Y`P)Wl|j3(7Vy}aXPKaMZd8@%CjM`zdZCuFZae7T R@iU!Ej_Wv%yWCoC{U6>XX~F;i literal 0 HcmV?d00001 diff --git a/ros2controlcli/ros2controlcli/command/__pycache__/__init__.cpython-38.pyc b/ros2controlcli/ros2controlcli/command/__pycache__/__init__.cpython-38.pyc new file mode 100644 index 0000000000000000000000000000000000000000..f52a0bbd93b80dff61e36f906023554172fcaee1 GIT binary patch literal 196 zcmWIL<>g`kg34{xi6Hthh(HF6K#l_t7qb9~6oz01O-8?!3`HPe1o10VKO;XkRX?LN zF)v-eD8JY!K0PrfF}Wl&S--d_86={coS#=xl%G=!k%scX94I?EClj5UoS&PUn3tj- fAD@|*SrQ+wS5SG2!zMRBr8Fni4&;Q-K+FID>UuT+ literal 0 HcmV?d00001 diff --git a/ros2controlcli/ros2controlcli/verb/__pycache__/__init__.cpython-38.pyc b/ros2controlcli/ros2controlcli/verb/__pycache__/__init__.cpython-38.pyc new file mode 100644 index 0000000000000000000000000000000000000000..b9514cd1d8ec32821112c46d3f974c5e1ff754bd GIT binary patch literal 193 zcmWIL<>g`kg34{xi6Hthh(HF6K#l_t7qb9~6oz01O-8?!3`HPe1o10PKO;XkRX?LN zF)v-eD8JY!K0PrfF}Wl&S--d_86={coS#=xl%G=!k%scX94I?EClj4pmRgjgA0MBY cmst`YuUAlci^C>2KczG$)edC;XCP((06&8?%>V!Z literal 0 HcmV?d00001 From 7639234c567d27597800d5d059017116a9f4b73a Mon Sep 17 00:00:00 2001 From: Xi Huang Date: Thu, 9 Dec 2021 17:53:05 +0100 Subject: [PATCH 5/9] Remove cache and setting files. --- .vscode/settings.json | 68 ------------------ .../__pycache__/__init__.cpython-38.pyc | Bin 188 -> 0 bytes .../api/__pycache__/__init__.cpython-38.pyc | Bin 4254 -> 0 bytes .../__pycache__/__init__.cpython-38.pyc | Bin 196 -> 0 bytes .../verb/__pycache__/__init__.cpython-38.pyc | Bin 193 -> 0 bytes 5 files changed, 68 deletions(-) delete mode 100644 .vscode/settings.json delete mode 100644 ros2controlcli/ros2controlcli/__pycache__/__init__.cpython-38.pyc delete mode 100644 ros2controlcli/ros2controlcli/api/__pycache__/__init__.cpython-38.pyc delete mode 100644 ros2controlcli/ros2controlcli/command/__pycache__/__init__.cpython-38.pyc delete mode 100644 ros2controlcli/ros2controlcli/verb/__pycache__/__init__.cpython-38.pyc diff --git a/.vscode/settings.json b/.vscode/settings.json deleted file mode 100644 index 32a0042511..0000000000 --- a/.vscode/settings.json +++ /dev/null @@ -1,68 +0,0 @@ -{ - "files.associations": { - "array": "cpp", - "*.tcc": "cpp", - "functional": "cpp", - "istream": "cpp", - "tuple": "cpp", - "utility": "cpp", - "memory": "cpp", - "variant": "cpp", - "cctype": "cpp", - "clocale": "cpp", - "cmath": "cpp", - "cstdarg": "cpp", - "cstddef": "cpp", - "cstdio": "cpp", - "cstdlib": "cpp", - "cstring": "cpp", - "ctime": "cpp", - "cwchar": "cpp", - "cwctype": "cpp", - "atomic": "cpp", - "strstream": "cpp", - "bitset": "cpp", - "chrono": "cpp", - "complex": "cpp", - "condition_variable": "cpp", - "cstdint": "cpp", - "deque": "cpp", - "list": "cpp", - "unordered_map": "cpp", - "vector": "cpp", - "exception": "cpp", - "fstream": "cpp", - "initializer_list": "cpp", - "iomanip": "cpp", - "iosfwd": "cpp", - "iostream": "cpp", - "limits": "cpp", - "mutex": "cpp", - "new": "cpp", - "ostream": "cpp", - "numeric": "cpp", - "ratio": "cpp", - "sstream": "cpp", - "stdexcept": "cpp", - "streambuf": "cpp", - "system_error": "cpp", - "thread": "cpp", - "cfenv": "cpp", - "cinttypes": "cpp", - "regex": "cpp", - "type_traits": "cpp", - "typeindex": "cpp", - "typeinfo": "cpp", - "algorithm": "cpp", - "iterator": "cpp", - "map": "cpp", - "memory_resource": "cpp", - "optional": "cpp", - "random": "cpp", - "set": "cpp", - "string": "cpp", - "string_view": "cpp", - "shared_mutex": "cpp", - "bit": "cpp" - } -} diff --git a/ros2controlcli/ros2controlcli/__pycache__/__init__.cpython-38.pyc b/ros2controlcli/ros2controlcli/__pycache__/__init__.cpython-38.pyc deleted file mode 100644 index bccec83eec61d9f24ef09614f40fdd4a0e69c09c..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 188 zcmWIL<>g`kg34{xi6Hthh(HF6K#l_t7qb9~6oz01O-8?!3`HPe1o10TKO;XkRX?LN zF)v-eD8JY!K0PrfF}Wl&S--d_86={coS#=xl%G=!k%scX94I?EClj3;AD@|*SrQ+w XS5SG2!zMRBr8Fni4rKLbAZ7pnE#flj diff --git a/ros2controlcli/ros2controlcli/api/__pycache__/__init__.cpython-38.pyc b/ros2controlcli/ros2controlcli/api/__pycache__/__init__.cpython-38.pyc deleted file mode 100644 index f4ae5ec065c5b655402db2ed321bf127ffe2475f..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 4254 zcmbtY&5ztj6|b`0ZjYzuBbnKJOvtWxg;-kb^n|cMY!sqMlU**Tfkcp%5waXtb&n@* zds231cAB0G31T@!2#FhtG!3U*IPypE2XOEaPW%gt(DHj_clXTXYo&F|<*N6}@4b58 z?w>C$xeV9A-+vwcbCt1w)8hDLVsQ(1{wEBA37)V4_c^Dzkr)HhHwTt)4H|w!!_36? zZQ!h=IdFVO!y3uL!1Y}{x0A&|%Wq-c6wVXoFNp==;<+pqMGMaru_Tu9T>TLfD`NEt z6RVF6{|r`V#96G)Vs%!miFNE*6Xspke(OJwjJ0{Wn#6e#L|Iy>EJ>uw+h%!TJ4<_U zf2ic$B2)!dHL^Va<17?2>@ma|Ax`b?g1mb>Rx&EOnUGkWp=UQ7$n9*fm&igY0N0~1 zNdhg#UZ{WsIFCq$)2oSzgL@ly{x1xpe-!-4oG{Mtv;-fsqlPeqIWhhWB|bAJ{Lmbm zNA?()iE(I+t)u3I7nZU#k7S!LW`91a*?=HbtC+yG$^+iy!iBiSk9=pcC zh1Dl9==muqz(B38zoN4IYE&sdO5)%PzEv{c4U~L1lsU9CTHg=jB2N2WFH>GF)uT9) z-srqnWZr!lWdrGjkHRGYvFx^9g&wzFX(yfiC3q8~=Z{OwCK^4GmV_<*R_ zkn3K_-O;-re-_ETBF<88e$T=m@lgDj< zhx!H$muVm^tG8(&w>T#KTUz?l1U|v0lae!o8DA<7@UJiVJ6m&Yk;P7PhqcHFzKZx8 zb4sVFbYD0_rG0YF2=|GL9Pk$I`~n8ad@h)82=1Gd@KMh!VZqTF<@xPdnL1HnMjvg% z^C<-)k0H=g%2lC9^~TG3I{(#`&zqGWk6bG6oG3yz*i+SCBw21_&`!?(aP`nC_}Cg7 zyO8Rsafm|nd-gll^-4R`)+41G3czw!?Gm>;pv*@P==49yh$b8uT^zcnthKN=6AndclF zN=K$5-;ax((oAFufYG^9^7J_`?R?nl#h)GXJNE|ZhO*(!khl?B8}hqs^x-RX+zA4m zTZ7JxB*QGfiOG~m+f+nCQ3Ws0!Yk@#Vj9qGyR>qd^h$eANr+M|!VbAa%255hv>)u# zQ|riInOR6HDzBt&6LlT%&Qy+H0#rwdOTNXdFKyG}BkxTI=)#jgh3p>$r5gl;Obip6 zw}Rl|FidJpGYCW$1%di5F`q}So{=o-Jq(|c7?j=|ZW$W#f_oG87H#_g!)f;xmT2@X zxRNcJPdHrN5esnnhH&BXi{)i%x+L{uwMtJq{*~7{K4ljvj{AS7w5k3)lD-qC^~{C6 zs+=wS)-au~PUIT$PP(`0AQU9jcm=wVQQyQ8U0&-^n8Z4v6f%d;Zvw1-K*L;lQyW&* zi`tc(s4_nK)@x4+_p4rcb->;NnL1!Q^MHM9?3#xS9jsg!Zwywm`$9~T`E*QD2G=op zj$(2bF}ceh*c6q!Miq(rpy;j}NQY?N^x3HbKpF4jQS#_HuhcSkqVE7PzX78_lE zlP+EpidGehq*X-@hn{HWJZ-z;-XY``4OeKOM=Lvc3sZ zKW!ZHg3|}pQy45TG#;N-^hx#9#HZF0Vr+Tzi=R?5QXZ1faDaw1r{)?R=&`^-o2)w% z^wUD>((!>OWG_SysdrONJ(cr1BV#ift#3|SJoGo4HI>o(6vb4UiXAl9vsY(a?)>Dg z<}_M+3E@xRVAhUI_R3~0Lg`R>Numgcd5j;bwMxZDeKZ(k+}YK_h+S*m^rD|8`WD-=4p zYkev*Qn@f~z7_4)`CpYLbbdNJY|y)Nr38b%3aah-9th=>LYQGu&aZy|wVN%DHS!a> zq)&ituWQ#=6a@aIqdrr&Y`P)Wl|j3(7Vy}aXPKaMZd8@%CjM`zdZCuFZae7T R@iU!Ej_Wv%yWCoC{U6>XX~F;i diff --git a/ros2controlcli/ros2controlcli/command/__pycache__/__init__.cpython-38.pyc b/ros2controlcli/ros2controlcli/command/__pycache__/__init__.cpython-38.pyc deleted file mode 100644 index f52a0bbd93b80dff61e36f906023554172fcaee1..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 196 zcmWIL<>g`kg34{xi6Hthh(HF6K#l_t7qb9~6oz01O-8?!3`HPe1o10VKO;XkRX?LN zF)v-eD8JY!K0PrfF}Wl&S--d_86={coS#=xl%G=!k%scX94I?EClj5UoS&PUn3tj- fAD@|*SrQ+wS5SG2!zMRBr8Fni4&;Q-K+FID>UuT+ diff --git a/ros2controlcli/ros2controlcli/verb/__pycache__/__init__.cpython-38.pyc b/ros2controlcli/ros2controlcli/verb/__pycache__/__init__.cpython-38.pyc deleted file mode 100644 index b9514cd1d8ec32821112c46d3f974c5e1ff754bd..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 193 zcmWIL<>g`kg34{xi6Hthh(HF6K#l_t7qb9~6oz01O-8?!3`HPe1o10PKO;XkRX?LN zF)v-eD8JY!K0PrfF}Wl&S--d_86={coS#=xl%G=!k%scX94I?EClj4pmRgjgA0MBY cmst`YuUAlci^C>2KczG$)edC;XCP((06&8?%>V!Z From 01ed035c80de8bb941f19dffd3bb3fcb796204cf Mon Sep 17 00:00:00 2001 From: Xi Huang Date: Thu, 3 Feb 2022 13:55:04 +0100 Subject: [PATCH 6/9] Fix the bug in the test. All tests are passed. --- .../test/test_controller_manager.cpp | 39 +++++++++++++------ 1 file changed, 27 insertions(+), 12 deletions(-) diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index 9dc6f904d4..899bc1ff11 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -27,17 +27,23 @@ using ::testing::_; using ::testing::Return; -class TestControllerManager -: public ControllerManagerFixture, - public testing::WithParamInterface> +struct Strictness +{ + int strictness = STRICT; + controller_interface::return_type expected_return; + unsigned int expected_counter; +}; +class TestControllerManager : public ControllerManagerFixture, + public testing::WithParamInterface { }; TEST_P(TestControllerManager, controller_lifecycle) { const auto param = GetParam(); - auto strictness = param.first; - auto expected_return = param.second; + auto strictness = param.strictness; + auto expected_return = param.expected_return; + auto expected_counter = param.expected_counter; auto test_controller = std::make_shared(); auto test_controller2 = std::make_shared(); constexpr char TEST_CONTROLLER2_NAME[] = "test_controller2_name"; @@ -61,7 +67,7 @@ TEST_P(TestControllerManager, controller_lifecycle) // configure controller cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME); // Comment out this line on purpose. Otherwise, the test will crash. - // cm_->configure_controller(TEST_CONTROLLER2_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))); @@ -77,12 +83,21 @@ TEST_P(TestControllerManager, controller_lifecycle) std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, start_controllers, stop_controllers, strictness, true, rclcpp::Duration(0, 0)); - EXPECT_EQ(expected_return, switch_future.get()); 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(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, expected_counter); - // Start controller, will take effect at the end of the update function + // 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( @@ -145,7 +160,7 @@ TEST_P(TestControllerManager, controller_lifecycle) TEST_P(TestControllerManager, per_controller_update_rate) { - auto strictness = GetParam().first; + auto strictness = GetParam().strictness; auto test_controller = std::make_shared(); cm_->add_controller( test_controller, test_controller::TEST_CONTROLLER_NAME, @@ -200,7 +215,7 @@ TEST_P(TestControllerManager, per_controller_update_rate) EXPECT_EQ(test_controller->get_update_rate(), 4u); } -auto strict_pair = std::make_pair(STRICT, controller_interface::return_type::ERROR); -auto best_effort_pair = std::make_pair(BEST_EFFORT, controller_interface::return_type::OK); +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_pair, best_effort_pair)); + test_strict_best_effort, TestControllerManager, testing::Values(strict, best_effort)); From 1ad395d31645912f2a2321a23bed915b9a290a90 Mon Sep 17 00:00:00 2001 From: Xi-Huang Date: Thu, 3 Feb 2022 14:48:25 +0100 Subject: [PATCH 7/9] Apply suggestions from code review MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Denis Štogl --- controller_manager/test/test_controller_manager.cpp | 12 ++++-------- 1 file changed, 4 insertions(+), 8 deletions(-) diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index 899bc1ff11..d7d2bbab57 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -40,10 +40,7 @@ class TestControllerManager : public ControllerManagerFixture, TEST_P(TestControllerManager, controller_lifecycle) { - const auto param = GetParam(); - auto strictness = param.strictness; - auto expected_return = param.expected_return; - auto expected_counter = param.expected_counter; + 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"; @@ -66,7 +63,6 @@ TEST_P(TestControllerManager, controller_lifecycle) // configure controller cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME); - // Comment out this line on purpose. Otherwise, the test will crash. cm_->configure_controller(TEST_CONTROLLER2_NAME); EXPECT_EQ( controller_interface::return_type::OK, @@ -89,20 +85,20 @@ TEST_P(TestControllerManager, controller_lifecycle) EXPECT_EQ(0u, test_controller2->internal_counter) << "Controller is started at the end of update"; { ControllerManagerRunner cm_runner(this); - EXPECT_EQ(expected_return, switch_future.get()); + 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, expected_counter); + 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, strictness, 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"; From e8e356ab00dfe9b80ed0c67631aaf4593b20a35e Mon Sep 17 00:00:00 2001 From: Xi-Huang Date: Thu, 3 Feb 2022 14:48:47 +0100 Subject: [PATCH 8/9] Update controller_manager/test/test_controller_manager.cpp MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Denis Štogl --- controller_manager/test/test_controller_manager.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index d7d2bbab57..a8cae08a4f 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -124,7 +124,7 @@ TEST_P(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, strictness, 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"; From b045efb0e92f76c05677f6064f307de743bc0e17 Mon Sep 17 00:00:00 2001 From: Xi-Huang Date: Fri, 4 Feb 2022 10:02:42 +0100 Subject: [PATCH 9/9] Fix the a bug to make the review suggestion work. --- controller_manager/test/test_controller_manager.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index a8cae08a4f..ea2d7300be 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -77,7 +77,7 @@ TEST_P(TestControllerManager, controller_lifecycle) 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_controllers, stop_controllers, test_param.strictness, true, rclcpp::Duration(0, 0)); EXPECT_EQ( controller_interface::return_type::OK,