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