From bb035a5350641d5bf0007416a3a803dc6e74ed0b Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sat, 1 Jul 2023 21:29:09 +0200 Subject: [PATCH 1/6] Added test case to test the controllers with equal and higher update rates w.r.t controller manager --- .../test/test_controller_manager.cpp | 104 ++++++++++++++++++ 1 file changed, 104 insertions(+) diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index 22221b6e21..103b3af80a 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -253,5 +253,109 @@ 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) +{ + auto strictness = GetParam().strictness; + 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 : %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"; + + 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()); + + // 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"; + + 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); + + 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"; + + 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"; + 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)); From 2ca3c7bb0d5f827485864d58fef34c685b6884d3 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sat, 1 Jul 2023 21:29:48 +0200 Subject: [PATCH 2/6] Added the fix to solve inconsistency w.r.t controller update rate --- controller_manager/src/controller_manager.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 69a3bbb5f3..83be5ffbd0 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -1816,8 +1816,9 @@ controller_interface::return_type ControllerManager::update( { const auto controller_update_rate = loaded_controller.c->get_update_rate(); - bool controller_go = - controller_update_rate == 0 || ((update_loop_counter_ % controller_update_rate) == 0); + bool controller_go = controller_update_rate == 0 || + ((update_loop_counter_ % controller_update_rate) == 0) || + (controller_update_rate >= update_rate_); RCLCPP_DEBUG( get_logger(), "update_loop_counter: '%d ' controller_go: '%s ' controller_name: '%s '", update_loop_counter_, controller_go ? "True" : "False", From f3c1e8ec4e935dc6ade3bd270baa588dc1c5560c Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 3 Jul 2023 20:45:43 +0200 Subject: [PATCH 3/6] added logging for expectation management --- controller_manager/src/controller_manager.cpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 83be5ffbd0..e3d5c5ccf3 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -553,6 +553,18 @@ controller_interface::return_type ControllerManager::configure_controller( controller_name, std::make_unique(controller, update_rate_)); } + const auto controller_update_rate = controller->get_update_rate(); + const auto cm_update_rate = get_update_rate(); + if (controller_update_rate > cm_update_rate) + { + RCLCPP_WARN( + get_logger(), + "The controller : %s update rate : %d Hz should be less than or equal to controller " + "manager's update rate : %d Hz!. The controller will be updated at controller_manager's " + "update rate.", + controller_name.c_str(), controller_update_rate, cm_update_rate); + } + // CHAINABLE CONTROLLERS: get reference interfaces from chainable controllers if (controller->is_chainable()) { From 8a2021e64e36f2ee35535d6e438f713675dc33b1 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 4 Jul 2023 19:51:43 +0100 Subject: [PATCH 4/6] Clean up some compilation warnings --- controller_manager/test/test_controller/test_controller.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controller_manager/test/test_controller/test_controller.hpp b/controller_manager/test/test_controller/test_controller.hpp index 1b070ab302..f73f592037 100644 --- a/controller_manager/test/test_controller/test_controller.hpp +++ b/controller_manager/test/test_controller/test_controller.hpp @@ -65,7 +65,7 @@ class TestController : public controller_interface::ControllerInterface CONTROLLER_MANAGER_PUBLIC void set_state_interface_configuration(const controller_interface::InterfaceConfiguration & cfg); - size_t internal_counter = 0; + unsigned int internal_counter = 0; bool simulate_cleanup_failure = false; // Variable where we store when cleanup was called, pointer because the controller // is usually destroyed after cleanup From 505743a14309eb5f0606152026265cc8a184aebb Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 4 Jul 2023 19:51:56 +0100 Subject: [PATCH 5/6] 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)); From 9eb95d07d4231807e8a35af950968d9ad2e319a4 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 4 Jul 2023 19:53:38 +0100 Subject: [PATCH 6/6] reformat --- controller_manager/test/test_controller_manager.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index fe605243de..5a452bb0b1 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -362,4 +362,5 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd } INSTANTIATE_TEST_SUITE_P( - per_controller_equal_and_higher_update_rate, TestControllerManagerWithUpdateRates, testing::Values(100, 232, 400)); + per_controller_equal_and_higher_update_rate, TestControllerManagerWithUpdateRates, + testing::Values(100, 232, 400));