diff --git a/controller_manager/controller_manager/__init__.py b/controller_manager/controller_manager/__init__.py index f49bed4d343..108cfdf40d9 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 62b350f7d3e..82a5fde2f9b 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 3ca56009975..16cbd5df55c 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/controller_manager/controller_manager/spawner.py @@ -28,6 +28,7 @@ load_controller, switch_controllers, unload_controller, + cleanup_controller, ) import rclpy @@ -101,6 +102,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 44b3c3215a0..33adff2bbf7 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -39,6 +39,7 @@ #include "controller_manager_msgs/srv/set_hardware_component_state.hpp" #include "controller_manager_msgs/srv/switch_controller.hpp" #include "controller_manager_msgs/srv/unload_controller.hpp" +#include "controller_manager_msgs/srv/cleanup_controller.hpp" #include "diagnostic_updater/diagnostic_updater.hpp" #include "hardware_interface/handle.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, @@ -521,6 +530,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 e7a2293672d..0cca262ad1d 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -504,6 +504,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", @@ -664,6 +668,61 @@ controller_interface::return_type ControllerManager::unload_controller( return controller_interface::return_type::OK; } +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); + + // Transfers the active controllers over, skipping the one to be removed and the active ones. + 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 clear 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; + + if (is_controller_active(*controller.c)) + { + to.clear(); + RCLCPP_ERROR( + get_logger(), "Could not clear controller with name '%s' because it is still active", + controller_name.c_str()); + return controller_interface::return_type::ERROR; + } + + RCLCPP_DEBUG(get_logger(), "Cleanup controller"); + // TODO(destogl): remove reference interface if chainable; i.e., add a separate method for + // cleaning-up controllers? + controller.c->get_node()->cleanup(); + executor_->remove_node(controller.c->get_node()->get_node_base_interface()); + to.erase(found_it); + + // Destroys the old controllers list when the realtime thread is finished with it. + RCLCPP_DEBUG(get_logger(), "Realtime switches over to new controller list"); + rt_controllers_wrapper_.switch_updated_list(guard); + std::vector & new_unused_list = rt_controllers_wrapper_.get_unused_list(guard); + RCLCPP_DEBUG(get_logger(), "Destruct controller"); + new_unused_list.clear(); + RCLCPP_DEBUG(get_logger(), "Destruct controller finished"); + + RCLCPP_DEBUG(get_logger(), "Successfully cleaned controller '%s'", controller_name.c_str()); + + return controller_interface::return_type::OK; +} + std::vector ControllerManager::get_loaded_controllers() const { std::lock_guard guard(rt_controllers_wrapper_.controllers_lock_); @@ -1878,6 +1937,22 @@ 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_msgs/CMakeLists.txt b/controller_manager_msgs/CMakeLists.txt index 2a863c29ddf..ba00ce48afb 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 00000000000..e6aa12e5bac --- /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 00000000000..1aecacdefae --- /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 ff1df137ed8..36aaa4117a0 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", ], }, )