From 925b09af1f1d26343f71ba44f2a0b5495b458147 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 25 Jul 2023 19:55:32 +0200 Subject: [PATCH] add warning for the update rate to be a perfect factor --- controller_manager/src/controller_manager.cpp | 18 +++++++++++++++--- 1 file changed, 15 insertions(+), 3 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index c4887db15ab..dfaf9f68ce2 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -621,6 +621,16 @@ 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) + { + 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 +1897,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",