From 380af894cf53536def9a067d2ea6cc4dc3805f32 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 24 May 2024 18:31:55 +0000 Subject: [PATCH] Fix the initialization of the next update time --- controller_manager/src/controller_manager.cpp | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 41fe1ad95d..1b9a2a713d 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -2147,6 +2147,15 @@ controller_interface::return_type ControllerManager::update( run_controller_at_cm_rate ? period : rclcpp::Duration::from_seconds((1.0 / controller_update_rate)); + if ( + *loaded_controller.next_update_cycle_time == + rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type())) + { + // it is zero after activation + RCLCPP_DEBUG(get_logger(), "Set next_update_cycle_time to %fs", time.seconds()); + *loaded_controller.next_update_cycle_time = time; + } + bool controller_go = (time == rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type())) || @@ -2182,12 +2191,6 @@ controller_interface::return_type ControllerManager::update( controller_ret = controller_interface::return_type::ERROR; } - if ( - *loaded_controller.next_update_cycle_time == - rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type())) - { - *loaded_controller.next_update_cycle_time = time; - } *loaded_controller.next_update_cycle_time += controller_period; if (controller_ret != controller_interface::return_type::OK)