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