Add cleanup_controller lifecycle transition

controller_manager/CMakeLists.txt

@@ -139,6 +139,17 @@ if(BUILD_TESTING)
     ros2_control_test_assets::ros2_control_test_assets
   )
 
+  ament_add_gmock(test_cleanup_controller
+    test/test_cleanup_controller.cpp
+    APPEND_ENV AMENT_PREFIX_PATH=${ament_index_build_path}_$<CONFIG>
+  )
+  target_link_libraries(test_cleanup_controller
+    controller_manager
+    test_controller
+    test_controller_failed_init
+    ros2_control_test_assets::ros2_control_test_assets
+  )
+
   ament_add_gmock(test_controllers_chaining_with_controller_manager
     test/test_controllers_chaining_with_controller_manager.cpp
   )

controller_manager/controller_manager/__init__.py

@@ -23,6 +23,7 @@
     set_hardware_component_state,
     switch_controllers,
     unload_controller,
+    cleanup_controller,
     get_parameter_from_param_files,
     set_controller_parameters,
     set_controller_parameters_from_param_files,
@@ -40,6 +41,7 @@
     "set_hardware_component_state",
     "switch_controllers",
     "unload_controller",
+    "cleanup_controller",
     "get_parameter_from_param_files",
     "set_controller_parameters",
     "set_controller_parameters_from_param_files",

controller_manager/controller_manager/controller_manager_services.py

@@ -23,6 +23,7 @@
     SetHardwareComponentState,
     SwitchController,
     UnloadController,
+    CleanupController,
 )
 
 import rclpy
@@ -310,6 +311,21 @@ def unload_controller(
     )
 
 
+def cleanup_controller(
+    node, controller_manager_name, controller_name, service_timeout=0.0, call_timeout=10.0
+):
+    request = CleanupController.Request()
+    request.name = controller_name
+    return service_caller(
+        node,
+        f"{controller_manager_name}/cleanup_controller",
+        CleanupController,
+        request,
+        service_timeout,
+        call_timeout,
+    )
+
+
 def get_params_files_with_controller_parameters(
     node, controller_name: str, namespace: str, parameter_files: list
 ):

controller_manager/include/controller_manager/controller_manager.hpp

@@ -28,6 +28,7 @@
 
 #include "controller_manager/controller_spec.hpp"
 #include "controller_manager_msgs/msg/controller_manager_activity.hpp"
+#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"
@@ -108,6 +109,8 @@ class ControllerManager : public rclcpp::Node
 
   controller_interface::return_type unload_controller(const std::string & controller_name);
 
+  controller_interface::return_type cleanup_controller(const std::string & controller_name);
+
   std::vector<ControllerSpec> get_loaded_controllers() const;
 
   template <
@@ -313,6 +316,10 @@ 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);
 
+  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);
+
   void list_controller_types_srv_cb(
     const std::shared_ptr<controller_manager_msgs::srv::ListControllerTypes::Request> request,
     std::shared_ptr<controller_manager_msgs::srv::ListControllerTypes::Response> response);
@@ -640,6 +647,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

@@ -844,6 +844,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",
@@ -1092,9 +1096,14 @@ controller_interface::return_type ControllerManager::unload_controller(
     return controller_interface::return_type::ERROR;
   }
 
+  // find and clean controller if it is inactive
+  if (cleanup_controller(controller_name) != controller_interface::return_type::OK)
+  {
+    return controller_interface::return_type::ERROR;
+  }
+
   RCLCPP_DEBUG(get_logger(), "Shutdown controller");
   controller_chain_spec_cleanup(controller_chain_spec_, controller_name);
-  cleanup_controller_exported_interfaces(controller);
   if (is_controller_inactive(*controller.c) || is_controller_unconfigured(*controller.c))
   {
     RCLCPP_DEBUG(
@@ -1144,6 +1153,55 @@ controller_interface::return_type ControllerManager::cleanup_controller(
   return controller_interface::return_type::OK;
 }
 
+controller_interface::return_type ControllerManager::cleanup_controller(
+  const std::string & controller_name)
+{
+
+  const auto & controllers = get_loaded_controllers();
+
+  auto found_it = std::find_if(
+    controllers.begin(), controllers.end(),
+    std::bind(controller_name_compare, std::placeholders::_1, controller_name));
+
+  if (found_it == controllers.end())
+  {
+    RCLCPP_ERROR(
+      get_logger(),
+      "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;
+
+  if (is_controller_unconfigured(*controller))
+  {
+    // all good nothing to do!
+    return controller_interface::return_type::OK;
+  }
+
+  RCLCPP_INFO(get_logger(), "Cleanup controller '%s'", controller_name.c_str());
+  auto state = controller->get_lifecycle_state();
+  if (
+    state.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE ||
+    state.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED)
+  {
+    RCLCPP_ERROR(
+      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;
+  }
+
+  RCLCPP_DEBUG(get_logger(), "Calling cleanup for controller '%s'", controller_name.c_str());
+  auto result = cleanup_controller(*found_it);
+
+  if (result == controller_interface::return_type::OK)
+  {
+    RCLCPP_DEBUG(get_logger(), "Successfully cleaned-up controller '%s'", controller_name.c_str());
+  }
+
+  return result;
+}
+
 void ControllerManager::shutdown_controller(
   const controller_manager::ControllerSpec & controller) const
 {
@@ -2691,6 +2749,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_cleanup_controller.cpp

@@ -0,0 +1,118 @@
+// Copyright 2024 Stogl Robotics Consulting UG (haftungsbeschränkt)
+//
+// 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.
+
+#include <gmock/gmock.h>
+#include <gtest/gtest.h>
+#include <memory>
+#include <string>
+#include <tuple>
+#include <vector>
+
+#include "controller_interface/controller_interface.hpp"
+#include "controller_manager/controller_manager.hpp"
+#include "controller_manager_test_common.hpp"
+#include "lifecycle_msgs/msg/state.hpp"
+#include "test_controller/test_controller.hpp"
+#include "test_controller_failed_init/test_controller_failed_init.hpp"
+
+using test_controller::TEST_CONTROLLER_CLASS_NAME;
+using ::testing::_;
+using ::testing::Return;
+const auto CONTROLLER_NAME_1 = "test_controller1";
+using strvec = std::vector<std::string>;
+
+class TestCleanupController : public ControllerManagerFixture<controller_manager::ControllerManager>
+{
+};
+
+TEST_F(TestCleanupController, cleanup_unknown_controller)
+{
+  ASSERT_EQ(
+    cm_->cleanup_controller("unknown_controller_name"), controller_interface::return_type::ERROR);
+}
+
+TEST_F(TestCleanupController, cleanup_controller_failed_init)
+{
+  auto controller_if = cm_->load_controller(
+    "test_controller_failed_init",
+    test_controller_failed_init::TEST_CONTROLLER_FAILED_INIT_CLASS_NAME);
+
+  ASSERT_EQ(
+    cm_->cleanup_controller("test_controller_failed_init"),
+    controller_interface::return_type::ERROR);
+}
+
+TEST_F(TestCleanupController, cleanup_non_loaded_controller_fails)
+{
+  // try cleanup non-loaded controller
+  EXPECT_EQ(cm_->cleanup_controller(CONTROLLER_NAME_1), controller_interface::return_type::ERROR);
+}
+
+TEST_F(TestCleanupController, cleanup_unconfigured_controller)
+{
+  auto controller_if =
+    cm_->load_controller(CONTROLLER_NAME_1, test_controller::TEST_CONTROLLER_CLASS_NAME);
+  ASSERT_NE(controller_if, nullptr);
+
+  ASSERT_EQ(
+    lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
+    controller_if->get_lifecycle_state().id());
+  ASSERT_EQ(cm_->cleanup_controller(CONTROLLER_NAME_1), controller_interface::return_type::OK);
+  EXPECT_EQ(
+    lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
+    controller_if->get_lifecycle_state().id());
+}
+
+TEST_F(TestCleanupController, cleanup_inactive_controller)
+{
+  auto controller_if =
+    cm_->load_controller(CONTROLLER_NAME_1, test_controller::TEST_CONTROLLER_CLASS_NAME);
+  ASSERT_NE(controller_if, nullptr);
+  ASSERT_EQ(
+    lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
+    controller_if->get_lifecycle_state().id());
+
+  cm_->configure_controller(CONTROLLER_NAME_1);
+
+  EXPECT_EQ(
+    lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_lifecycle_state().id());
+  ASSERT_EQ(cm_->cleanup_controller(CONTROLLER_NAME_1), controller_interface::return_type::OK);
+  EXPECT_EQ(
+    lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
+    controller_if->get_lifecycle_state().id());
+}
+
+TEST_F(TestCleanupController, cleanup_active_controller)
+{
+  auto controller_if =
+    cm_->load_controller(CONTROLLER_NAME_1, test_controller::TEST_CONTROLLER_CLASS_NAME);
+  ASSERT_NE(controller_if, nullptr);
+  ASSERT_EQ(
+    lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
+    controller_if->get_lifecycle_state().id());
+
+  cm_->configure_controller(CONTROLLER_NAME_1);
+
+  EXPECT_EQ(
+    lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_lifecycle_state().id());
+
+  switch_test_controllers(strvec{CONTROLLER_NAME_1}, strvec{}, STRICT);
+
+  EXPECT_EQ(
+    lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if->get_lifecycle_state().id());
+
+  ASSERT_EQ(cm_->cleanup_controller(CONTROLLER_NAME_1), controller_interface::return_type::ERROR);
+  EXPECT_EQ(
+    lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if->get_lifecycle_state().id());
+}