From 54caafee681e5f5a42b3debe4307b78a219bed3a Mon Sep 17 00:00:00 2001
From: Sai Kishor Kothakota <sai.kishor@pal-robotics.com>
Date: Tue, 1 Oct 2024 18:26:58 +0200
Subject: [PATCH] change the logic from next_update_cycle_time to
 last_update_cycle_time to unify with RM Fixes
 https://github.com/ros-controls/ros2_control/issues/1769

---
 .../controller_manager/controller_manager.hpp |  2 +-
 .../controller_manager/controller_spec.hpp    |  2 +-
 controller_manager/src/controller_manager.cpp | 30 +++++++++++--------
 3 files changed, 20 insertions(+), 14 deletions(-)

diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp
index 6c0b4fde9b..068eefc1f9 100644
--- a/controller_manager/include/controller_manager/controller_manager.hpp
+++ b/controller_manager/include/controller_manager/controller_manager.hpp
@@ -123,7 +123,7 @@ class ControllerManager : public rclcpp::Node
     controller_spec.c = controller;
     controller_spec.info.name = controller_name;
     controller_spec.info.type = controller_type;
-    controller_spec.next_update_cycle_time = std::make_shared<rclcpp::Time>(0);
+    controller_spec.last_update_cycle_time = std::make_shared<rclcpp::Time>(0);
     return add_controller_impl(controller_spec);
   }
 
diff --git a/controller_manager/include/controller_manager/controller_spec.hpp b/controller_manager/include/controller_manager/controller_spec.hpp
index 9ce1aab8b6..0f44867814 100644
--- a/controller_manager/include/controller_manager/controller_spec.hpp
+++ b/controller_manager/include/controller_manager/controller_spec.hpp
@@ -37,7 +37,7 @@ struct ControllerSpec
 {
   hardware_interface::ControllerInfo info;
   controller_interface::ControllerInterfaceBaseSharedPtr c;
-  std::shared_ptr<rclcpp::Time> next_update_cycle_time;
+  std::shared_ptr<rclcpp::Time> last_update_cycle_time;
 };
 
 struct ControllerChainSpec
diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp
index a8668f7f1b..c7eea3342f 100644
--- a/controller_manager/src/controller_manager.cpp
+++ b/controller_manager/src/controller_manager.cpp
@@ -543,7 +543,7 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c
   controller_spec.c = controller;
   controller_spec.info.name = controller_name;
   controller_spec.info.type = controller_type;
-  controller_spec.next_update_cycle_time = std::make_shared<rclcpp::Time>(
+  controller_spec.last_update_cycle_time = std::make_shared<rclcpp::Time>(
     0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type());
 
   // We have to fetch the parameters_file at the time of loading the controller, because this way we
@@ -1668,8 +1668,8 @@ void ControllerManager::activate_controllers(
       continue;
     }
     auto controller = found_it->c;
-    // reset the next update cycle time for newly activated controllers
-    *found_it->next_update_cycle_time =
+    // reset the last update cycle time for newly activated controllers
+    *found_it->last_update_cycle_time =
       rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type());
 
     bool assignment_successful = true;
@@ -2354,31 +2354,34 @@ controller_interface::return_type ControllerManager::update(
         run_controller_at_cm_rate ? period
                                   : rclcpp::Duration::from_seconds((1.0 / controller_update_rate));
 
+      const rclcpp::Time current_time = get_clock()->now();
       if (
-        *loaded_controller.next_update_cycle_time ==
+        *loaded_controller.last_update_cycle_time ==
         rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type()))
       {
         // it is zero after activation
+        *loaded_controller.last_update_cycle_time = current_time - controller_period;
         RCLCPP_DEBUG(
-          get_logger(), "Setting next_update_cycle_time to %fs for the controller : %s",
-          time.seconds(), loaded_controller.info.name.c_str());
-        *loaded_controller.next_update_cycle_time = time;
+          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);
 
-      bool controller_go =
+      const bool controller_go =
+        run_controller_at_cm_rate ||
         (time ==
          rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type())) ||
-        (time.seconds() >= loaded_controller.next_update_cycle_time->seconds());
+        (controller_actual_period.seconds() * controller_update_rate >= 0.99);
 
       RCLCPP_DEBUG(
         get_logger(), "update_loop_counter: '%d ' controller_go: '%s ' controller_name: '%s '",
         update_loop_counter_, controller_go ? "True" : "False",
         loaded_controller.info.name.c_str());
 
+      RCLCPP_DEBUG(get_logger(), "The update time is %f", time.seconds());
       if (controller_go)
       {
-        const auto controller_actual_period =
-          (time - *loaded_controller.next_update_cycle_time) + controller_period;
         auto controller_ret = controller_interface::return_type::OK;
         bool trigger_status = true;
         // Catch exceptions thrown by the controller update function
@@ -2402,7 +2405,10 @@ controller_interface::return_type ControllerManager::update(
           controller_ret = controller_interface::return_type::ERROR;
         }
 
-        *loaded_controller.next_update_cycle_time += controller_period;
+        *loaded_controller.last_update_cycle_time = current_time;
+        RCLCPP_DEBUG(
+          get_logger(), "[%s] Setting last_update_cycle_time to %fs for the controller",
+          loaded_controller.info.name.c_str(), loaded_controller.last_update_cycle_time->seconds());
 
         if (controller_ret != controller_interface::return_type::OK)
         {