From 99d2f61b53c3be1181c36557edd8b28b309bf86f Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 21 Nov 2023 18:42:50 +0100 Subject: [PATCH] Compute the actual update period for each controller (#1140) --- controller_manager/src/controller_manager.cpp | 4 +++- .../test/test_controller/test_controller.cpp | 3 ++- .../test/test_controller/test_controller.hpp | 1 + controller_manager/test/test_controller_manager.cpp | 12 ++++++++++++ 4 files changed, 18 insertions(+), 2 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 640dac8f5d..1c3433058b 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -2063,7 +2063,9 @@ controller_interface::return_type ControllerManager::update( if (controller_go) { - auto controller_ret = loaded_controller.c->update(time, controller_period); + const auto controller_actual_period = + (time - *loaded_controller.next_update_cycle_time) + controller_period; + auto controller_ret = loaded_controller.c->update(time, controller_actual_period); if ( *loaded_controller.next_update_cycle_time == diff --git a/controller_manager/test/test_controller/test_controller.cpp b/controller_manager/test/test_controller/test_controller.cpp index 06f76bd044..7585ae36e5 100644 --- a/controller_manager/test/test_controller/test_controller.cpp +++ b/controller_manager/test/test_controller/test_controller.cpp @@ -60,8 +60,9 @@ controller_interface::InterfaceConfiguration TestController::state_interface_con } controller_interface::return_type TestController::update( - const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) + const rclcpp::Time & /*time*/, const rclcpp::Duration & period) { + update_period_ = period.seconds(); ++internal_counter; // set value to hardware to produce and test different behaviors there diff --git a/controller_manager/test/test_controller/test_controller.hpp b/controller_manager/test/test_controller/test_controller.hpp index d7b318c597..14ad753803 100644 --- a/controller_manager/test/test_controller/test_controller.hpp +++ b/controller_manager/test/test_controller/test_controller.hpp @@ -80,6 +80,7 @@ class TestController : public controller_interface::ControllerInterface // enables external setting of values to command interfaces - used for simulation of hardware // errors double set_first_command_interface_value_to; + double update_period_ = 0; }; } // namespace test_controller diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index 8769a24749..4c9603ab42 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -351,6 +351,7 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd ASSERT_EQ(std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100))) << "switch_controller should be blocking until next update cycle"; + time_ += rclcpp::Duration::from_seconds(0.01); EXPECT_EQ( controller_interface::return_type::OK, cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); @@ -365,11 +366,17 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd const auto pre_internal_counter = test_controller->internal_counter; rclcpp::Rate loop_rate(cm_->get_update_rate()); + const auto cm_update_rate = cm_->get_update_rate(); for (size_t i = 0; i < 2 * cm_->get_update_rate(); i++) { + time_ += rclcpp::Duration::from_seconds(0.01); EXPECT_EQ( controller_interface::return_type::OK, cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + // In case of a non perfect divisor, the update period should respect the rule + // [controller_update_rate, 2*controller_update_rate) + ASSERT_GE(test_controller->update_period_, 1.0 / cm_update_rate); + ASSERT_LT(test_controller->update_period_, 2.0 / cm_update_rate); loop_rate.sleep(); } // if we do 2 times of the controller_manager update rate, the internal counter should be @@ -448,6 +455,7 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate) ASSERT_EQ(std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100))) << "switch_controller should be blocking until next update cycle"; + time_ += rclcpp::Duration::from_seconds(0.01); EXPECT_EQ( controller_interface::return_type::OK, cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); @@ -470,6 +478,10 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate) EXPECT_EQ( controller_interface::return_type::OK, cm_->update(time, rclcpp::Duration::from_seconds(0.01))); + // In case of a non perfect divisor, the update period should respect the rule + // [controller_update_rate, 2*controller_update_rate) + ASSERT_GE(test_controller->update_period_, 1.0 / controller_update_rate); + ASSERT_LT(test_controller->update_period_, 2.0 / controller_update_rate); time += rclcpp::Duration::from_seconds(0.01); if (update_counter % cm_update_rate == 0)