Issue 759: This will add the cleanup service

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

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
+    )

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

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<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,
@@ -531,6 +540,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_;

controller_manager/src/controller_manager.cpp

@@ -41,6 +41,18 @@ rclcpp::QoS qos_services =
     .reliable()
     .durability_volatile();
 
+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;
@@ -504,6 +516,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",
@@ -619,52 +635,109 @@ 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<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);
+  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",
       controller_name.c_str());
     async_controller_threads_.erase(controller_name);
   }
 
+  // CHAINABLE CONTROLLERS: remove reference interfaces of chainable controllers
+  if (controller->is_chainable())
+  {
+    RCLCPP_DEBUG(
+      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");
-  // TODO(destogl): remove reference interface if chainable; i.e., add a separate method for
-  // cleaning-up controllers?
-  controller.c->get_node()->cleanup();
+  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<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);
+
+  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);
 
@@ -740,7 +813,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;
   }
@@ -1865,6 +1938,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<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/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<rclcpp::Node>("srv_client");
+  srv_executor.add_node(srv_node);
+  rclcpp::Client<controller_manager_msgs::srv::CleanupController>::SharedPtr client =
+    srv_node->create_client<controller_manager_msgs::srv::CleanupController>(
+      "test_controller_manager/cleanup_controller");
+
+  auto request = std::make_shared<controller_manager_msgs::srv::CleanupController::Request>();
+  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<TestController>();
+  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<TestController>(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;

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}

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