From d4f74aff78a40a11900a4756acc5acd46a88db9c Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 26 Feb 2025 12:10:06 +0100 Subject: [PATCH] Cleanup chainable controller exported interfaces to allow reconfiguring (#2073) --- .../src/chainable_controller_interface.cpp | 6 ++ .../test_chainable_controller_interface.cpp | 14 ++++ .../controller_manager/controller_manager.hpp | 7 ++ controller_manager/src/controller_manager.cpp | 20 ++++-- .../test/test_controller_manager_srvs.cpp | 68 +++++++++++++++++++ 5 files changed, 109 insertions(+), 6 deletions(-) diff --git a/controller_interface/src/chainable_controller_interface.cpp b/controller_interface/src/chainable_controller_interface.cpp index c03b056845..c81a5dc422 100644 --- a/controller_interface/src/chainable_controller_interface.cpp +++ b/controller_interface/src/chainable_controller_interface.cpp @@ -53,6 +53,9 @@ ChainableControllerInterface::export_state_interfaces() state_interfaces_ptrs_vec.reserve(state_interfaces.size()); ordered_exported_state_interfaces_.reserve(state_interfaces.size()); exported_state_interface_names_.reserve(state_interfaces.size()); + exported_state_interfaces_.clear(); + exported_state_interface_names_.clear(); + ordered_exported_state_interfaces_.clear(); // check if the names of the controller state interfaces begin with the controller's name for (const auto & interface : state_interfaces) @@ -117,6 +120,9 @@ ChainableControllerInterface::export_reference_interfaces() reference_interfaces_ptrs_vec.reserve(reference_interfaces.size()); exported_reference_interface_names_.reserve(reference_interfaces.size()); ordered_exported_reference_interfaces_.reserve(reference_interfaces.size()); + exported_reference_interfaces_.clear(); + exported_reference_interface_names_.clear(); + ordered_exported_reference_interfaces_.clear(); // BEGIN (Handle export change): for backward compatibility // check if the "reference_interfaces_" variable is resized to number of interfaces diff --git a/controller_interface/test/test_chainable_controller_interface.cpp b/controller_interface/test/test_chainable_controller_interface.cpp index 9919ec3d60..c69aa7b3a1 100644 --- a/controller_interface/test/test_chainable_controller_interface.cpp +++ b/controller_interface/test/test_chainable_controller_interface.cpp @@ -54,6 +54,13 @@ TEST_F(ChainableControllerInterfaceTest, export_state_interfaces) EXPECT_EQ( exported_state_interfaces[0]->get_value().value(), EXPORTED_STATE_INTERFACE_VALUE); + + // calling export_state_interfaces again should return the same interface and shouldn't throw + EXPECT_NO_THROW(exported_state_interfaces = controller.export_state_interfaces()); + + ASSERT_THAT(exported_state_interfaces, SizeIs(1)); + EXPECT_EQ(exported_state_interfaces[0]->get_prefix_name(), TEST_CONTROLLER_NAME); + EXPECT_EQ(exported_state_interfaces[0]->get_interface_name(), "test_state"); } TEST_F(ChainableControllerInterfaceTest, export_reference_interfaces) @@ -74,6 +81,13 @@ TEST_F(ChainableControllerInterfaceTest, export_reference_interfaces) EXPECT_EQ(reference_interfaces[0]->get_interface_name(), "test_itf"); EXPECT_EQ(reference_interfaces[0]->get_value().value(), INTERFACE_VALUE); + + // calling export_reference_interfaces again should return the same interface and shouldn't throw + EXPECT_NO_THROW(reference_interfaces = controller.export_reference_interfaces()); + + ASSERT_THAT(reference_interfaces, SizeIs(1)); + EXPECT_EQ(reference_interfaces[0]->get_prefix_name(), TEST_CONTROLLER_NAME); + EXPECT_EQ(reference_interfaces[0]->get_interface_name(), "test_itf"); } TEST_F(ChainableControllerInterfaceTest, interfaces_prefix_is_not_node_name) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 9df9fb56ca..ec17bec402 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -445,6 +445,13 @@ class ControllerManager : public rclcpp::Node */ rclcpp::NodeOptions determine_controller_node_options(const ControllerSpec & controller) const; + /** + * @brief cleanup_controller_exported_interfaces - A method that cleans up the exported interfaces + * of a chainable controller + * @param controller - controller info + */ + void cleanup_controller_exported_interfaces(const ControllerSpec & controller); + std::shared_ptr cm_param_listener_; std::shared_ptr params_; diagnostic_updater::Updater diagnostics_updater_; diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 64a2a1594b..549539cb54 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -812,14 +812,11 @@ controller_interface::return_type ControllerManager::unload_controller( RCLCPP_DEBUG(get_logger(), "Shutdown controller"); controller_chain_spec_cleanup(controller_chain_spec_, controller_name); - // TODO(destogl): remove reference interface if chainable; i.e., add a separate method for - // cleaning-up controllers? + cleanup_controller_exported_interfaces(controller); if (is_controller_inactive(*controller.c) || is_controller_unconfigured(*controller.c)) { RCLCPP_DEBUG( get_logger(), "Controller '%s' is shutdown before unloading!", controller_name.c_str()); - // TODO(destogl): remove reference interface if chainable; i.e., add a separate method for - // cleaning-up controllers? shutdown_controller(controller); } executor_->remove_node(controller.c->get_node()->get_node_base_interface()); @@ -908,10 +905,9 @@ controller_interface::return_type ControllerManager::configure_controller( { RCLCPP_DEBUG( get_logger(), "Controller '%s' is cleaned-up before configuring", controller_name.c_str()); - // TODO(destogl): remove reference interface if chainable; i.e., add a separate method for - // cleaning-up controllers? try { + cleanup_controller_exported_interfaces(*found_it); new_state = controller->get_node()->cleanup(); if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) { @@ -3642,4 +3638,16 @@ rclcpp::NodeOptions ControllerManager::determine_controller_node_options( return controller_node_options; } +void ControllerManager::cleanup_controller_exported_interfaces(const ControllerSpec & controller) +{ + if (is_controller_inactive(controller.c) && controller.c->is_chainable()) + { + RCLCPP_DEBUG( + get_logger(), "Removing controller '%s' exported interfaces from resource manager.", + controller.info.name.c_str()); + resource_manager_->remove_controller_exported_state_interfaces(controller.info.name); + resource_manager_->remove_controller_reference_interfaces(controller.info.name); + } +} + } // namespace controller_manager diff --git a/controller_manager/test/test_controller_manager_srvs.cpp b/controller_manager/test/test_controller_manager_srvs.cpp index 3a1ea5877b..100be75598 100644 --- a/controller_manager/test/test_controller_manager_srvs.cpp +++ b/controller_manager/test/test_controller_manager_srvs.cpp @@ -573,6 +573,67 @@ TEST_F(TestControllerManagerSrvs, configure_controller_srv) lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_lifecycle_state().id()); + // call configure again and check the state + it shouldn't throw any exception + result = call_service_and_wait(*client, request, srv_executor, true); + ASSERT_TRUE(result->ok); + EXPECT_EQ(1u, cm_->get_loaded_controllers().size()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + cm_->get_loaded_controllers()[0].c->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller->get_lifecycle_state().id()); + + // Now repeat the same with the chainable controller + request->name = test_chainable_controller::TEST_CONTROLLER_NAME; + result = call_service_and_wait(*client, request, srv_executor); + ASSERT_FALSE(result->ok) << "Controller not loaded: " << request->name; + + auto test_chainable_controller = std::make_shared(); + controller_interface::InterfaceConfiguration chained_cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, {"joint1/position"}}; + controller_interface::InterfaceConfiguration chained_state_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {"joint1/position", "joint1/velocity"}}; + test_chainable_controller->set_command_interface_configuration(chained_cmd_cfg); + test_chainable_controller->set_state_interface_configuration(chained_state_cfg); + test_chainable_controller->set_reference_interface_names({"joint1/position", "joint1/velocity"}); + test_chainable_controller->set_exported_state_interface_names( + {"joint1/position", "joint1/velocity"}); + + auto abstract_test_chainable_controller = cm_->add_controller( + test_chainable_controller, test_chainable_controller::TEST_CONTROLLER_NAME, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_NE(nullptr, abstract_test_chainable_controller); + EXPECT_EQ(2u, cm_->get_loaded_controllers().size()); + + result = call_service_and_wait(*client, request, srv_executor, true); + ASSERT_TRUE(result->ok); + EXPECT_EQ(2u, cm_->get_loaded_controllers().size()); + EXPECT_EQ( + test_chainable_controller::TEST_CONTROLLER_NAME, + cm_->get_loaded_controllers()[0].c->get_name()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + cm_->get_loaded_controllers()[0].c->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_chainable_controller->get_lifecycle_state().id()); + + // call configure again and check the state + it shouldn't throw any exception + result = call_service_and_wait(*client, request, srv_executor, true); + ASSERT_TRUE(result->ok); + EXPECT_EQ(2u, cm_->get_loaded_controllers().size()); + EXPECT_EQ( + test_chainable_controller::TEST_CONTROLLER_NAME, + cm_->get_loaded_controllers()[0].c->get_name()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + cm_->get_loaded_controllers()[0].c->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_chainable_controller->get_lifecycle_state().id()); + // now unload the controller and check the state auto unload_request = std::make_shared(); unload_request->name = test_controller::TEST_CONTROLLER_NAME; @@ -580,6 +641,13 @@ TEST_F(TestControllerManagerSrvs, configure_controller_srv) EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, test_controller->get_lifecycle_state().id()); + EXPECT_EQ(1u, cm_->get_loaded_controllers().size()); + + unload_request->name = test_chainable_controller::TEST_CONTROLLER_NAME; + ASSERT_TRUE(call_service_and_wait(*unload_client, unload_request, srv_executor, true)->ok); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, + test_chainable_controller->get_lifecycle_state().id()); EXPECT_EQ(0u, cm_->get_loaded_controllers().size()); }