Revert "Issue-759: Adding cleanup service"

This reverts commit 9b8d702.

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",
 ]

controller_manager/controller_manager/controller_manager_services.py

@@ -23,7 +23,6 @@
  SetHardwareComponentState,
  SwitchController,
  UnloadController,
- CleanupController,
 )
 
 import rclpy
@@ -150,11 +149,3 @@ def unload_controller(node, controller_manager_name, controller_name):
  return service_caller(
  node, f"{controller_manager_name}/unload_controller", UnloadController, request
  )
-
-
-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
- )

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

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<ControllerSpec> get_loaded_controllers() const;
 
@@ -300,11 +296,6 @@ 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,
@@ -530,8 +521,6 @@ 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_;

controller_manager/src/controller_manager.cpp

@@ -495,10 +495,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<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",
@@ -659,61 +655,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<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_);
@@ -1925,22 +1866,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<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)

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}

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