From e4114200ced4ad4680266ab42188819181572e18 Mon Sep 17 00:00:00 2001
From: Sai Kishor Kothakota <sai.kishor@pal-robotics.com>
Date: Fri, 11 Oct 2024 11:55:10 +0200
Subject: [PATCH] Check the update_rate set to the controllers to be a valid
 one

---
 .../src/controller_interface_base.cpp         | 22 ++++++++++-
 .../test/test_controller_interface.cpp        | 37 ++++++++++++++++---
 2 files changed, 52 insertions(+), 7 deletions(-)

diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp
index 61d2c780b8..e2c1fa480a 100644
--- a/controller_interface/src/controller_interface_base.cpp
+++ b/controller_interface/src/controller_interface_base.cpp
@@ -28,13 +28,14 @@ return_type ControllerInterfaceBase::init(
   const std::string & node_namespace, const rclcpp::NodeOptions & node_options)
 {
   urdf_ = urdf;
+  update_rate_ = cm_update_rate;
   node_ = std::make_shared<rclcpp_lifecycle::LifecycleNode>(
     controller_name, node_namespace, node_options,
     false);  // disable LifecycleNode service interfaces
 
   try
   {
-    auto_declare<int>("update_rate", cm_update_rate);
+    auto_declare<int>("update_rate", update_rate_);
     auto_declare<bool>("is_async", false);
   }
   catch (const std::exception & e)
@@ -85,7 +86,24 @@ const rclcpp_lifecycle::State & ControllerInterfaceBase::configure()
   // Other solution is to add check into the LifecycleNode if a transition is valid to trigger
   if (get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED)
   {
-    update_rate_ = static_cast<unsigned int>(get_node()->get_parameter("update_rate").as_int());
+    const auto update_rate = get_node()->get_parameter("update_rate").as_int();
+    if (update_rate < 0)
+    {
+      RCLCPP_ERROR(get_node()->get_logger(), "Update rate cannot be a negative value!");
+      return get_lifecycle_state();
+    }
+    if (update_rate_ != 0u && update_rate > update_rate_)
+    {
+      RCLCPP_WARN(
+        get_node()->get_logger(),
+        "The update rate of the controller : '%ld Hz' cannot be higher than the update rate of the "
+        "controller manager : '%d Hz'. Setting it to the update rate of the controller manager.",
+        update_rate, update_rate_);
+    }
+    else
+    {
+      update_rate_ = static_cast<unsigned int>(update_rate);
+    }
     is_async_ = get_node()->get_parameter("is_async").as_bool();
   }
 
diff --git a/controller_interface/test/test_controller_interface.cpp b/controller_interface/test/test_controller_interface.cpp
index f5c0a6b3de..4204d32f45 100644
--- a/controller_interface/test/test_controller_interface.cpp
+++ b/controller_interface/test/test_controller_interface.cpp
@@ -19,6 +19,7 @@
 #include <string>
 #include <vector>
 
+#include "lifecycle_msgs/msg/state.hpp"
 #include "rclcpp/executor_options.hpp"
 #include "rclcpp/executors/multi_threaded_executor.hpp"
 
@@ -46,8 +47,8 @@ TEST(TestableControllerInterface, init)
     controller_interface::return_type::OK);
   ASSERT_NO_THROW(controller.get_node());
 
-  // update_rate is set to default 0
-  ASSERT_EQ(controller.get_update_rate(), 0u);
+  // update_rate is set to controller_manager's rate
+  ASSERT_EQ(controller.get_update_rate(), 10u);
 
   // Even after configure is 10
   controller.configure();
@@ -56,6 +57,32 @@ TEST(TestableControllerInterface, init)
   rclcpp::shutdown();
 }
 
+TEST(TestableControllerInterface, setting_negative_update_rate_in_configure)
+{
+  // mocks the declaration of overrides parameters in a yaml file
+  rclcpp::init(0, nullptr);
+
+  TestableControllerInterface controller;
+  // initialize, create node
+  auto node_options = controller.define_custom_node_options();
+  std::vector<std::string> node_options_arguments = node_options.arguments();
+  node_options_arguments.push_back("--ros-args");
+  node_options_arguments.push_back("-p");
+  node_options_arguments.push_back("update_rate:=-100");
+  node_options = node_options.arguments(node_options_arguments);
+  ASSERT_EQ(
+    controller.init(TEST_CONTROLLER_NAME, "", 1000.0, "", node_options),
+    controller_interface::return_type::OK);
+
+  // update_rate is set to controller_manager's rate
+  ASSERT_EQ(controller.get_update_rate(), 1000u);
+
+  // The configure should fail and the update rate should stay the same
+  ASSERT_EQ(controller.configure().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
+  ASSERT_EQ(controller.get_update_rate(), 1000u);
+  rclcpp::shutdown();
+}
+
 TEST(TestableControllerInterface, setting_update_rate_in_configure)
 {
   // mocks the declaration of overrides parameters in a yaml file
@@ -70,7 +97,7 @@ TEST(TestableControllerInterface, setting_update_rate_in_configure)
   node_options_arguments.push_back("update_rate:=2812");
   node_options = node_options.arguments(node_options_arguments);
   ASSERT_EQ(
-    controller.init(TEST_CONTROLLER_NAME, "", 1.0, "", node_options),
+    controller.init(TEST_CONTROLLER_NAME, "", 5000.0, "", node_options),
     controller_interface::return_type::OK);
 
   // initialize executor to be able to get parameter update
@@ -85,8 +112,8 @@ TEST(TestableControllerInterface, setting_update_rate_in_configure)
   using namespace std::chrono_literals;
   std::this_thread::sleep_for(50ms);
 
-  // update_rate is set to default 0 because it is set on configure
-  ASSERT_EQ(controller.get_update_rate(), 0u);
+  // update_rate is set to controller_manager's rate
+  ASSERT_EQ(controller.get_update_rate(), 5000u);
 
   // Even after configure is 0
   controller.configure();