diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index 1d6c76b5d5..01b70bdbd9 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -122,6 +122,17 @@ if(BUILD_TESTING) ros2_control_test_assets::ros2_control_test_assets ) + ament_add_gmock(test_cleanup_controller + test/test_cleanup_controller.cpp + APPEND_ENV AMENT_PREFIX_PATH=${ament_index_build_path}_$ + ) + target_link_libraries(test_cleanup_controller + controller_manager + test_controller + test_controller_failed_init + ros2_control_test_assets::ros2_control_test_assets + ) + ament_add_gmock(test_controllers_chaining_with_controller_manager test/test_controllers_chaining_with_controller_manager.cpp ) diff --git a/controller_manager/controller_manager/__init__.py b/controller_manager/controller_manager/__init__.py index f49bed4d34..108cfdf40d 100644 --- a/controller_manager/controller_manager/__init__.py +++ b/controller_manager/controller_manager/__init__.py @@ -23,6 +23,7 @@ set_hardware_component_state, switch_controllers, unload_controller, + cleanup_controller, ) __all__ = [ @@ -36,4 +37,5 @@ "set_hardware_component_state", "switch_controllers", "unload_controller", + "cleanup_controller", ] diff --git a/controller_manager/controller_manager/controller_manager_services.py b/controller_manager/controller_manager/controller_manager_services.py index 62b350f7d3..82a5fde2f9 100644 --- a/controller_manager/controller_manager/controller_manager_services.py +++ b/controller_manager/controller_manager/controller_manager_services.py @@ -23,6 +23,7 @@ SetHardwareComponentState, SwitchController, UnloadController, + CleanupController, ) import rclpy @@ -175,3 +176,11 @@ def unload_controller(node, controller_manager_name, controller_name, service_ti request, service_timeout, ) + + +def cleanup_controller(node, controller_manager_name, controller_name): + request = CleanupController.Request() + request.name = controller_name + return service_caller( + node, f"{controller_manager_name}/cleanup_controller", CleanupController, request + ) diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py index cf78442856..e39c4be256 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/controller_manager/controller_manager/spawner.py @@ -98,6 +98,7 @@ def wait_for_controller_manager(node, controller_manager, timeout_duration): f"{controller_manager}/reload_controller_libraries", f"{controller_manager}/switch_controller", f"{controller_manager}/unload_controller", + f"{controller_manager}/cleanup_controller", ) # Wait for controller_manager diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index f5ee6854a1..c1fe6d7f64 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -29,6 +29,7 @@ #include "controller_manager/controller_spec.hpp" #include "controller_manager/visibility_control.h" +#include "controller_manager_msgs/srv/cleanup_controller.hpp" #include "controller_manager_msgs/srv/configure_controller.hpp" #include "controller_manager_msgs/srv/list_controller_types.hpp" #include "controller_manager_msgs/srv/list_controllers.hpp" @@ -107,6 +108,9 @@ class ControllerManager : public rclcpp::Node CONTROLLER_MANAGER_PUBLIC controller_interface::return_type unload_controller(const std::string & controller_name); + CONTROLLER_MANAGER_PUBLIC + controller_interface::return_type cleanup_controller(const std::string & controller_name); + CONTROLLER_MANAGER_PUBLIC std::vector get_loaded_controllers() const; @@ -296,6 +300,11 @@ class ControllerManager : public rclcpp::Node const std::shared_ptr request, std::shared_ptr response); + CONTROLLER_MANAGER_PUBLIC + void cleanup_controller_service_cb( + const std::shared_ptr request, + std::shared_ptr response); + CONTROLLER_MANAGER_PUBLIC void list_controller_types_srv_cb( const std::shared_ptr request, @@ -531,6 +540,8 @@ class ControllerManager : public rclcpp::Node switch_controller_service_; rclcpp::Service::SharedPtr unload_controller_service_; + rclcpp::Service::SharedPtr + cleanup_controller_service_; rclcpp::Service::SharedPtr list_hardware_components_service_; diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 6b5077afe2..d4fd927bed 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -55,6 +55,18 @@ static const rmw_qos_profile_t qos_services = { false}; #endif +inline bool is_controller_unconfigured( + const controller_interface::ControllerInterfaceBase & controller) +{ + return controller.get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED; +} + +inline bool is_controller_unconfigured( + const controller_interface::ControllerInterfaceBaseSharedPtr & controller) +{ + return is_controller_unconfigured(*controller); +} + inline bool is_controller_inactive(const controller_interface::ControllerInterfaceBase & controller) { return controller.get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE; @@ -479,6 +491,10 @@ void ControllerManager::init_services() "~/unload_controller", std::bind(&ControllerManager::unload_controller_service_cb, this, _1, _2), qos_services, best_effort_callback_group_); + cleanup_controller_service_ = create_service( + "~/cleanup_controller", + std::bind(&ControllerManager::cleanup_controller_service_cb, this, _1, _2), qos_services, + best_effort_callback_group_); list_hardware_components_service_ = create_service( "~/list_hardware_components", @@ -594,41 +610,46 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c return load_controller(controller_name, controller_type); } -controller_interface::return_type ControllerManager::unload_controller( +controller_interface::return_type ControllerManager::cleanup_controller( const std::string & controller_name) { - std::lock_guard guard(rt_controllers_wrapper_.controllers_lock_); - std::vector & to = rt_controllers_wrapper_.get_unused_list(guard); - const std::vector & from = rt_controllers_wrapper_.get_updated_list(guard); + RCLCPP_INFO(get_logger(), "Cleanup controller '%s'", controller_name.c_str()); - // Transfers the active controllers over, skipping the one to be removed and the active ones. - to = from; + const auto & controllers = get_loaded_controllers(); auto found_it = std::find_if( - to.begin(), to.end(), + controllers.begin(), controllers.end(), std::bind(controller_name_compare, std::placeholders::_1, controller_name)); - if (found_it == to.end()) + + if (found_it == controllers.end()) { - // Fails if we could not remove the controllers - to.clear(); RCLCPP_ERROR( get_logger(), - "Could not unload controller with name '%s' because no controller with this name exists", + "Could not cleanup controller with name '%s' because no controller with this name exists", controller_name.c_str()); return controller_interface::return_type::ERROR; } + auto controller = found_it->c; - auto & controller = *found_it; + if (is_controller_unconfigured(controller)) + { + // all good nothing to do! + return controller_interface::return_type::OK; + } - if (is_controller_active(*controller.c)) + auto state = controller->get_state(); + if ( + state.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE || + state.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) { - to.clear(); RCLCPP_ERROR( - get_logger(), "Could not unload controller with name '%s' because it is still active", - controller_name.c_str()); + get_logger(), "Controller '%s' can not be cleaned-up from '%s' state.", + controller_name.c_str(), state.label().c_str()); return controller_interface::return_type::ERROR; } - if (controller.c->is_async()) + + // ASYNCHRONOUS CONTROLLERS: Stop background thread for update + if (controller->is_async()) { RCLCPP_DEBUG( get_logger(), "Removing controller '%s' from the list of async controllers", @@ -636,23 +657,63 @@ controller_interface::return_type ControllerManager::unload_controller( async_controller_threads_.erase(controller_name); } - RCLCPP_DEBUG(get_logger(), "Cleanup controller"); - // TODO(destogl): remove reference interface if chainable; i.e., add a separate method for - // cleaning-up controllers? - if (is_controller_inactive(*controller.c)) + // CHAINABLE CONTROLLERS: remove reference interfaces of chainable controllers + if (controller->is_chainable()) { RCLCPP_DEBUG( - get_logger(), "Controller '%s' is cleaned-up before unloading!", controller_name.c_str()); - // TODO(destogl): remove reference interface if chainable; i.e., add a separate method for - // cleaning-up controllers? - const auto new_state = controller.c->get_node()->cleanup(); - if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) - { - RCLCPP_WARN( - get_logger(), "Failed to clean-up the controller '%s' before unloading!", - controller_name.c_str()); - } + get_logger(), + "Controller '%s' is chainable. Interfaces are being removed from resource manager.", + controller_name.c_str()); + resource_manager_->remove_controller_reference_interfaces(controller_name); + } + + RCLCPP_DEBUG(get_logger(), "Cleanup controller"); + + auto new_state = controller->get_node()->cleanup(); + if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) + { + RCLCPP_ERROR( + get_logger(), "After cleanup-up, controller '%s' is in state '%s', expected 'unconfigured'.", + controller_name.c_str(), new_state.label().c_str()); + return controller_interface::return_type::ERROR; + } + + RCLCPP_DEBUG(get_logger(), "Successfully cleaned-up controller '%s'", controller_name.c_str()); + + return controller_interface::return_type::OK; +} + +controller_interface::return_type ControllerManager::unload_controller( + const std::string & controller_name) +{ + // first find and clean controller if it is inactive + if (cleanup_controller(controller_name) != controller_interface::return_type::OK) + { + return controller_interface::return_type::ERROR; + } + + std::lock_guard guard(rt_controllers_wrapper_.controllers_lock_); + std::vector & to = rt_controllers_wrapper_.get_unused_list(guard); + const std::vector & from = rt_controllers_wrapper_.get_updated_list(guard); + + to = from; + auto found_it = std::find_if( + to.begin(), to.end(), + std::bind(controller_name_compare, std::placeholders::_1, controller_name)); + if (found_it == to.end()) + { + // Fails if we could not remove the controllers + to.clear(); + RCLCPP_ERROR( + get_logger(), + "Could not unload controller with name '%s' because no controller with this name exists", + controller_name.c_str()); + return controller_interface::return_type::ERROR; } + + auto & controller = *found_it; + + RCLCPP_DEBUG(get_logger(), "Unload controller"); executor_->remove_node(controller.c->get_node()->get_node_base_interface()); to.erase(found_it); @@ -728,7 +789,7 @@ controller_interface::return_type ControllerManager::configure_controller( if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { RCLCPP_ERROR( - get_logger(), "After configuring, controller '%s' is in state '%s' , expected inactive.", + get_logger(), "After configuring, controller '%s' is in state '%s', expected 'inactive'.", controller_name.c_str(), new_state.label().c_str()); return controller_interface::return_type::ERROR; } @@ -1856,6 +1917,21 @@ void ControllerManager::unload_controller_service_cb( get_logger(), "unloading service finished for controller '%s' ", request->name.c_str()); } +void ControllerManager::cleanup_controller_service_cb( + const std::shared_ptr request, + std::shared_ptr response) +{ + // lock services + RCLCPP_DEBUG(get_logger(), "cleanup service called for controller '%s' ", request->name.c_str()); + std::lock_guard guard(services_lock_); + RCLCPP_DEBUG(get_logger(), "cleanup service locked"); + + response->ok = cleanup_controller(request->name) == controller_interface::return_type::OK; + + RCLCPP_DEBUG( + get_logger(), "cleanup service finished for controller '%s' ", request->name.c_str()); +} + void ControllerManager::list_hardware_components_srv_cb( const std::shared_ptr, std::shared_ptr response) diff --git a/controller_manager/test/test_cleanup_controller.cpp b/controller_manager/test/test_cleanup_controller.cpp new file mode 100644 index 0000000000..bed152ac39 --- /dev/null +++ b/controller_manager/test/test_cleanup_controller.cpp @@ -0,0 +1,108 @@ +// Copyright 2024 Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include +#include + +#include "controller_interface/controller_interface.hpp" +#include "controller_manager/controller_manager.hpp" +#include "controller_manager_test_common.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "test_controller/test_controller.hpp" + +using test_controller::TEST_CONTROLLER_CLASS_NAME; +using ::testing::_; +using ::testing::Return; +const auto CONTROLLER_NAME_1 = "test_controller1"; +using strvec = std::vector; + +class TestCleanupController : public ControllerManagerFixture +{ +}; + +TEST_F(TestCleanupController, cleanup_unknown_controller) +{ + ASSERT_EQ( + cm_->cleanup_controller("unknown_controller_name"), controller_interface::return_type::ERROR); +} + +TEST_F(TestCleanupController, cleanup_controller_failed_init) +{ + auto controller_if = cm_->load_controller( + "test_controller_failed_init", + test_controller_failed_init::TEST_CONTROLLER_FAILED_INIT_CLASS_NAME); + + ASSERT_EQ( + cm_->cleanup_controller("test_controller_failed_init"), + controller_interface::return_type::ERROR); +} + +TEST_F(TestCleanupController, cleanup_non_loaded_controller_fails) +{ + // try cleanup non-loaded controller + EXPECT_EQ(cm_->cleanup_controller(CONTROLLER_NAME_1), controller_interface::return_type::ERROR); +} + +TEST_F(TestCleanupController, cleanup_unconfigured_controller) +{ + auto controller_if = + cm_->load_controller(CONTROLLER_NAME_1, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_NE(controller_if, nullptr); + + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if->get_state().id()); + ASSERT_EQ(cm_->cleanup_controller(CONTROLLER_NAME_1), controller_interface::return_type::OK); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if->get_state().id()); +} + +TEST_F(TestCleanupController, cleanup_inactive_controller) +{ + auto controller_if = + cm_->load_controller(CONTROLLER_NAME_1, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_NE(controller_if, nullptr); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if->get_state().id()); + + cm_->configure_controller(CONTROLLER_NAME_1); + + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_state().id()); + ASSERT_EQ(cm_->cleanup_controller(CONTROLLER_NAME_1), controller_interface::return_type::OK); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if->get_state().id()); +} + +TEST_F(TestCleanupController, cleanup_active_controller) +{ + auto controller_if = + cm_->load_controller(CONTROLLER_NAME_1, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_NE(controller_if, nullptr); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if->get_state().id()); + + cm_->configure_controller(CONTROLLER_NAME_1); + + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_state().id()); + + switch_test_controllers(strvec{CONTROLLER_NAME_1}, strvec{}, STRICT); + + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if->get_state().id()); + + ASSERT_EQ(cm_->cleanup_controller(CONTROLLER_NAME_1), controller_interface::return_type::ERROR); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if->get_state().id()); +} diff --git a/controller_manager/test/test_controller_manager_srvs.cpp b/controller_manager/test/test_controller_manager_srvs.cpp index 36d8aa7c4e..d2c2920f7d 100644 --- a/controller_manager/test/test_controller_manager_srvs.cpp +++ b/controller_manager/test/test_controller_manager_srvs.cpp @@ -434,6 +434,68 @@ TEST_F(TestControllerManagerSrvs, load_controller_srv) cm_->get_loaded_controllers()[0].c->get_state().id()); } +TEST_F(TestControllerManagerSrvs, cleanup_controller_srv) +{ + rclcpp::executors::SingleThreadedExecutor srv_executor; + rclcpp::Node::SharedPtr srv_node = std::make_shared("srv_client"); + srv_executor.add_node(srv_node); + rclcpp::Client::SharedPtr client = + srv_node->create_client( + "test_controller_manager/cleanup_controller"); + + auto request = std::make_shared(); + request->name = test_controller::TEST_CONTROLLER_NAME; + + // variation - 1: + // scenario: call the cleanup service when no controllers are loaded + // expected: it should return ERROR as no controllers will be found to cleanup + auto result = call_service_and_wait(*client, request, srv_executor); + ASSERT_FALSE(result->ok) << "Controller not loaded: " << request->name; + + // variation - 2: + // scenario: call the cleanup service when one controller is loaded + // expected: it should return OK as one controller will be found to cleanup + auto test_controller = std::make_shared(); + auto abstract_test_controller = cm_->add_controller( + test_controller, test_controller::TEST_CONTROLLER_NAME, + test_controller::TEST_CONTROLLER_CLASS_NAME); + EXPECT_EQ(1u, cm_->get_loaded_controllers().size()); + + result = call_service_and_wait(*client, request, srv_executor, true); + ASSERT_TRUE(result->ok); + EXPECT_EQ(1u, cm_->get_loaded_controllers().size()); + + // variation - 3: + // scenario: call the cleanup service when controller state is ACTIVE + // expected: it should return ERROR as cleanup is restricted for ACTIVE controllers + test_controller = std::dynamic_pointer_cast(cm_->load_controller( + test_controller::TEST_CONTROLLER_NAME, test_controller::TEST_CONTROLLER_CLASS_NAME)); + cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME); + // activate controllers + cm_->switch_controller( + {test_controller::TEST_CONTROLLER_NAME}, {}, + controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0)); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + cm_->get_loaded_controllers()[0].c->get_state().id()); + result = call_service_and_wait(*client, request, srv_executor, true); + ASSERT_FALSE(result->ok) << "Controller can not be cleaned in active state: " << request->name; + EXPECT_EQ(1u, cm_->get_loaded_controllers().size()); + + // variation - 4: + // scenario: call the cleanup service when controller state is INACTIVE + // expected: it should return OK as cleanup will be done for INACTIVE controllers + cm_->switch_controller( + {}, {test_controller::TEST_CONTROLLER_NAME}, + controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0)); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + cm_->get_loaded_controllers()[0].c->get_state().id()); + result = call_service_and_wait(*client, request, srv_executor, true); + ASSERT_TRUE(result->ok) << "Controller cleaned in inactive state: " << request->name; + EXPECT_EQ(1u, cm_->get_loaded_controllers().size()); +} + TEST_F(TestControllerManagerSrvs, unload_controller_srv) { rclcpp::executors::SingleThreadedExecutor srv_executor; diff --git a/controller_manager_msgs/CMakeLists.txt b/controller_manager_msgs/CMakeLists.txt index 2a863c29dd..ba00ce48af 100644 --- a/controller_manager_msgs/CMakeLists.txt +++ b/controller_manager_msgs/CMakeLists.txt @@ -23,6 +23,7 @@ set(srv_files srv/SetHardwareComponentState.srv srv/SwitchController.srv srv/UnloadController.srv + srv/CleanupController.srv ) rosidl_generate_interfaces(${PROJECT_NAME} diff --git a/controller_manager_msgs/srv/CleanupController.srv b/controller_manager_msgs/srv/CleanupController.srv new file mode 100644 index 0000000000..e6aa12e5ba --- /dev/null +++ b/controller_manager_msgs/srv/CleanupController.srv @@ -0,0 +1,10 @@ +# The CleanupController service allows you to cleanup a single controller +# from controller_manager + +# To cleanup a controller, specify the "name" of the controller. +# The return value "ok" indicates if the controller was successfully +# cleaned up or not + +string name +--- +bool ok diff --git a/ros2controlcli/ros2controlcli/verb/cleanup_controller.py b/ros2controlcli/ros2controlcli/verb/cleanup_controller.py new file mode 100644 index 0000000000..1aecacdefa --- /dev/null +++ b/ros2controlcli/ros2controlcli/verb/cleanup_controller.py @@ -0,0 +1,40 @@ +# Copyright 2020 PAL Robotics S.L. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from controller_manager import cleanup_controller + +from ros2cli.node.direct import add_arguments +from ros2cli.node.strategy import NodeStrategy +from ros2cli.verb import VerbExtension + +from ros2controlcli.api import add_controller_mgr_parsers, LoadedControllerNameCompleter + + +class CleanupControllerVerb(VerbExtension): + """Cleanup a controller in a controller manager.""" + + def add_arguments(self, parser, cli_name): + add_arguments(parser) + arg = parser.add_argument("controller_name", help="Name of the controller") + arg.completer = LoadedControllerNameCompleter() + add_controller_mgr_parsers(parser) + + def main(self, *, args): + with NodeStrategy(args) as node: + response = cleanup_controller(node, args.controller_manager, args.controller_name) + if not response.ok: + return "Error cleanup controllers, check controller_manager logs" + + print(f"Successfully cleaned up controller {args.controller_name}") + return 0 diff --git a/ros2controlcli/setup.py b/ros2controlcli/setup.py index 65469af3c0..a892391667 100644 --- a/ros2controlcli/setup.py +++ b/ros2controlcli/setup.py @@ -64,6 +64,7 @@ ros2controlcli.verb.set_controller_state:SetControllerStateVerb", "switch_controllers = ros2controlcli.verb.switch_controllers:SwitchControllersVerb", "unload_controller = ros2controlcli.verb.unload_controller:UnloadControllerVerb", + "cleanup_controller = ros2controlcli.verb.cleanup_controller:CleanupControllerVerb", ], }, )