From 2bdc09cbb3ba715c03f88e394a028177d0a1cbef Mon Sep 17 00:00:00 2001
From: vx792 <szitanics@gmail.com>
Date: Sat, 14 Aug 2021 11:19:34 +0200
Subject: [PATCH] use cm update_rate inside the update() loop iinstead of main
 loop counter

minor corrections after rebase

minor rebase corrections #2

minor rebase corrections #3
---
 .../controller_interface.hpp                  |  4 ++++
 .../src/controller_interface.cpp              |  9 +++++++++
 controller_manager/src/controller_manager.cpp | 20 ++++++-------------
 controller_manager/src/ros2_control_node.cpp  |  1 +
 4 files changed, 20 insertions(+), 14 deletions(-)

diff --git a/controller_interface/include/controller_interface/controller_interface.hpp b/controller_interface/include/controller_interface/controller_interface.hpp
index 7861707784..4385c38ec5 100644
--- a/controller_interface/include/controller_interface/controller_interface.hpp
+++ b/controller_interface/include/controller_interface/controller_interface.hpp
@@ -148,11 +148,15 @@ class ControllerInterface : public rclcpp_lifecycle::node_interfaces::LifecycleN
   CONTROLLER_INTERFACE_PUBLIC
   const rclcpp_lifecycle::State & get_state() const;
 
+  int get_update_rate() const;
+
 protected:
   std::vector<hardware_interface::LoanedCommandInterface> command_interfaces_;
   std::vector<hardware_interface::LoanedStateInterface> state_interfaces_;
   std::shared_ptr<rclcpp::Node> node_;
   rclcpp_lifecycle::State lifecycle_state_;
+  int update_rate_ = 0;
+
 };
 
 using ControllerInterfaceSharedPtr = std::shared_ptr<ControllerInterface>;
diff --git a/controller_interface/src/controller_interface.cpp b/controller_interface/src/controller_interface.cpp
index 37dc676b16..a0671d6862 100644
--- a/controller_interface/src/controller_interface.cpp
+++ b/controller_interface/src/controller_interface.cpp
@@ -64,6 +64,9 @@ const rclcpp_lifecycle::State & ControllerInterface::configure()
       case LifecycleNodeInterface::CallbackReturn::FAILURE:
         break;
     }
+
+    update_rate_ = node_->get_parameter("update_rate").as_int();
+
   }
   return lifecycle_state_;
 }
@@ -170,4 +173,10 @@ std::shared_ptr<rclcpp::Node> ControllerInterface::get_node()
   return node_;
 }
 
+int ControllerInterface::get_update_rate() const
+{
+  return update_rate_;
+}
+
+
 }  // namespace controller_interface
diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp
index e4469ce47e..67e4f9ea0a 100644
--- a/controller_manager/src/controller_manager.cpp
+++ b/controller_manager/src/controller_manager.cpp
@@ -1153,21 +1153,13 @@ controller_interface::return_type ControllerManager::update(
     rt_controllers_wrapper_.update_and_get_used_by_rt_list();
 
   auto ret = controller_interface::return_type::OK;
-<<<<<<< HEAD
-  for (auto loaded_controller : rt_controller_list)
-  {
-    // TODO(v-lopez) we could cache this information
-    // https://github.com/ros-controls/ros2_control/issues/153
-    if (is_controller_active(*loaded_controller.c))
-    {
-      auto controller_ret = loaded_controller.c->update(time, period);
-      if (controller_ret != controller_interface::return_type::OK)
-      {
-        ret = controller_ret;
 =======
+>>>>>>> d75b793... use cm update_rate inside the update() loop iinstead of main loop counter
   int main_update_rate = 100;
+=======
+>>>>>>> febf658... minor rebase corrections #2
   update_loop_counter_ += 1;
-  update_loop_counter_ %= main_update_rate;
+  update_loop_counter_ %= update_rate_;
 
   for (auto loaded_controller : rt_controller_list) {
     // TODO(v-lopez) we could cache this information
@@ -1187,7 +1179,6 @@ controller_interface::return_type ControllerManager::update(
         if (controller_ret != controller_interface::return_type::OK) {
           ret = controller_ret;
         }
->>>>>>> 6bb8dc1... add update_rate member field to controller manager
       }
     }
   }
@@ -1268,7 +1259,8 @@ void ControllerManager::RTControllerListWrapper::wait_until_rt_not_using(
   }
 }
 
-int ControllerManager::get_update_rate() const {
+int ControllerManager::get_update_rate() const 
+{
   return update_rate_;
 }
 
diff --git a/controller_manager/src/ros2_control_node.cpp b/controller_manager/src/ros2_control_node.cpp
index 3dc18b866b..d85845b033 100644
--- a/controller_manager/src/ros2_control_node.cpp
+++ b/controller_manager/src/ros2_control_node.cpp
@@ -39,6 +39,7 @@ int main(int argc, char ** argv)
   // converted back to a timer.
   std::thread cm_thread([cm]() {
       // load controller_manager update time parameter
+      cm->configure();
       int update_rate = cm->get_update_rate();
       RCLCPP_INFO(cm->get_logger(), "update rate is %d Hz", update_rate);