From 2056d6c2152db3d03cb8af8daf739ca194302f11 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 20 Nov 2023 19:05:02 +0100 Subject: [PATCH] Pass controller manager update rate on the init of the controller interface (#1141) --- .../controller_interface_base.hpp | 2 +- .../src/controller_interface_base.cpp | 6 +++--- .../test/test_chainable_controller_interface.cpp | 12 ++++++------ .../test/test_controller_interface.cpp | 14 ++++++++------ .../test/test_controller_with_options.cpp | 4 ++-- .../test/test_controller_with_options.hpp | 4 ++-- controller_manager/src/controller_manager.cpp | 8 ++++---- .../test_controller_failed_init.cpp | 3 ++- .../test_controller_failed_init.hpp | 2 +- 9 files changed, 29 insertions(+), 26 deletions(-) diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index 96141a034b..bbc45c8583 100644 --- 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 index fcbc28590a..4594e7ac07 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -25,8 +25,8 @@ 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( controller_name, namespace_, node_options, false); // disable LifecycleNode service interfaces @@ -34,7 +34,7 @@ return_type ControllerInterfaceBase::init( try { - auto_declare("update_rate", 0); + auto_declare("update_rate", cm_update_rate); auto_declare("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 index 9c46287ba7..d81d2bfdad 100644 --- 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 index 9be0dba9f0..24e780fab3 100644 --- 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 index 55c9db65c9..51169e945c 100644 --- 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 index 892ec56f9d..1221536784 100644 --- 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 index e61e3c769b..640dac8f5d 100644 --- 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 index ba762573c7..262ed55beb 100644 --- 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 index 69530e40b0..28cb48ab6c 100644 --- 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()