Check the update_rate set to the controllers to be a valid one

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();
  }
 

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();

controller_manager/test/test_controller_manager.cpp

@@ -398,7 +398,7 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd
  // if we do 2 times of the controller_manager update rate, the internal counter should be
  // similarly incremented
  EXPECT_EQ(test_controller->internal_counter, pre_internal_counter + (2 * cm_->get_update_rate()));
- EXPECT_EQ(test_controller->get_update_rate(), ctrl_update_rate);
+ EXPECT_EQ(test_controller->get_update_rate(), cm_->get_update_rate());
 
  auto deactivate_future = std::async(
  std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_,

controller_manager/test/test_load_controller.cpp

@@ -72,7 +72,7 @@ TEST_F(TestLoadController, can_set_and_get_non_default_update_rate)
  EXPECT_EQ(
  lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_lifecycle_state().id());
 
- EXPECT_EQ(1337u, controller_if->get_update_rate());
+ EXPECT_EQ(cm_->get_update_rate(), controller_if->get_update_rate());
 }
 
 class TestLoadedController : public TestLoadController