From fe199f537afbd0c85f0fb416bda4a83f026fed5d Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 22 Sep 2025 11:29:07 +0200 Subject: [PATCH] [Controllers] Receive a valid period on the first update cycle --- controller_manager/src/controller_manager.cpp | 18 +++++------------- .../test/test_controller_manager.cpp | 14 ++++++++++---- 2 files changed, 15 insertions(+), 17 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 63d21cb2a2..b3a01dd653 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -2948,21 +2948,13 @@ controller_interface::return_type ControllerManager::update( run_controller_at_cm_rate ? period : rclcpp::Duration::from_seconds((1.0 / controller_update_rate)); - bool first_update_cycle = false; + const bool first_update_cycle = + (*loaded_controller.last_update_cycle_time == + rclcpp::Time(0, 0, this->get_trigger_clock()->get_clock_type())); const rclcpp::Time current_time = get_clock()->started() ? get_trigger_clock()->now() : time; - if ( - *loaded_controller.last_update_cycle_time == - rclcpp::Time(0, 0, this->get_trigger_clock()->get_clock_type())) - { - // last_update_cycle_time is zero after activation - first_update_cycle = true; - *loaded_controller.last_update_cycle_time = current_time; - RCLCPP_DEBUG( - get_logger(), "Setting last_update_cycle_time to %fs for the controller : %s", - loaded_controller.last_update_cycle_time->seconds(), loaded_controller.info.name.c_str()); - } const auto controller_actual_period = - (current_time - *loaded_controller.last_update_cycle_time); + first_update_cycle ? controller_period + : (current_time - *loaded_controller.last_update_cycle_time); /// @note The factor 0.99 is used to avoid the controllers skipping update cycles due to the /// jitter in the system sleep cycles. diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index ac1a869600..958af9e9c0 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -472,8 +472,11 @@ TEST_P(TestControllerManagerWithStrictness, async_controller_lifecycle) std::this_thread::sleep_for( std::chrono::milliseconds(1000 / (test_controller->get_update_rate()))); EXPECT_EQ(test_controller->internal_counter, 1u); - EXPECT_EQ(test_controller->update_period_.seconds(), 0.0) - << "The first trigger cycle should have zero period"; + EXPECT_NEAR( + test_controller->update_period_.seconds(), + 1.0 / (static_cast(test_controller->get_update_rate())), 1.e-6) + << "The first trigger cycle should have non-zero period to allow for integration in the " + "controllers"; const double exp_period = (cm_->get_trigger_clock()->now() - time_).seconds(); time_ = cm_->get_trigger_clock()->now(); @@ -1061,8 +1064,11 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate) } else { - // Check that the first cycle update period is zero - EXPECT_EQ(test_controller->update_period_.seconds(), 0.0); + EXPECT_NEAR( + test_controller->update_period_.seconds(), + 1.0 / (static_cast(test_controller->get_update_rate())), 1.e-6) + << "The first trigger cycle should have non-zero period to allow for integration in the " + "controllers"; } if (update_counter > 0 && update_counter % cm_update_rate == 0)