From d39ddcdb2a6cf423530f93587646f2ce502b85d4 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 4 Jul 2023 21:32:56 +0200 Subject: [PATCH] Fix equal and higher controller update rate (#1070) --- controller_manager/src/controller_manager.cpp | 17 ++- .../test/test_controller/test_controller.hpp | 2 +- .../test/test_controller_manager.cpp | 121 +++++++++++++++++- 3 files changed, 131 insertions(+), 9 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 58f91a786d..cb875f25de 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -610,6 +610,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()) { @@ -1873,8 +1885,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", 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 diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index 22221b6e21..5a452bb0b1 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(); @@ -254,4 +254,113 @@ TEST_P(TestControllerManager, per_controller_update_rate) } INSTANTIATE_TEST_SUITE_P( - test_strict_best_effort, TestControllerManager, testing::Values(strict, best_effort)); + test_strict_best_effort, TestControllerManagerWithStrictness, + testing::Values(strict, best_effort)); + +class TestControllerManagerWithUpdateRates +: public ControllerManagerFixture, + public testing::WithParamInterface +{ +}; + +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; + RCLCPP_INFO( + rclcpp::get_logger("test_controller_manager"), "Testing update rate : %u 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( + per_controller_equal_and_higher_update_rate, TestControllerManagerWithUpdateRates, + testing::Values(100, 232, 400));