diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index c4887db15a..0c47febf27 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -621,6 +621,19 @@ controller_interface::return_type ControllerManager::configure_controller( "update rate.", controller_name.c_str(), controller_update_rate, cm_update_rate); } + else if (controller_update_rate != 0 && cm_update_rate % controller_update_rate != 0) + { + // NOTE: The following computation is done to compute the approx controller update that can be + // achieved w.r.t to the CM's update rate. This is done this way to take into account the + // unsigned integer division. + const auto act_ctrl_update_rate = cm_update_rate / (cm_update_rate / controller_update_rate); + RCLCPP_WARN( + get_logger(), + "The controller : %s update rate : %d Hz is not a perfect divisor of the controller " + "manager's update rate : %d Hz!. The controller will be updated with nearest divisor's " + "update rate which is : %d Hz.", + controller_name.c_str(), controller_update_rate, cm_update_rate, act_ctrl_update_rate); + } // CHAINABLE CONTROLLERS: get reference interfaces from chainable controllers if (controller->is_chainable()) @@ -1887,10 +1900,12 @@ controller_interface::return_type ControllerManager::update( if (!loaded_controller.c->is_async() && is_controller_active(*loaded_controller.c)) { const auto controller_update_rate = loaded_controller.c->get_update_rate(); + const auto controller_update_factor = + (controller_update_rate == 0) || (controller_update_rate >= update_rate_) + ? 1u + : update_rate_ / controller_update_rate; - bool controller_go = controller_update_rate == 0 || - ((update_loop_counter_ % controller_update_rate) == 0) || - (controller_update_rate >= update_rate_); + bool controller_go = ((update_loop_counter_ % controller_update_factor) == 0); RCLCPP_DEBUG( get_logger(), "update_loop_counter: '%d ' controller_go: '%s ' controller_name: '%s '", update_loop_counter_, controller_go ? "True" : "False", @@ -1899,7 +1914,7 @@ controller_interface::return_type ControllerManager::update( if (controller_go) { auto controller_ret = loaded_controller.c->update( - time, (controller_update_rate != update_rate_ && controller_update_rate != 0) + time, (controller_update_factor != 1u) ? rclcpp::Duration::from_seconds(1.0 / controller_update_rate) : period); diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index 5a452bb0b1..6db4cfd1b2 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -246,9 +246,13 @@ TEST_P(TestControllerManagerWithStrictness, per_controller_update_rate) 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))); + // As the controller frequency is 4Hz, it needs to pass 25 iterations for 1 update cycle + for (size_t i = 0; i < 25; i++) + { + 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); } @@ -364,3 +368,82 @@ 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)); + +class TestControllerUpdateRates +: public ControllerManagerFixture, + public testing::WithParamInterface +{ +}; + +TEST_P(TestControllerUpdateRates, check_the_controller_update_rate) +{ + const unsigned int ctrl_update_rate = GetParam(); + 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( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id()); + + test_controller->get_node()->set_parameter({"update_rate", static_cast(ctrl_update_rate)}); + // 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 + const auto strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT; + 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(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(test_controller->get_update_rate(), ctrl_update_rate); + const auto cm_update_rate = cm_->get_update_rate(); + const auto controller_update_rate = test_controller->get_update_rate(); + const auto controller_factor = (cm_update_rate / controller_update_rate); + const auto expected_controller_update_rate = + static_cast(std::round(cm_update_rate / static_cast(controller_factor))); + + const auto initial_counter = test_controller->internal_counter; + for (size_t update_counter = 1; update_counter <= 10 * cm_update_rate; ++update_counter) + { + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + + if (update_counter % cm_update_rate == 0) + { + const auto no_of_secs_passed = update_counter / cm_update_rate; + EXPECT_EQ( + test_controller->internal_counter - initial_counter, + (expected_controller_update_rate * no_of_secs_passed)); + } + } +} + +INSTANTIATE_TEST_SUITE_P( + per_controller_update_rate_check, TestControllerUpdateRates, + testing::Values(10, 12, 16, 23, 37, 40, 50, 63, 71, 85, 98));