From 620c341a801dfbb2a7c78538cd55f7fd5a5730e9 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 3 Mar 2025 23:07:18 +0100 Subject: [PATCH 1/3] Add check to verify that the command interfaces are unique before proceeding activation --- .../controller_manager/controller_manager.hpp | 10 ++- controller_manager/src/controller_manager.cpp | 81 +++++++++++++++---- 2 files changed, 73 insertions(+), 18 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index ec17bec402..f05c23c84f 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -323,12 +323,20 @@ class ControllerManager : public rclcpp::Node void initialize_parameters(); + /** + * Call cleanup to change the given controller lifecycle node to the unconfigured state. + * + * \param[in] controller controller to be shutdown. + */ + controller_interface::return_type cleanup_controller( + const controller_manager::ControllerSpec & controller); + /** * Call shutdown to change the given controller lifecycle node to the finalized state. * * \param[in] controller controller to be shutdown. */ - void shutdown_controller(controller_manager::ControllerSpec & controller) const; + void shutdown_controller(const controller_manager::ControllerSpec & controller) const; /** * Clear request lists used when switching controllers. The lists are shared between "callback" diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index f89fe11a4b..ff1334aa43 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -253,6 +253,12 @@ void get_controller_list_command_interfaces( } } } +template +[[nodiscard]] auto is_unique(Collection collection) +{ + std::sort(collection.begin(), collection.end()); + return std::adjacent_find(collection.cbegin(), collection.cend()) == collection.cend(); +} } // namespace namespace controller_manager @@ -835,7 +841,33 @@ controller_interface::return_type ControllerManager::unload_controller( return controller_interface::return_type::OK; } -void ControllerManager::shutdown_controller(controller_manager::ControllerSpec & controller) const +controller_interface::return_type ControllerManager::cleanup_controller( + const controller_manager::ControllerSpec & controller) +{ + try + { + cleanup_controller_exported_interfaces(controller); + const auto new_state = controller.c->get_node()->cleanup(); + if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) + { + RCLCPP_ERROR( + get_logger(), "Controller '%s' is not cleaned-up properly, it is still in state '%s'", + controller.info.name.c_str(), new_state.label().c_str()); + return controller_interface::return_type::ERROR; + } + } + catch (...) + { + RCLCPP_ERROR( + get_logger(), "Caught exception while cleaning-up the controller '%s'", + controller.info.name.c_str()); + return controller_interface::return_type::ERROR; + } + return controller_interface::return_type::OK; +} + +void ControllerManager::shutdown_controller( + const controller_manager::ControllerSpec & controller) const { try { @@ -905,23 +937,8 @@ controller_interface::return_type ControllerManager::configure_controller( { RCLCPP_DEBUG( get_logger(), "Controller '%s' is cleaned-up before configuring", controller_name.c_str()); - try + if (cleanup_controller(*found_it) != controller_interface::return_type::OK) { - cleanup_controller_exported_interfaces(*found_it); - new_state = controller->get_node()->cleanup(); - if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) - { - RCLCPP_ERROR( - get_logger(), "Controller '%s' can not be cleaned-up before configuring", - controller_name.c_str()); - return controller_interface::return_type::ERROR; - } - } - catch (...) - { - RCLCPP_ERROR( - get_logger(), "Caught exception while cleaning-up controller '%s' before configuring", - controller_name.c_str()); return controller_interface::return_type::ERROR; } } @@ -1013,6 +1030,36 @@ controller_interface::return_type ControllerManager::configure_controller( // let's update the list of following and preceding controllers const auto cmd_itfs = controller->command_interface_configuration().names; const auto state_itfs = controller->state_interface_configuration().names; + + // Check if the cmd_itfs and the state_itfs are unique + if (!is_unique(cmd_itfs)) + { + std::string cmd_itfs_str = std::accumulate( + std::next(cmd_itfs.begin()), cmd_itfs.end(), cmd_itfs.front(), + [](std::string a, std::string b) { return a + ", " + b; }); + RCLCPP_ERROR( + get_logger(), + "The command interfaces of the controller '%s' are not unique. Please make sure that the " + "command interfaces are unique : '%s'.", + controller_name.c_str(), cmd_itfs_str.c_str()); + cleanup_controller(*found_it); + return controller_interface::return_type::ERROR; + } + + if (!is_unique(state_itfs)) + { + std::string state_itfs_str = std::accumulate( + std::next(state_itfs.begin()), state_itfs.end(), state_itfs.front(), + [](std::string a, std::string b) { return a + ", " + b; }); + RCLCPP_ERROR( + get_logger(), + "The state interfaces of the controller '%s' are not unique. Please make sure that the state " + "interfaces are unique : '%s'.", + controller_name.c_str(), state_itfs_str.c_str()); + cleanup_controller(*found_it); + return controller_interface::return_type::ERROR; + } + for (const auto & cmd_itf : cmd_itfs) { controller_manager::ControllersListIterator ctrl_it; From d81d6d2a2848347999aac40290f2d74cade4fa0a Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 4 Mar 2025 00:10:24 +0100 Subject: [PATCH 2/3] Add a test case of duplicated command interfaces --- .../test/test_controller_manager_srvs.cpp | 50 +++++++++++++++++++ 1 file changed, 50 insertions(+) diff --git a/controller_manager/test/test_controller_manager_srvs.cpp b/controller_manager/test/test_controller_manager_srvs.cpp index f4d1c1bb5a..07e73ee208 100644 --- a/controller_manager/test/test_controller_manager_srvs.cpp +++ b/controller_manager/test/test_controller_manager_srvs.cpp @@ -592,6 +592,9 @@ TEST_F(TestControllerManagerSrvs, configure_controller_srv) auto test_chainable_controller = std::make_shared(); controller_interface::InterfaceConfiguration chained_cmd_cfg = { controller_interface::interface_configuration_type::INDIVIDUAL, {"joint1/position"}}; + controller_interface::InterfaceConfiguration duplicated_chained_cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {"joint1/position", "joint1/position"}}; controller_interface::InterfaceConfiguration chained_state_cfg = { controller_interface::interface_configuration_type::INDIVIDUAL, {"joint1/position", "joint1/velocity"}}; @@ -634,6 +637,53 @@ TEST_F(TestControllerManagerSrvs, configure_controller_srv) lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_chainable_controller->get_lifecycle_state().id()); + // Now try to configure the chainable controller with duplicated command interfaces + test_chainable_controller->set_command_interface_configuration(duplicated_chained_cmd_cfg); + result = call_service_and_wait(*client, request, srv_executor, true); + ASSERT_FALSE(result->ok) << "Controller not configured: " << request->name; + EXPECT_EQ( + test_chainable_controller::TEST_CONTROLLER_NAME, + cm_->get_loaded_controllers()[0].c->get_name()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + cm_->get_loaded_controllers()[0].c->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_chainable_controller->get_lifecycle_state().id()); + + // Now duplicate state interfaces + controller_interface::InterfaceConfiguration duplicated_chained_state_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {"joint1/position", "joint1/position", "joint1/velocity"}}; + test_chainable_controller->set_command_interface_configuration(chained_cmd_cfg); + test_chainable_controller->set_state_interface_configuration(duplicated_chained_state_cfg); + result = call_service_and_wait(*client, request, srv_executor, true); + ASSERT_FALSE(result->ok) << "Controller not configured: " << request->name; + EXPECT_EQ( + test_chainable_controller::TEST_CONTROLLER_NAME, + cm_->get_loaded_controllers()[0].c->get_name()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + cm_->get_loaded_controllers()[0].c->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_chainable_controller->get_lifecycle_state().id()); + + // Now try to configure the chainable controller again with unique state and command interfaces + test_chainable_controller->set_command_interface_configuration(chained_cmd_cfg); + test_chainable_controller->set_state_interface_configuration(chained_state_cfg); + result = call_service_and_wait(*client, request, srv_executor, true); + ASSERT_TRUE(result->ok); + 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; From 61e7549acc9e7dadd2f618f7a65005d2d41b6d9a Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 5 Mar 2025 23:18:57 +0100 Subject: [PATCH 3/3] Update controller_manager/src/controller_manager.cpp --- controller_manager/src/controller_manager.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index ff1334aa43..3c57cebfa8 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -254,7 +254,7 @@ void get_controller_list_command_interfaces( } } template -[[nodiscard]] auto is_unique(Collection collection) +[[nodiscard]] bool is_unique(Collection collection) { std::sort(collection.begin(), collection.end()); return std::adjacent_find(collection.cbegin(), collection.cend()) == collection.cend();