Fix the initialization of the next update time

ros-controls · May 24, 2024 · ab80113 · ab80113
1 parent 181cda0
commit ab80113
Showing 1 changed file with 9 additions and 6 deletions.
diff --git 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)