Pass controller manager update rate on the init of the controller int…

…erface (#1141)
ros-controls · Nov 20, 2023 · 2056d6c · 2056d6c
1 parent cf405bc
commit 2056d6c
Show file tree

Hide file tree

Showing 9 changed files with 29 additions and 26 deletions.
diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp
@@ -114,7 +114,7 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy
 
   CONTROLLER_INTERFACE_PUBLIC
   virtual return_type init(
-    const std::string & controller_name, const std::string & urdf,
+    const std::string & controller_name, const std::string & urdf, unsigned int cm_update_rate,
     const std::string & namespace_ = "",
     const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions().enable_logger_service(true));
 

diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp
@@ -25,16 +25,16 @@
 namespace controller_interface
 {
 return_type ControllerInterfaceBase::init(
-  const std::string & controller_name, const std::string & urdf, const std::string & namespace_,
-  const rclcpp::NodeOptions & node_options)
+  const std::string & controller_name, const std::string & urdf, unsigned int cm_update_rate,
+  const std::string & namespace_, const rclcpp::NodeOptions & node_options)
 {
   node_ = std::make_shared<rclcpp_lifecycle::LifecycleNode>(
     controller_name, namespace_, node_options, false);  // disable LifecycleNode service interfaces
   urdf_ = urdf;
 
   try
   {
-    auto_declare<int>("update_rate", 0);
+    auto_declare<int>("update_rate", cm_update_rate);
     auto_declare<bool>("is_async", false);
   }
   catch (const std::exception & e)

diff --git a/controller_interface/test/test_chainable_controller_interface.cpp b/controller_interface/test/test_chainable_controller_interface.cpp
@@ -21,7 +21,7 @@ TEST_F(ChainableControllerInterfaceTest, default_returns)
   TestableChainableControllerInterface controller;
 
   // initialize, create node
-  ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, ""), controller_interface::return_type::OK);
+  ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, "", 50.0), controller_interface::return_type::OK);
   ASSERT_NO_THROW(controller.get_node());
 
   EXPECT_TRUE(controller.is_chainable());
@@ -33,7 +33,7 @@ TEST_F(ChainableControllerInterfaceTest, export_reference_interfaces)
   TestableChainableControllerInterface controller;
 
   // initialize, create node
-  ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, ""), controller_interface::return_type::OK);
+  ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, "", 50.0), controller_interface::return_type::OK);
   ASSERT_NO_THROW(controller.get_node());
 
   auto reference_interfaces = controller.export_reference_interfaces();
@@ -50,7 +50,7 @@ TEST_F(ChainableControllerInterfaceTest, reference_interfaces_storage_not_correc
   TestableChainableControllerInterface controller;
 
   // initialize, create node
-  ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, ""), controller_interface::return_type::OK);
+  ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, "", 50.0), controller_interface::return_type::OK);
   ASSERT_NO_THROW(controller.get_node());
 
   // expect empty return because storage is not resized
@@ -64,7 +64,7 @@ TEST_F(ChainableControllerInterfaceTest, reference_interfaces_prefix_is_not_node
   TestableChainableControllerInterface controller;
 
   // initialize, create node
-  ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, ""), controller_interface::return_type::OK);
+  ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, "", 50.0), controller_interface::return_type::OK);
   ASSERT_NO_THROW(controller.get_node());
 
   controller.set_name_prefix_of_reference_interfaces("some_not_correct_interface_prefix");
@@ -79,7 +79,7 @@ TEST_F(ChainableControllerInterfaceTest, setting_chained_mode)
   TestableChainableControllerInterface controller;
 
   // initialize, create node
-  ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, ""), controller_interface::return_type::OK);
+  ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, "", 50.0), controller_interface::return_type::OK);
   ASSERT_NO_THROW(controller.get_node());
 
   auto reference_interfaces = controller.export_reference_interfaces();
@@ -126,7 +126,7 @@ TEST_F(ChainableControllerInterfaceTest, test_update_logic)
   TestableChainableControllerInterface controller;
 
   // initialize, create node
-  ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, ""), controller_interface::return_type::OK);
+  ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, "", 50.0), controller_interface::return_type::OK);
   ASSERT_NO_THROW(controller.get_node());
 
   EXPECT_FALSE(controller.is_in_chained_mode());

diff --git a/controller_interface/test/test_controller_interface.cpp b/controller_interface/test/test_controller_interface.cpp
@@ -38,15 +38,15 @@ TEST(TestableControllerInterface, init)
   ASSERT_THROW(controller.get_node(), std::runtime_error);
 
   // initialize, create node
-  ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, ""), controller_interface::return_type::OK);
+  ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, "", 10.0), 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);
 
-  // Even after configure is 0
+  // Even after configure is 10
   controller.configure();
-  ASSERT_EQ(controller.get_update_rate(), 0u);
+  ASSERT_EQ(controller.get_update_rate(), 10u);
 
   rclcpp::shutdown();
 }
@@ -60,7 +60,7 @@ TEST(TestableControllerInterface, setting_update_rate_in_configure)
 
   TestableControllerInterface controller;
   // initialize, create node
-  ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, ""), controller_interface::return_type::OK);
+  ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, "", 1.0), controller_interface::return_type::OK);
 
   // initialize executor to be able to get parameter update
   auto executor =
@@ -122,7 +122,8 @@ TEST(TestableControllerInterfaceInitError, init_with_error)
   TestableControllerInterfaceInitError controller;
 
   // initialize, create node
-  ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, ""), controller_interface::return_type::ERROR);
+  ASSERT_EQ(
+    controller.init(TEST_CONTROLLER_NAME, "", 100.0), controller_interface::return_type::ERROR);
 
   rclcpp::shutdown();
 }
@@ -136,7 +137,8 @@ TEST(TestableControllerInterfaceInitFailure, init_with_failure)
   TestableControllerInterfaceInitFailure controller;
 
   // initialize, create node
-  ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, ""), controller_interface::return_type::ERROR);
+  ASSERT_EQ(
+    controller.init(TEST_CONTROLLER_NAME, "", 50.0), controller_interface::return_type::ERROR);
 
   rclcpp::shutdown();
 }

diff --git a/controller_interface/test/test_controller_with_options.cpp b/controller_interface/test/test_controller_with_options.cpp
@@ -42,7 +42,7 @@ TEST(ControllerWithOption, init_with_overrides)
   rclcpp::init(argc, argv);
   // creates the controller
   FriendControllerWithOptions controller;
-  EXPECT_EQ(controller.init("controller_name", ""), controller_interface::return_type::OK);
+  EXPECT_EQ(controller.init("controller_name", "", 50.0), controller_interface::return_type::OK);
   // checks that the node options have been updated
   const auto & node_options = controller.get_node()->get_node_options();
   EXPECT_TRUE(node_options.allow_undeclared_parameters());
@@ -63,7 +63,7 @@ TEST(ControllerWithOption, init_without_overrides)
   rclcpp::init(argc, argv);
   // creates the controller
   FriendControllerWithOptions controller;
-  EXPECT_EQ(controller.init("controller_name", ""), controller_interface::return_type::ERROR);
+  EXPECT_EQ(controller.init("controller_name", "", 50.0), controller_interface::return_type::ERROR);
   // checks that the node options have been updated
   const auto & node_options = controller.get_node()->get_node_options();
   EXPECT_TRUE(node_options.allow_undeclared_parameters());

diff --git a/controller_interface/test/test_controller_with_options.hpp b/controller_interface/test/test_controller_with_options.hpp
@@ -40,14 +40,14 @@ class ControllerWithOptions : public controller_interface::ControllerInterface
   }
 
   controller_interface::return_type init(
-    const std::string & controller_name, const std::string & urdf,
+    const std::string & controller_name, const std::string & urdf, unsigned int cm_update_rate,
     const std::string & namespace_ = "",
     const rclcpp::NodeOptions & node_options =
       rclcpp::NodeOptions()
         .allow_undeclared_parameters(true)
         .automatically_declare_parameters_from_overrides(true)) override
   {
-    ControllerInterface::init(controller_name, urdf, namespace_, node_options);
+    ControllerInterface::init(controller_name, urdf, cm_update_rate, namespace_, node_options);
 
     switch (on_init())
     {

diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp
@@ -741,7 +741,7 @@ controller_interface::return_type ControllerManager::configure_controller(
       "update rate.",
       controller_name.c_str(), controller_update_rate, cm_update_rate);
   }
-  else if (controller_update_rate != 0 && cm_update_rate % controller_update_rate != 0)
+  else if (cm_update_rate % controller_update_rate != 0)
   {
     RCLCPP_WARN(
       get_logger(),
@@ -1296,7 +1296,8 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::add_co
   }
 
   if (
-    controller.c->init(controller.info.name, robot_description_, get_namespace()) ==
+    controller.c->init(
+      controller.info.name, robot_description_, get_update_rate(), get_namespace()) ==
     controller_interface::return_type::ERROR)
   {
     to.clear();
@@ -2045,8 +2046,7 @@ controller_interface::return_type ControllerManager::update(
     if (!loaded_controller.c->is_async() && is_controller_active(*loaded_controller.c))
     {
       const auto controller_update_rate = loaded_controller.c->get_update_rate();
-      const bool run_controller_at_cm_rate =
-        (controller_update_rate == 0) || (controller_update_rate >= update_rate_);
+      const bool run_controller_at_cm_rate = (controller_update_rate >= update_rate_);
       const auto controller_period =
         run_controller_at_cm_rate ? period
                                   : rclcpp::Duration::from_seconds((1.0 / controller_update_rate));

diff --git a/controller_manager/test/test_controller_failed_init/test_controller_failed_init.cpp b/controller_manager/test/test_controller_failed_init/test_controller_failed_init.cpp
@@ -33,7 +33,8 @@ TestControllerFailedInit::on_init()
 
 controller_interface::return_type TestControllerFailedInit::init(
   const std::string & /* controller_name */, const std::string & /* urdf */,
-  const std::string & /*namespace_*/, const rclcpp::NodeOptions & /*node_options*/)
+  unsigned int cm_update_rate, const std::string & /*namespace_*/,
+  const rclcpp::NodeOptions & /*node_options*/)
 {
   return controller_interface::return_type::ERROR;
 }

diff --git a/controller_manager/test/test_controller_failed_init/test_controller_failed_init.hpp b/controller_manager/test/test_controller_failed_init/test_controller_failed_init.hpp
@@ -40,7 +40,7 @@ class TestControllerFailedInit : public controller_interface::ControllerInterfac
 
   CONTROLLER_INTERFACE_PUBLIC
   controller_interface::return_type init(
-    const std::string & controller_name, const std::string & urdf,
+    const std::string & controller_name, const std::string & urdf, unsigned int cm_update_rate,
     const std::string & namespace_ = "",
     const rclcpp::NodeOptions & node_options =
       rclcpp::NodeOptions()