From 28711db71ab4867e26376b1cbb5aea0d27f973b9 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 3 Oct 2023 19:50:04 +0200 Subject: [PATCH] Proper controller update rate (#1105) --- .../controller_manager/controller_manager.hpp | 1 + .../controller_manager/controller_spec.hpp | 2 + controller_manager/src/controller_manager.cpp | 42 +++++++++++-------- .../test/test_controller_manager.cpp | 19 +++++---- 4 files changed, 39 insertions(+), 25 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 2850b2be80..3ef8882a5b 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -122,6 +122,7 @@ class ControllerManager : public rclcpp::Node controller_spec.c = controller; controller_spec.info.name = controller_name; controller_spec.info.type = controller_type; + controller_spec.next_update_cycle_time = std::make_shared(0); return add_controller_impl(controller_spec); } diff --git a/controller_manager/include/controller_manager/controller_spec.hpp b/controller_manager/include/controller_manager/controller_spec.hpp index 63823f8d33..6f7483f3ec 100644 --- a/controller_manager/include/controller_manager/controller_spec.hpp +++ b/controller_manager/include/controller_manager/controller_spec.hpp @@ -20,6 +20,7 @@ #define CONTROLLER_MANAGER__CONTROLLER_SPEC_HPP_ #include +#include #include #include #include "controller_interface/controller_interface.hpp" @@ -37,6 +38,7 @@ struct ControllerSpec { hardware_interface::ControllerInfo info; controller_interface::ControllerInterfaceBaseSharedPtr c; + std::shared_ptr next_update_cycle_time; }; } // namespace controller_manager diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index c13eb74a80..6bb37a13b0 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -565,6 +565,7 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c controller_spec.c = controller; controller_spec.info.name = controller_name; controller_spec.info.type = controller_type; + controller_spec.next_update_cycle_time = std::make_shared(0); return add_controller_impl(controller_spec); } @@ -740,16 +741,13 @@ controller_interface::return_type ControllerManager::configure_controller( } 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); + "The controller : %s update cycles won't be triggered at a constant period : %f sec, as the " + "controller's update rate : %d Hz is not a perfect divisor of the controller manager's " + "update rate : %d Hz!.", + controller_name.c_str(), 1.0 / controller_update_rate, controller_update_rate, + cm_update_rate); } // CHAINABLE CONTROLLERS: get reference interfaces from chainable controllers @@ -1458,6 +1456,8 @@ void ControllerManager::activate_controllers( } auto controller = found_it->c; auto controller_name = found_it->info.name; + // reset the next update cycle time for newly activated controllers + *found_it->next_update_cycle_time = rclcpp::Time(0); bool assignment_successful = true; // assign command interfaces to the controller @@ -2030,19 +2030,22 @@ controller_interface::return_type ControllerManager::update( ++update_loop_counter_; update_loop_counter_ %= update_rate_; - for (auto loaded_controller : rt_controller_list) + for (const auto & loaded_controller : rt_controller_list) { // TODO(v-lopez) we could cache this information // https://github.com/ros-controls/ros2_control/issues/153 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; + const bool run_controller_at_cm_rate = + (controller_update_rate == 0) || (controller_update_rate >= update_rate_); + const auto controller_period = + run_controller_at_cm_rate ? period + : rclcpp::Duration::from_seconds((1.0 / controller_update_rate)); + + bool controller_go = (time == rclcpp::Time(0)) || + (time.seconds() >= loaded_controller.next_update_cycle_time->seconds()); - 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", @@ -2050,10 +2053,13 @@ controller_interface::return_type ControllerManager::update( if (controller_go) { - auto controller_ret = loaded_controller.c->update( - time, (controller_update_factor != 1u) - ? rclcpp::Duration::from_seconds(1.0 / controller_update_rate) - : period); + auto controller_ret = loaded_controller.c->update(time, controller_period); + + if (*loaded_controller.next_update_cycle_time == rclcpp::Time(0)) + { + *loaded_controller.next_update_cycle_time = time; + } + *loaded_controller.next_update_cycle_time += controller_period; if (controller_ret != controller_interface::return_type::OK) { diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index 6db4cfd1b2..b4d66f9039 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -423,23 +423,28 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate) 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) + rclcpp::Time time = rclcpp::Time(0); + for (size_t update_counter = 0; 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))); + cm_->update(time, rclcpp::Duration::from_seconds(0.01))); + time += 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( + // NOTE: here EXPECT_NEAR is used because it is observed that in the first iteration of whole + // cycle of cm_update_rate counts, there is one count missing, but in rest of the 9 cycles it + // is clearly tracking, so adding 1 here won't affect the final count. + // For instance, a controller with update rate 37 Hz, seems to have 36 in the first update + // cycle and then on accumulating 37 on every other update cycle so at the end of the 10 + // cycles it will have 369 instead of 370. + EXPECT_NEAR( test_controller->internal_counter - initial_counter, - (expected_controller_update_rate * no_of_secs_passed)); + (controller_update_rate * no_of_secs_passed), 1); } } }