From 5561b9556d0af68eed2a8041352a35ca143398fc Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 4 Dec 2024 23:41:46 +0100 Subject: [PATCH] Optimize the valid time check in the update loop --- controller_manager/src/controller_manager.cpp | 44 +++++++++---------- 1 file changed, 21 insertions(+), 23 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 42542dec7c..d4b4076bb3 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -2366,6 +2366,26 @@ controller_interface::return_type ControllerManager::update( ++update_loop_counter_; update_loop_counter_ %= update_rate_; + // Check for valid time + if ( + !get_clock()->started() && + time == rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type())) + { + throw std::runtime_error( + "No clock received, and time argument is zero. Check your controller_manager node's " + "clock configuration (use_sim_time parameter) and if a valid clock source is " + "available. Also pass a proper time argument to the update method."); + } + else + { + // this can happen with use_sim_time=true until the /clock is received + rclcpp::Clock clock = rclcpp::Clock(); + RCLCPP_WARN_THROTTLE( + get_logger(), clock, 1000, + "No clock received, using time argument instead! Check your node's clock " + "configuration (use_sim_time parameter) and if a valid clock source is available"); + } + std::vector failed_controllers_list; for (const auto & loaded_controller : rt_controller_list) { @@ -2390,30 +2410,8 @@ controller_interface::return_type ControllerManager::update( run_controller_at_cm_rate ? period : rclcpp::Duration::from_seconds((1.0 / controller_update_rate)); - rclcpp::Time current_time; bool first_update_cycle = false; - if (get_clock()->started()) - { - current_time = get_clock()->now(); - } - else if ( - time == rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type())) - { - throw std::runtime_error( - "No clock received, and time argument is zero. Check your controller_manager node's " - "clock configuration (use_sim_time parameter) and if a valid clock source is " - "available. Also pass a proper time argument to the update method."); - } - else - { - // this can happen with use_sim_time=true until the /clock is received - rclcpp::Clock clock = rclcpp::Clock(); - RCLCPP_WARN_THROTTLE( - get_logger(), clock, 1000, - "No clock received, using time argument instead! Check your node's clock " - "configuration (use_sim_time parameter) and if a valid clock source is available"); - current_time = time; - } + const rclcpp::Time current_time = get_clock()->started() ? get_clock()->now() : time; if ( *loaded_controller.last_update_cycle_time == rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type()))