From b20412079d641f166489f4f6b86318be49f055c7 Mon Sep 17 00:00:00 2001 From: baila Date: Mon, 18 Dec 2023 19:07:57 +0530 Subject: [PATCH] Revert "Issue-759: Adding cleanup service" This reverts commit 9b8d7025f7378e1135509494893d4149741c8095. --- .../controller_manager/__init__.py | 2 - .../controller_manager_services.py | 9 --- .../controller_manager/spawner.py | 2 - .../controller_manager/controller_manager.hpp | 11 --- controller_manager/src/controller_manager.cpp | 75 ------------------- controller_manager_msgs/CMakeLists.txt | 1 - .../srv/CleanupController.srv | 10 --- .../ros2controlcli/verb/cleanup_controller.py | 40 ---------- ros2controlcli/setup.py | 1 - 9 files changed, 151 deletions(-) delete mode 100644 controller_manager_msgs/srv/CleanupController.srv delete mode 100644 ros2controlcli/ros2controlcli/verb/cleanup_controller.py diff --git a/controller_manager/controller_manager/__init__.py b/controller_manager/controller_manager/__init__.py index 108cfdf40d..f49bed4d34 100644 --- a/controller_manager/controller_manager/__init__.py +++ b/controller_manager/controller_manager/__init__.py @@ -23,7 +23,6 @@ set_hardware_component_state, switch_controllers, unload_controller, - cleanup_controller, ) __all__ = [ @@ -37,5 +36,4 @@ "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 82a5fde2f9..62b350f7d3 100644 --- a/controller_manager/controller_manager/controller_manager_services.py +++ b/controller_manager/controller_manager/controller_manager_services.py @@ -23,7 +23,6 @@ SetHardwareComponentState, SwitchController, UnloadController, - CleanupController, ) import rclpy @@ -176,11 +175,3 @@ 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 16cbd5df55..3ca5600997 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/controller_manager/controller_manager/spawner.py @@ -28,7 +28,6 @@ load_controller, switch_controllers, unload_controller, - cleanup_controller, ) import rclpy @@ -102,7 +101,6 @@ 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 33adff2bbf..44b3c3215a 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -39,7 +39,6 @@ #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" @@ -108,9 +107,6 @@ 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; @@ -300,11 +296,6 @@ 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, @@ -530,8 +521,6 @@ 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 26a9112a9a..f588720b3d 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -504,10 +504,6 @@ 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", @@ -668,61 +664,6 @@ 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_); @@ -1905,22 +1846,6 @@ 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 ba00ce48af..2a863c29dd 100644 --- a/controller_manager_msgs/CMakeLists.txt +++ b/controller_manager_msgs/CMakeLists.txt @@ -23,7 +23,6 @@ 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 deleted file mode 100644 index e6aa12e5ba..0000000000 --- a/controller_manager_msgs/srv/CleanupController.srv +++ /dev/null @@ -1,10 +0,0 @@ -# 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 deleted file mode 100644 index 1aecacdefa..0000000000 --- a/ros2controlcli/ros2controlcli/verb/cleanup_controller.py +++ /dev/null @@ -1,40 +0,0 @@ -# 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 13e1d49fd6..b3c870e6e0 100644 --- a/ros2controlcli/setup.py +++ b/ros2controlcli/setup.py @@ -64,7 +64,6 @@ 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", ], }, )