diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index c9d41cc80e..50861ce5ca 100644 --- a/controller_manager/CMakeLists.txt +++ b/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}_$ + ) + 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 ) diff --git a/controller_manager/controller_manager/__init__.py b/controller_manager/controller_manager/__init__.py index 638a28ce86..e2bd212f0c 100644 --- a/controller_manager/controller_manager/__init__.py +++ b/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", diff --git a/controller_manager/controller_manager/controller_manager_services.py b/controller_manager/controller_manager/controller_manager_services.py index dc2647993f..d505ebc4e2 100644 --- a/controller_manager/controller_manager/controller_manager_services.py +++ b/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 ): diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 88a749f078..a4ed749320 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/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" @@ -107,6 +108,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 get_loaded_controllers() const; template < @@ -327,6 +330,10 @@ class ControllerManager : public rclcpp::Node const std::shared_ptr request, std::shared_ptr response); + void cleanup_controller_service_cb( + const std::shared_ptr request, + std::shared_ptr response); + void list_controller_types_srv_cb( const std::shared_ptr request, std::shared_ptr response); @@ -653,6 +660,8 @@ class ControllerManager : public rclcpp::Node switch_controller_service_; rclcpp::Service::SharedPtr unload_controller_service_; + rclcpp::Service::SharedPtr + cleanup_controller_service_; rclcpp::Service::SharedPtr list_hardware_components_service_; diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 18a22caad9..4d39f65660 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -765,6 +765,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( + "~/cleanup_controller", + std::bind(&ControllerManager::cleanup_controller_service_cb, this, _1, _2), qos_services, + best_effort_callback_group_); list_hardware_components_service_ = create_service( "~/list_hardware_components", @@ -975,9 +979,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( @@ -1025,6 +1034,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) +{ + RCLCPP_INFO(get_logger(), "Cleanup controller '%s'", controller_name.c_str()); + + 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; + } + + 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 { @@ -2529,6 +2587,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 request, + std::shared_ptr response) +{ + // lock services + RCLCPP_DEBUG(get_logger(), "cleanup service called for controller '%s' ", request->name.c_str()); + std::lock_guard 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, std::shared_ptr response) diff --git a/controller_manager/test/test_cleanup_controller.cpp b/controller_manager/test/test_cleanup_controller.cpp new file mode 100644 index 0000000000..78dad42c15 --- /dev/null +++ b/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 +#include +#include +#include +#include +#include + +#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; + +class TestCleanupController : public ControllerManagerFixture +{ +}; + +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()); +} diff --git a/controller_manager/test/test_controller_manager_srvs.cpp b/controller_manager/test/test_controller_manager_srvs.cpp index 54c34bd55d..275a170700 100644 --- a/controller_manager/test/test_controller_manager_srvs.cpp +++ b/controller_manager/test/test_controller_manager_srvs.cpp @@ -474,6 +474,77 @@ TEST_F(TestControllerManagerSrvs, load_controller_srv) cm_->get_loaded_controllers()[0].c->get_lifecycle_state().id()); } +TEST_F(TestControllerManagerSrvs, cleanup_controller_srv) +{ + rclcpp::executors::SingleThreadedExecutor srv_executor; + rclcpp::Node::SharedPtr srv_node = std::make_shared("srv_client"); + srv_executor.add_node(srv_node); + rclcpp::Client::SharedPtr client = + srv_node->create_client( + "test_controller_manager/cleanup_controller"); + + auto request = std::make_shared(); + 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(); + 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( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + cm_->get_loaded_controllers()[0].c->get_lifecycle_state().id()); + 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(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_lifecycle_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( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + cm_->get_loaded_controllers()[0].c->get_lifecycle_state().id()); + 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_lifecycle_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( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + cm_->get_loaded_controllers()[0].c->get_lifecycle_state().id()); + EXPECT_EQ(1u, cm_->get_loaded_controllers().size()); +} + TEST_F(TestControllerManagerSrvs, unload_controller_srv) { rclcpp::executors::SingleThreadedExecutor srv_executor; diff --git a/controller_manager_msgs/CMakeLists.txt b/controller_manager_msgs/CMakeLists.txt index 3d5330c8f9..beee338e4e 100644 --- a/controller_manager_msgs/CMakeLists.txt +++ b/controller_manager_msgs/CMakeLists.txt @@ -26,6 +26,7 @@ set(srv_files srv/SetHardwareComponentState.srv srv/SwitchController.srv srv/UnloadController.srv + srv/CleanupController.srv ) rosidl_generate_interfaces(${PROJECT_NAME} diff --git a/controller_manager_msgs/srv/CleanupController.srv b/controller_manager_msgs/srv/CleanupController.srv new file mode 100644 index 0000000000..e6aa12e5ba --- /dev/null +++ b/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 --git a/ros2controlcli/doc/userdoc.rst b/ros2controlcli/doc/userdoc.rst index f6eb6cdc6a..23221f3bd7 100644 --- a/ros2controlcli/doc/userdoc.rst +++ b/ros2controlcli/doc/userdoc.rst @@ -19,6 +19,7 @@ Currently supported commands are - ros2 control set_hardware_component_state - ros2 control switch_controllers - ros2 control unload_controller + - ros2 control cleanup_controller - ros2 control view_controller_chains @@ -344,6 +345,30 @@ unload_controller Consider hidden nodes as well --ros-args ... Pass arbitrary arguments to the executable +cleanup_controller +---------------------- + +.. code-block:: console + + $ ros2 control cleanup_controller -h + usage: ros2 control cleanup_controller [-h] [--spin-time SPIN_TIME] [-s] [-c CONTROLLER_MANAGER] [--include-hidden-nodes] [--ros-args ...] controller_name + + Cleanup a controller in a controller manager + + positional arguments: + controller_name Name of the controller + + options: + -h, --help show this help message and exit + --spin-time SPIN_TIME + Spin time in seconds to wait for discovery (only applies when not using an already running daemon) + -s, --use-sim-time Enable ROS simulation time + -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER + Name of the controller manager ROS node (default: controller_manager) + --include-hidden-nodes + Consider hidden nodes as well + --ros-args ... Pass arbitrary arguments to the executable + view_controller_chains ---------------------- diff --git a/ros2controlcli/ros2controlcli/verb/cleanup_controller.py b/ros2controlcli/ros2controlcli/verb/cleanup_controller.py new file mode 100644 index 0000000000..b9d7683457 --- /dev/null +++ b/ros2controlcli/ros2controlcli/verb/cleanup_controller.py @@ -0,0 +1,45 @@ +# 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, bcolors + +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).direct_node as node: + response = cleanup_controller(node, args.controller_manager, args.controller_name) + if not response.ok: + print( + f"{bcolors.FAIL}Error cleaning up controller {args.controller_name}, check controller_manager logs{bcolors.ENDC}" + ) + return 1 + + print( + f"{bcolors.OKBLUE}Successfully cleaned up controller {args.controller_name}{bcolors.ENDC}" + ) + return 0 diff --git a/ros2controlcli/setup.py b/ros2controlcli/setup.py index 6994f2204d..96ca88d319 100644 --- a/ros2controlcli/setup.py +++ b/ros2controlcli/setup.py @@ -69,6 +69,7 @@ ros2controlcli.verb.set_hardware_component_state:SetHardwareComponentStateVerb", "switch_controllers = ros2controlcli.verb.switch_controllers:SwitchControllersVerb", "unload_controller = ros2controlcli.verb.unload_controller:UnloadControllerVerb", + "cleanup_controller = ros2controlcli.verb.cleanup_controller:CleanupControllerVerb", ], }, )