Optimize the valid time check in the update loop (#1923)

ros-controls · Dec 9, 2024 · 5e53146 · 5e53146
1 parent 6e896bf
commit 5e53146
Showing 1 changed file with 21 additions and 23 deletions.
diff --git 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<std::string> 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()))