Issue-759: Adding cleanup service

Author: bailaC

Diff for: 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 for: 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 for: 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 for: 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<ControllerSpec> get_loaded_controllers() const;
 
@@ -296,6 +300,11 @@ class ControllerManager : public rclcpp::Node
     const std::shared_ptr<controller_manager_msgs::srv::UnloadController::Request> request,
     std::shared_ptr<controller_manager_msgs::srv::UnloadController::Response> response);
 
+  CONTROLLER_MANAGER_PUBLIC
+  void cleanup_controller_service_cb(
+    const std::shared_ptr<controller_manager_msgs::srv::CleanupController::Request> request,
+    std::shared_ptr<controller_manager_msgs::srv::CleanupController::Response> response);
+
   CONTROLLER_MANAGER_PUBLIC
   void list_controller_types_srv_cb(
     const std::shared_ptr<controller_manager_msgs::srv::ListControllerTypes::Request> request,
@@ -521,6 +530,8 @@ class ControllerManager : public rclcpp::Node
     switch_controller_service_;
   rclcpp::Service<controller_manager_msgs::srv::UnloadController>::SharedPtr
     unload_controller_service_;
+  rclcpp::Service<controller_manager_msgs::srv::CleanupController>::SharedPtr
+    cleanup_controller_service_;
 
   rclcpp::Service<controller_manager_msgs::srv::ListHardwareComponents>::SharedPtr
     list_hardware_components_service_;

Diff for: 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<controller_manager_msgs::srv::CleanupController>(
+    "~/cleanup_controller",
+    std::bind(&ControllerManager::cleanup_controller_service_cb, this, _1, _2), qos_services,
+    best_effort_callback_group_);
   list_hardware_components_service_ =
     create_service<controller_manager_msgs::srv::ListHardwareComponents>(
       "~/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<std::recursive_mutex> guard(rt_controllers_wrapper_.controllers_lock_);
+  std::vector<ControllerSpec> & to = rt_controllers_wrapper_.get_unused_list(guard);
+  const std::vector<ControllerSpec> & 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<ControllerSpec> & 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<ControllerSpec> ControllerManager::get_loaded_controllers() const
 {
   std::lock_guard<std::recursive_mutex> 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<controller_manager_msgs::srv::CleanupController::Request> request,
+  std::shared_ptr<controller_manager_msgs::srv::CleanupController::Response> response)
+{
+  // lock services
+  RCLCPP_DEBUG(
+    get_logger(), "cleanup service called for controller '%s' ", request->name.c_str());
+  std::lock_guard<std::mutex> 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<controller_manager_msgs::srv::ListHardwareComponents::Request>,
   std::shared_ptr<controller_manager_msgs::srv::ListHardwareComponents::Response> response)

Diff for: 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 for: 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 for: 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 for: 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",
         ],
     },
 )