From ae8b3596e4b935f5f06b12a208aad3eb6d96216e Mon Sep 17 00:00:00 2001 From: baila Date: Mon, 18 Dec 2023 19:01:46 +0530 Subject: [PATCH 01/10] Issue-759: Adding cleanup service --- .../controller_manager/__init__.py | 2 + .../controller_manager_services.py | 9 +++ .../controller_manager/spawner.py | 2 + .../controller_manager/controller_manager.hpp | 11 +++ controller_manager/src/controller_manager.cpp | 75 +++++++++++++++++++ controller_manager_msgs/CMakeLists.txt | 1 + .../srv/CleanupController.srv | 10 +++ .../ros2controlcli/verb/cleanup_controller.py | 40 ++++++++++ ros2controlcli/setup.py | 1 + 9 files changed, 151 insertions(+) create mode 100644 controller_manager_msgs/srv/CleanupController.srv create mode 100644 ros2controlcli/ros2controlcli/verb/cleanup_controller.py diff --git a/controller_manager/controller_manager/__init__.py b/controller_manager/controller_manager/__init__.py index f49bed4d34..108cfdf40d 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, ) __all__ = [ @@ -36,4 +37,5 @@ "set_hardware_component_state", "switch_controllers", "unload_controller", + "cleanup_controller", ] diff --git a/controller_manager/controller_manager/controller_manager_services.py b/controller_manager/controller_manager/controller_manager_services.py index 62b350f7d3..82a5fde2f9 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 @@ -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 + ) diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py index 3ca5600997..16cbd5df55 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/controller_manager/controller_manager/spawner.py @@ -28,6 +28,7 @@ load_controller, switch_controllers, unload_controller, + cleanup_controller, ) import rclpy @@ -101,6 +102,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 diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 44b3c3215a..33adff2bbf 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -39,6 +39,7 @@ #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" @@ -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 get_loaded_controllers() const; @@ -296,6 +300,11 @@ class ControllerManager : public rclcpp::Node const std::shared_ptr request, std::shared_ptr response); + CONTROLLER_MANAGER_PUBLIC + void cleanup_controller_service_cb( + const std::shared_ptr request, + std::shared_ptr response); + CONTROLLER_MANAGER_PUBLIC void list_controller_types_srv_cb( const std::shared_ptr request, @@ -521,6 +530,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 f588720b3d..26a9112a9a 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -504,6 +504,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", @@ -664,6 +668,61 @@ 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 guard(rt_controllers_wrapper_.controllers_lock_); + std::vector & to = rt_controllers_wrapper_.get_unused_list(guard); + const std::vector & 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 & 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 ControllerManager::get_loaded_controllers() const { std::lock_guard guard(rt_controllers_wrapper_.controllers_lock_); @@ -1846,6 +1905,22 @@ 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_msgs/CMakeLists.txt b/controller_manager_msgs/CMakeLists.txt index 2a863c29dd..ba00ce48af 100644 --- a/controller_manager_msgs/CMakeLists.txt +++ b/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} 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/ros2controlcli/verb/cleanup_controller.py b/ros2controlcli/ros2controlcli/verb/cleanup_controller.py new file mode 100644 index 0000000000..1aecacdefa --- /dev/null +++ b/ros2controlcli/ros2controlcli/verb/cleanup_controller.py @@ -0,0 +1,40 @@ +# 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 + +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) as node: + response = cleanup_controller(node, args.controller_manager, args.controller_name) + if not response.ok: + return "Error cleanup controllers, check controller_manager logs" + + print(f"Successfully cleaned up controller {args.controller_name}") + return 0 diff --git a/ros2controlcli/setup.py b/ros2controlcli/setup.py index b3c870e6e0..13e1d49fd6 100644 --- a/ros2controlcli/setup.py +++ b/ros2controlcli/setup.py @@ -64,6 +64,7 @@ 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", ], }, ) From b20412079d641f166489f4f6b86318be49f055c7 Mon Sep 17 00:00:00 2001 From: baila Date: Mon, 18 Dec 2023 19:07:57 +0530 Subject: [PATCH 02/10] Revert "Issue-759: Adding cleanup service" This reverts commit 9b8d7025f7378e1135509494893d4149741c8095. --- .../controller_manager/__init__.py | 2 - .../controller_manager_services.py | 9 --- .../controller_manager/spawner.py | 2 - .../controller_manager/controller_manager.hpp | 11 --- controller_manager/src/controller_manager.cpp | 75 ------------------- controller_manager_msgs/CMakeLists.txt | 1 - .../srv/CleanupController.srv | 10 --- .../ros2controlcli/verb/cleanup_controller.py | 40 ---------- ros2controlcli/setup.py | 1 - 9 files changed, 151 deletions(-) delete mode 100644 controller_manager_msgs/srv/CleanupController.srv delete mode 100644 ros2controlcli/ros2controlcli/verb/cleanup_controller.py diff --git a/controller_manager/controller_manager/__init__.py b/controller_manager/controller_manager/__init__.py index 108cfdf40d..f49bed4d34 100644 --- a/controller_manager/controller_manager/__init__.py +++ b/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", ] diff --git a/controller_manager/controller_manager/controller_manager_services.py b/controller_manager/controller_manager/controller_manager_services.py index 82a5fde2f9..62b350f7d3 100644 --- a/controller_manager/controller_manager/controller_manager_services.py +++ b/controller_manager/controller_manager/controller_manager_services.py @@ -23,7 +23,6 @@ SetHardwareComponentState, SwitchController, UnloadController, - CleanupController, ) import rclpy @@ -176,11 +175,3 @@ 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 - ) diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py index 16cbd5df55..3ca5600997 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/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 diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 33adff2bbf..44b3c3215a 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/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 get_loaded_controllers() const; @@ -300,11 +296,6 @@ class ControllerManager : public rclcpp::Node const std::shared_ptr request, std::shared_ptr response); - CONTROLLER_MANAGER_PUBLIC - void cleanup_controller_service_cb( - const std::shared_ptr request, - std::shared_ptr response); - CONTROLLER_MANAGER_PUBLIC void list_controller_types_srv_cb( const std::shared_ptr request, @@ -530,8 +521,6 @@ 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 26a9112a9a..f588720b3d 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -504,10 +504,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( - "~/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", @@ -668,61 +664,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 guard(rt_controllers_wrapper_.controllers_lock_); - std::vector & to = rt_controllers_wrapper_.get_unused_list(guard); - const std::vector & 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 & 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 ControllerManager::get_loaded_controllers() const { std::lock_guard guard(rt_controllers_wrapper_.controllers_lock_); @@ -1905,22 +1846,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 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_msgs/CMakeLists.txt b/controller_manager_msgs/CMakeLists.txt index ba00ce48af..2a863c29dd 100644 --- a/controller_manager_msgs/CMakeLists.txt +++ b/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} diff --git a/controller_manager_msgs/srv/CleanupController.srv b/controller_manager_msgs/srv/CleanupController.srv deleted file mode 100644 index e6aa12e5ba..0000000000 --- a/controller_manager_msgs/srv/CleanupController.srv +++ /dev/null @@ -1,10 +0,0 @@ -# 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/ros2controlcli/verb/cleanup_controller.py b/ros2controlcli/ros2controlcli/verb/cleanup_controller.py deleted file mode 100644 index 1aecacdefa..0000000000 --- a/ros2controlcli/ros2controlcli/verb/cleanup_controller.py +++ /dev/null @@ -1,40 +0,0 @@ -# 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 - -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) as node: - response = cleanup_controller(node, args.controller_manager, args.controller_name) - if not response.ok: - return "Error cleanup controllers, check controller_manager logs" - - print(f"Successfully cleaned up controller {args.controller_name}") - return 0 diff --git a/ros2controlcli/setup.py b/ros2controlcli/setup.py index 13e1d49fd6..b3c870e6e0 100644 --- a/ros2controlcli/setup.py +++ b/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", ], }, ) From 85a8fe94fed7196a30fb9eb2e6867c9725b4c4eb Mon Sep 17 00:00:00 2001 From: baila Date: Mon, 18 Dec 2023 19:01:46 +0530 Subject: [PATCH 03/10] Issue-759: Adding cleanup service --- .../controller_manager/__init__.py | 2 + .../controller_manager_services.py | 9 +++ .../controller_manager/spawner.py | 2 + .../controller_manager/controller_manager.hpp | 11 +++ controller_manager/src/controller_manager.cpp | 75 +++++++++++++++++++ controller_manager_msgs/CMakeLists.txt | 1 + .../srv/CleanupController.srv | 10 +++ .../ros2controlcli/verb/cleanup_controller.py | 40 ++++++++++ ros2controlcli/setup.py | 1 + 9 files changed, 151 insertions(+) create mode 100644 controller_manager_msgs/srv/CleanupController.srv create mode 100644 ros2controlcli/ros2controlcli/verb/cleanup_controller.py diff --git a/controller_manager/controller_manager/__init__.py b/controller_manager/controller_manager/__init__.py index f49bed4d34..108cfdf40d 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, ) __all__ = [ @@ -36,4 +37,5 @@ "set_hardware_component_state", "switch_controllers", "unload_controller", + "cleanup_controller", ] diff --git a/controller_manager/controller_manager/controller_manager_services.py b/controller_manager/controller_manager/controller_manager_services.py index 62b350f7d3..82a5fde2f9 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 @@ -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 + ) diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py index 3ca5600997..16cbd5df55 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/controller_manager/controller_manager/spawner.py @@ -28,6 +28,7 @@ load_controller, switch_controllers, unload_controller, + cleanup_controller, ) import rclpy @@ -101,6 +102,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 diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 44b3c3215a..33adff2bbf 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -39,6 +39,7 @@ #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" @@ -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 get_loaded_controllers() const; @@ -296,6 +300,11 @@ class ControllerManager : public rclcpp::Node const std::shared_ptr request, std::shared_ptr response); + CONTROLLER_MANAGER_PUBLIC + void cleanup_controller_service_cb( + const std::shared_ptr request, + std::shared_ptr response); + CONTROLLER_MANAGER_PUBLIC void list_controller_types_srv_cb( const std::shared_ptr request, @@ -521,6 +530,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 f588720b3d..26a9112a9a 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -504,6 +504,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", @@ -664,6 +668,61 @@ 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 guard(rt_controllers_wrapper_.controllers_lock_); + std::vector & to = rt_controllers_wrapper_.get_unused_list(guard); + const std::vector & 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 & 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 ControllerManager::get_loaded_controllers() const { std::lock_guard guard(rt_controllers_wrapper_.controllers_lock_); @@ -1846,6 +1905,22 @@ 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_msgs/CMakeLists.txt b/controller_manager_msgs/CMakeLists.txt index 2a863c29dd..ba00ce48af 100644 --- a/controller_manager_msgs/CMakeLists.txt +++ b/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} 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/ros2controlcli/verb/cleanup_controller.py b/ros2controlcli/ros2controlcli/verb/cleanup_controller.py new file mode 100644 index 0000000000..1aecacdefa --- /dev/null +++ b/ros2controlcli/ros2controlcli/verb/cleanup_controller.py @@ -0,0 +1,40 @@ +# 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 + +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) as node: + response = cleanup_controller(node, args.controller_manager, args.controller_name) + if not response.ok: + return "Error cleanup controllers, check controller_manager logs" + + print(f"Successfully cleaned up controller {args.controller_name}") + return 0 diff --git a/ros2controlcli/setup.py b/ros2controlcli/setup.py index b3c870e6e0..13e1d49fd6 100644 --- a/ros2controlcli/setup.py +++ b/ros2controlcli/setup.py @@ -64,6 +64,7 @@ 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", ], }, ) From bde033998fa1b2826230a6e3bdcea658790b6866 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Mon, 22 Jan 2024 16:13:03 +0100 Subject: [PATCH 04/10] Optimize cleanup controllers method. --- controller_manager/src/controller_manager.cpp | 117 ++++++++++-------- 1 file changed, 65 insertions(+), 52 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 26a9112a9a..8c7946fbef 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/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; @@ -606,41 +618,46 @@ 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 guard(rt_controllers_wrapper_.controllers_lock_); - std::vector & to = rt_controllers_wrapper_.get_unused_list(guard); - const std::vector & 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", @@ -648,36 +665,45 @@ controller_interface::return_type ControllerManager::unload_controller( async_controller_threads_.erase(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(); - executor_->remove_node(controller.c->get_node()->get_node_base_interface()); - to.erase(found_it); + // 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); + } - // 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 & 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(), "Cleanup controller"); + 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 unloaded controller '%s'", controller_name.c_str()); + RCLCPP_DEBUG(get_logger(), "Successfully cleaned-up controller '%s'", controller_name.c_str()); return controller_interface::return_type::OK; } -controller_interface::return_type ControllerManager::cleanup_controller( +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 guard(rt_controllers_wrapper_.controllers_lock_); std::vector & to = rt_controllers_wrapper_.get_unused_list(guard); const std::vector & 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)); @@ -687,26 +713,14 @@ controller_interface::return_type ControllerManager::cleanup_controller( to.clear(); RCLCPP_ERROR( get_logger(), - "Could not clear controller with name '%s' because no controller with this name exists", + "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; - 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(); + RCLCPP_DEBUG(get_logger(), "Unload controller"); executor_->remove_node(controller.c->get_node()->get_node_base_interface()); to.erase(found_it); @@ -718,7 +732,7 @@ controller_interface::return_type ControllerManager::cleanup_controller( new_unused_list.clear(); RCLCPP_DEBUG(get_logger(), "Destruct controller finished"); - RCLCPP_DEBUG(get_logger(), "Successfully cleaned controller '%s'", controller_name.c_str()); + RCLCPP_DEBUG(get_logger(), "Successfully unloaded controller '%s'", controller_name.c_str()); return controller_interface::return_type::OK; } @@ -782,7 +796,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; } @@ -1910,8 +1924,7 @@ void ControllerManager::cleanup_controller_service_cb( std::shared_ptr response) { // lock services - RCLCPP_DEBUG( - get_logger(), "cleanup service called for controller '%s' ", request->name.c_str()); + 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"); From 2607b386a0c641ee621b7dedfe3cdd43dd23df2b Mon Sep 17 00:00:00 2001 From: baila Date: Wed, 20 Dec 2023 18:45:45 +0530 Subject: [PATCH 05/10] Fixing format --- .../include/controller_manager/controller_manager.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 33adff2bbf..e22593a376 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/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" @@ -39,7 +40,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" From fd1449b45bef13e9ebb5c0954b95e35c0eba6da3 Mon Sep 17 00:00:00 2001 From: baila Date: Thu, 25 Jan 2024 09:57:31 +0530 Subject: [PATCH 06/10] fixing pre-commit changes --- controller_manager/controller_manager/spawner.py | 1 - 1 file changed, 1 deletion(-) diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py index 16cbd5df55..1b484caf7b 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/controller_manager/controller_manager/spawner.py @@ -28,7 +28,6 @@ load_controller, switch_controllers, unload_controller, - cleanup_controller, ) import rclpy From bcb013d159ba691519101c80b9e9c3d463a1e42f Mon Sep 17 00:00:00 2001 From: baila Date: Thu, 25 Jan 2024 23:01:28 +0530 Subject: [PATCH 07/10] Adding tests for cleanup service. --- .../test/test_controller_manager_srvs.cpp | 62 +++++++++++++++++++ 1 file changed, 62 insertions(+) diff --git a/controller_manager/test/test_controller_manager_srvs.cpp b/controller_manager/test/test_controller_manager_srvs.cpp index 2cd5645bd8..1071fd3d9f 100644 --- a/controller_manager/test/test_controller_manager_srvs.cpp +++ b/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("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(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_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; From 6a6fe91304ebab38b01170ef3dc2b2ef92b38ee5 Mon Sep 17 00:00:00 2001 From: baila Date: Tue, 5 Mar 2024 20:00:07 +0530 Subject: [PATCH 08/10] Adding controller_manager tests for cleanup controller --- controller_manager/CMakeLists.txt | 11 + .../test/test_cleanup_controller.cpp | 102 +++++++ o | 285 ++++++++++++++++++ 3 files changed, 398 insertions(+) create mode 100644 controller_manager/test/test_cleanup_controller.cpp create mode 100644 o diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index 376cef3362..cb9c0bbf30 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -121,6 +121,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/test/test_cleanup_controller.cpp b/controller_manager/test/test_cleanup_controller.cpp new file mode 100644 index 0000000000..d2f52a062b --- /dev/null +++ b/controller_manager/test/test_cleanup_controller.cpp @@ -0,0 +1,102 @@ +// 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" + +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_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_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_state().id()); + + cm_->configure_controller(CONTROLLER_NAME_1); + + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_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_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_state().id()); + + cm_->configure_controller(CONTROLLER_NAME_1); + + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_state().id()); + + switch_test_controllers(strvec{CONTROLLER_NAME_1}, strvec{}, STRICT); + + 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_state().id()); +} diff --git a/o b/o new file mode 100644 index 0000000000..755c65da9b --- /dev/null +++ b/o @@ -0,0 +1,285 @@ +build/controller_interface/Testing/20240305-1418/Test.xml: 6 tests, 0 errors, 0 failures, 0 skipped +build/controller_interface/test_results/controller_interface/test_chainable_controller_interface.gtest.xml: 6 tests, 0 errors, 0 failures, 0 skipped +build/controller_interface/test_results/controller_interface/test_controller_interface.gtest.xml: 5 tests, 0 errors, 0 failures, 0 skipped +build/controller_interface/test_results/controller_interface/test_controller_with_options.gtest.xml: 5 tests, 0 errors, 0 failures, 0 skipped +build/controller_interface/test_results/controller_interface/test_force_torque_sensor.gtest.xml: 3 tests, 0 errors, 0 failures, 0 skipped +build/controller_interface/test_results/controller_interface/test_imu_sensor.gtest.xml: 1 test, 0 errors, 0 failures, 0 skipped +build/controller_interface/test_results/controller_interface/test_semantic_component_interface.gtest.xml: 3 tests, 0 errors, 0 failures, 0 skipped +build/controller_manager/Testing/20240305-1418/Test.xml: 11 tests, 0 errors, 1 failure, 0 skipped +- test_cleanup_controller + <<< failure message + -- run_test.py: extra environment variables to append: + - AMENT_PREFIX_PATH+=_ + -- run_test.py: invoking following command in '/home/baila/stoglrobotics/issue-759/src/ros2_control/build/controller_manager': + - /home/baila/stoglrobotics/issue-759/src/ros2_control/build/controller_manager/test_cleanup_controller --gtest_output=xml:/home/baila/stoglrobotics/issue-759/src/ros2_control/build/controller_manager/test_results/controller_manager/test_cleanup_controller.gtest.xml + Running main() from gmock_main.cc + [==========] Running 6 tests from 1 test suite. + [----------] Global test environment set-up. + [----------] 6 tests from TestCleanupController + [ RUN ] TestCleanupController.cleanup_unknown_controller + [WARN] [1709648352.713765099] [test_controller_manager]: 'update_rate' parameter not set, using default value. + [INFO] [1709648352.716240433] [test_controller_manager]: Subscribing to '/test_controller_manager/robot_description' topic for robot description. + [INFO] [1709648352.716495380] [test_controller_manager]: Received robot description from topic. + [INFO] [1709648352.716955811] [resource_manager]: Loading hardware 'TestActuatorHardware' + [INFO] [1709648352.718008153] [resource_manager]: Initialize hardware 'TestActuatorHardware' + [INFO] [1709648352.718421785] [resource_manager]: Successful initialization of hardware 'TestActuatorHardware' + [INFO] [1709648352.718731404] [resource_manager]: Loading hardware 'TestSensorHardware' + [INFO] [1709648352.718911796] [resource_manager]: Initialize hardware 'TestSensorHardware' + [INFO] [1709648352.718952394] [resource_manager]: Successful initialization of hardware 'TestSensorHardware' + [INFO] [1709648352.718967880] [resource_manager]: Loading hardware 'TestSystemHardware' + [INFO] [1709648352.719086218] [resource_manager]: Initialize hardware 'TestSystemHardware' + [INFO] [1709648352.719107293] [resource_manager]: Successful initialization of hardware 'TestSystemHardware' + [INFO] [1709648352.719301299] [resource_manager]: 'configure' hardware 'TestSystemHardware' + [INFO] [1709648352.719320280] [resource_manager]: Successful 'configure' of hardware 'TestSystemHardware' + [INFO] [1709648352.719332430] [resource_manager]: 'activate' hardware 'TestSystemHardware' + [INFO] [1709648352.719335366] [resource_manager]: Successful 'activate' of hardware 'TestSystemHardware' + [INFO] [1709648352.719363813] [resource_manager]: 'configure' hardware 'TestSensorHardware' + [INFO] [1709648352.719372036] [resource_manager]: Successful 'configure' of hardware 'TestSensorHardware' + [INFO] [1709648352.719375913] [resource_manager]: 'activate' hardware 'TestSensorHardware' + [INFO] [1709648352.719378538] [resource_manager]: Successful 'activate' of hardware 'TestSensorHardware' + [INFO] [1709648352.719397089] [resource_manager]: 'configure' hardware 'TestActuatorHardware' + [INFO] [1709648352.719404121] [resource_manager]: Successful 'configure' of hardware 'TestActuatorHardware' + [INFO] [1709648352.719409089] [resource_manager]: 'activate' hardware 'TestActuatorHardware' + [INFO] [1709648352.719411703] [resource_manager]: Successful 'activate' of hardware 'TestActuatorHardware' + [INFO] [1709648352.741448290] [test_controller_manager]: Cleanup controller 'unknown_controller_name' + [ERROR] [1709648352.741724532] [test_controller_manager]: Could not cleanup controller with name 'unknown_controller_name' because no controller with this name exists + [ OK ] TestCleanupController.cleanup_unknown_controller (413 ms) + [ RUN ] TestCleanupController.cleanup_controller_failed_init + [WARN] [1709648352.779081582] [test_controller_manager]: 'update_rate' parameter not set, using default value. + [INFO] [1709648352.780118854] [test_controller_manager]: Subscribing to '/test_controller_manager/robot_description' topic for robot description. + [INFO] [1709648352.781130483] [test_controller_manager]: Received robot description from topic. + [INFO] [1709648352.781368280] [resource_manager]: Loading hardware 'TestActuatorHardware' + [INFO] [1709648352.782449371] [resource_manager]: Initialize hardware 'TestActuatorHardware' + [INFO] [1709648352.782724752] [resource_manager]: Successful initialization of hardware 'TestActuatorHardware' + [INFO] [1709648352.782838112] [resource_manager]: Loading hardware 'TestSensorHardware' + [INFO] [1709648352.782921031] [resource_manager]: Initialize hardware 'TestSensorHardware' + [INFO] [1709648352.782950409] [resource_manager]: Successful initialization of hardware 'TestSensorHardware' + [INFO] [1709648352.782962080] [resource_manager]: Loading hardware 'TestSystemHardware' + [INFO] [1709648352.783029512] [resource_manager]: Initialize hardware 'TestSystemHardware' + [INFO] [1709648352.783099559] [resource_manager]: Successful initialization of hardware 'TestSystemHardware' + [INFO] [1709648352.783174605] [resource_manager]: 'configure' hardware 'TestSystemHardware' + [INFO] [1709648352.783181397] [resource_manager]: Successful 'configure' of hardware 'TestSystemHardware' + [INFO] [1709648352.783188098] [resource_manager]: 'activate' hardware 'TestSystemHardware' + [INFO] [1709648352.783190602] [resource_manager]: Successful 'activate' of hardware 'TestSystemHardware' + [INFO] [1709648352.783196481] [resource_manager]: 'configure' hardware 'TestSensorHardware' + [INFO] [1709648352.783199257] [resource_manager]: Successful 'configure' of hardware 'TestSensorHardware' + [INFO] [1709648352.783202362] [resource_manager]: 'activate' hardware 'TestSensorHardware' + [INFO] [1709648352.783204666] [resource_manager]: Successful 'activate' of hardware 'TestSensorHardware' + [INFO] [1709648352.783209063] [resource_manager]: 'configure' hardware 'TestActuatorHardware' + [INFO] [1709648352.783211817] [resource_manager]: Successful 'configure' of hardware 'TestActuatorHardware' + [INFO] [1709648352.783215985] [resource_manager]: 'activate' hardware 'TestActuatorHardware' + [INFO] [1709648352.783218258] [resource_manager]: Successful 'activate' of hardware 'TestActuatorHardware' + [INFO] [1709648352.789308626] [test_controller_manager]: Loading controller 'test_controller_failed_init' + [ERROR] [1709648352.798406709] [test_controller_manager]: Could not initialize the controller named 'test_controller_failed_init' + [INFO] [1709648352.803143770] [test_controller_manager]: Cleanup controller 'test_controller_failed_init' + [ERROR] [1709648352.803258853] [test_controller_manager]: Could not cleanup controller with name 'test_controller_failed_init' because no controller with this name exists + [ OK ] TestCleanupController.cleanup_controller_failed_init (58 ms) + [ RUN ] TestCleanupController.cleanup_non_loaded_controller_fails + [WARN] [1709648352.831705234] [test_controller_manager]: 'update_rate' parameter not set, using default value. + [INFO] [1709648352.832424878] [test_controller_manager]: Subscribing to '/test_controller_manager/robot_description' topic for robot description. + [INFO] [1709648352.832546492] [test_controller_manager]: Received robot description from topic. + [INFO] [1709648352.832739695] [resource_manager]: Loading hardware 'TestActuatorHardware' + [INFO] [1709648352.833474846] [resource_manager]: Initialize hardware 'TestActuatorHardware' + [INFO] [1709648352.833753723] [resource_manager]: Successful initialization of hardware 'TestActuatorHardware' + [INFO] [1709648352.834030396] [resource_manager]: Loading hardware 'TestSensorHardware' + [INFO] [1709648352.834137326] [resource_manager]: Initialize hardware 'TestSensorHardware' + [INFO] [1709648352.834257747] [resource_manager]: Successful initialization of hardware 'TestSensorHardware' + [INFO] [1709648352.834276208] [resource_manager]: Loading hardware 'TestSystemHardware' + [INFO] [1709648352.834356172] [resource_manager]: Initialize hardware 'TestSystemHardware' + [INFO] [1709648352.834372349] [resource_manager]: Successful initialization of hardware 'TestSystemHardware' + [INFO] [1709648352.835561150] [resource_manager]: 'configure' hardware 'TestSystemHardware' + [INFO] [1709648352.835573430] [resource_manager]: Successful 'configure' of hardware 'TestSystemHardware' + [INFO] [1709648352.835580893] [resource_manager]: 'activate' hardware 'TestSystemHardware' + [INFO] [1709648352.835584348] [resource_manager]: Successful 'activate' of hardware 'TestSystemHardware' + [INFO] [1709648352.835591590] [resource_manager]: 'configure' hardware 'TestSensorHardware' + [INFO] [1709648352.835594866] [resource_manager]: Successful 'configure' of hardware 'TestSensorHardware' + [INFO] [1709648352.835598161] [resource_manager]: 'activate' hardware 'TestSensorHardware' + [INFO] [1709648352.835600496] [resource_manager]: Successful 'activate' of hardware 'TestSensorHardware' + [INFO] [1709648352.835605133] [resource_manager]: 'configure' hardware 'TestActuatorHardware' + [INFO] [1709648352.835608198] [resource_manager]: Successful 'configure' of hardware 'TestActuatorHardware' + [INFO] [1709648352.835612826] [resource_manager]: 'activate' hardware 'TestActuatorHardware' + [INFO] [1709648352.835615190] [resource_manager]: Successful 'activate' of hardware 'TestActuatorHardware' + [INFO] [1709648352.844268845] [test_controller_manager]: Cleanup controller 'test_controller1' + [ERROR] [1709648352.844355058] [test_controller_manager]: Could not cleanup controller with name 'test_controller1' because no controller with this name exists + [ OK ] TestCleanupController.cleanup_non_loaded_controller_fails (35 ms) + [ RUN ] TestCleanupController.cleanup_unconfigured_controller + [WARN] [1709648352.879524024] [test_controller_manager]: 'update_rate' parameter not set, using default value. + [INFO] [1709648352.880005109] [test_controller_manager]: Subscribing to '/test_controller_manager/robot_description' topic for robot description. + [INFO] [1709648352.880103474] [test_controller_manager]: Received robot description from topic. + [INFO] [1709648352.880227933] [resource_manager]: Loading hardware 'TestActuatorHardware' + [INFO] [1709648352.880996569] [resource_manager]: Initialize hardware 'TestActuatorHardware' + [INFO] [1709648352.881193329] [resource_manager]: Successful initialization of hardware 'TestActuatorHardware' + [INFO] [1709648352.881328605] [resource_manager]: Loading hardware 'TestSensorHardware' + [INFO] [1709648352.882264743] [resource_manager]: Initialize hardware 'TestSensorHardware' + [INFO] [1709648352.882319635] [resource_manager]: Successful initialization of hardware 'TestSensorHardware' + [INFO] [1709648352.882339098] [resource_manager]: Loading hardware 'TestSystemHardware' + [INFO] [1709648352.884077067] [resource_manager]: Initialize hardware 'TestSystemHardware' + [INFO] [1709648352.884096359] [resource_manager]: Successful initialization of hardware 'TestSystemHardware' + [INFO] [1709648352.884194314] [resource_manager]: 'configure' hardware 'TestSystemHardware' + [INFO] [1709648352.884202277] [resource_manager]: Successful 'configure' of hardware 'TestSystemHardware' + [INFO] [1709648352.884208928] [resource_manager]: 'activate' hardware 'TestSystemHardware' + [INFO] [1709648352.884211913] [resource_manager]: Successful 'activate' of hardware 'TestSystemHardware' + [INFO] [1709648352.884218674] [resource_manager]: 'configure' hardware 'TestSensorHardware' + [INFO] [1709648352.884221660] [resource_manager]: Successful 'configure' of hardware 'TestSensorHardware' + [INFO] [1709648352.884224895] [resource_manager]: 'activate' hardware 'TestSensorHardware' + [INFO] [1709648352.884227139] [resource_manager]: Successful 'activate' of hardware 'TestSensorHardware' + [INFO] [1709648352.884232227] [resource_manager]: 'configure' hardware 'TestActuatorHardware' + [INFO] [1709648352.884236414] [resource_manager]: Successful 'configure' of hardware 'TestActuatorHardware' + [INFO] [1709648352.884242204] [resource_manager]: 'activate' hardware 'TestActuatorHardware' + [INFO] [1709648352.884245028] [resource_manager]: Successful 'activate' of hardware 'TestActuatorHardware' + [INFO] [1709648352.892463199] [test_controller_manager]: Loading controller 'test_controller1' + [INFO] [1709648352.901130901] [test_controller_manager]: Cleanup controller 'test_controller1' + [ OK ] TestCleanupController.cleanup_unconfigured_controller (78 ms) + [ RUN ] TestCleanupController.cleanup_inactive_controller + [WARN] [1709648352.946091267] [test_controller_manager]: 'update_rate' parameter not set, using default value. + [INFO] [1709648352.947639820] [test_controller_manager]: Subscribing to '/test_controller_manager/robot_description' topic for robot description. + [INFO] [1709648352.947801780] [test_controller_manager]: Received robot description from topic. + [INFO] [1709648352.947941685] [resource_manager]: Loading hardware 'TestActuatorHardware' + [INFO] [1709648352.948895892] [resource_manager]: Initialize hardware 'TestActuatorHardware' + [INFO] [1709648352.949098662] [resource_manager]: Successful initialization of hardware 'TestActuatorHardware' + [INFO] [1709648352.949287859] [resource_manager]: Loading hardware 'TestSensorHardware' + [INFO] [1709648352.949439523] [resource_manager]: Initialize hardware 'TestSensorHardware' + [INFO] [1709648352.949527420] [resource_manager]: Successful initialization of hardware 'TestSensorHardware' + [INFO] [1709648352.949545771] [resource_manager]: Loading hardware 'TestSystemHardware' + [INFO] [1709648352.949624303] [resource_manager]: Initialize hardware 'TestSystemHardware' + [INFO] [1709648352.949689992] [resource_manager]: Successful initialization of hardware 'TestSystemHardware' + [INFO] [1709648352.949777259] [resource_manager]: 'configure' hardware 'TestSystemHardware' + [INFO] [1709648352.949832451] [resource_manager]: Successful 'configure' of hardware 'TestSystemHardware' + [INFO] [1709648352.949844451] [resource_manager]: 'activate' hardware 'TestSystemHardware' + [INFO] [1709648352.949863583] [resource_manager]: Successful 'activate' of hardware 'TestSystemHardware' + [INFO] [1709648352.949875012] [resource_manager]: 'configure' hardware 'TestSensorHardware' + [INFO] [1709648352.949878357] [resource_manager]: Successful 'configure' of hardware 'TestSensorHardware' + [INFO] [1709648352.949881804] [resource_manager]: 'activate' hardware 'TestSensorHardware' + [INFO] [1709648352.949884027] [resource_manager]: Successful 'activate' of hardware 'TestSensorHardware' + [INFO] [1709648352.949888865] [resource_manager]: 'configure' hardware 'TestActuatorHardware' + [INFO] [1709648352.949891760] [resource_manager]: Successful 'configure' of hardware 'TestActuatorHardware' + [INFO] [1709648352.949896278] [resource_manager]: 'activate' hardware 'TestActuatorHardware' + [INFO] [1709648352.949898562] [resource_manager]: Successful 'activate' of hardware 'TestActuatorHardware' + [INFO] [1709648352.956828178] [test_controller_manager]: Loading controller 'test_controller1' + [INFO] [1709648352.963166611] [test_controller_manager]: Configuring controller 'test_controller1' + [INFO] [1709648352.963721269] [test_controller_manager]: Cleanup controller 'test_controller1' + [ OK ] TestCleanupController.cleanup_inactive_controller (58 ms) + [ RUN ] TestCleanupController.cleanup_active_controller + [WARN] [1709648353.016982088] [test_controller_manager]: 'update_rate' parameter not set, using default value. + [INFO] [1709648353.017724200] [test_controller_manager]: Subscribing to '/test_controller_manager/robot_description' topic for robot description. + [INFO] [1709648353.019986023] [test_controller_manager]: Received robot description from topic. + [INFO] [1709648353.020122611] [resource_manager]: Loading hardware 'TestActuatorHardware' + [INFO] [1709648353.023900267] [resource_manager]: Initialize hardware 'TestActuatorHardware' + [INFO] [1709648353.024091937] [resource_manager]: Successful initialization of hardware 'TestActuatorHardware' + [INFO] [1709648353.024318978] [resource_manager]: Loading hardware 'TestSensorHardware' + [INFO] [1709648353.024456839] [resource_manager]: Initialize hardware 'TestSensorHardware' + [INFO] [1709648353.024494182] [resource_manager]: Successful initialization of hardware 'TestSensorHardware' + [INFO] [1709648353.024507994] [resource_manager]: Loading hardware 'TestSystemHardware' + [INFO] [1709648353.024578282] [resource_manager]: Initialize hardware 'TestSystemHardware' + [INFO] [1709648353.024593628] [resource_manager]: Successful initialization of hardware 'TestSystemHardware' + [INFO] [1709648353.024675194] [resource_manager]: 'configure' hardware 'TestSystemHardware' + [INFO] [1709648353.024684610] [resource_manager]: Successful 'configure' of hardware 'TestSystemHardware' + [INFO] [1709648353.024691732] [resource_manager]: 'activate' hardware 'TestSystemHardware' + [INFO] [1709648353.024694386] [resource_manager]: Successful 'activate' of hardware 'TestSystemHardware' + [INFO] [1709648353.024700817] [resource_manager]: 'configure' hardware 'TestSensorHardware' + [INFO] [1709648353.024703902] [resource_manager]: Successful 'configure' of hardware 'TestSensorHardware' + [INFO] [1709648353.024707198] [resource_manager]: 'activate' hardware 'TestSensorHardware' + [INFO] [1709648353.024709511] [resource_manager]: Successful 'activate' of hardware 'TestSensorHardware' + [INFO] [1709648353.024713969] [resource_manager]: 'configure' hardware 'TestActuatorHardware' + [INFO] [1709648353.024716875] [resource_manager]: Successful 'configure' of hardware 'TestActuatorHardware' + [INFO] [1709648353.024721342] [resource_manager]: 'activate' hardware 'TestActuatorHardware' + [INFO] [1709648353.024723565] [resource_manager]: Successful 'activate' of hardware 'TestActuatorHardware' + [INFO] [1709648353.031697483] [test_controller_manager]: Loading controller 'test_controller1' + [INFO] [1709648353.040695869] [test_controller_manager]: Configuring controller 'test_controller1' + [INFO] [1709648353.172182596] [test_controller_manager]: Cleanup controller 'test_controller1' + [ERROR] [1709648353.172382230] [test_controller_manager]: Controller 'test_controller1' can not be cleaned-up from 'active' state. + /home/baila/stoglrobotics/issue-759/src/ros2_control/controller_manager/test/test_cleanup_controller.cpp:108: Failure + Expected equality of these values: + cm_->cleanup_controller(CONTROLLER_NAME_1) + Which is: 1-byte object <01> + controller_interface::return_type::OK + Which is: 1-byte object <00> + [ FAILED ] TestCleanupController.cleanup_active_controller (213 ms) + [----------] 6 tests from TestCleanupController (856 ms total) + + [----------] Global test environment tear-down + [==========] 6 tests from 1 test suite ran. (863 ms total) + [ PASSED ] 5 tests. + [ FAILED ] 1 test, listed below: + [ FAILED ] TestCleanupController.cleanup_active_controller + + 1 FAILED TEST + -- run_test.py: return code 1 + -- run_test.py: inject classname prefix into gtest result file '/home/baila/stoglrobotics/issue-759/src/ros2_control/build/controller_manager/test_results/controller_manager/test_cleanup_controller.gtest.xml' + -- run_test.py: verify result file '/home/baila/stoglrobotics/issue-759/src/ros2_control/build/controller_manager/test_results/controller_manager/test_cleanup_controller.gtest.xml' + >>> +build/controller_manager/test_results/controller_manager/test_cleanup_controller.gtest.xml: 6 tests, 0 errors, 1 failure, 0 skipped +- controller_manager.TestCleanupController cleanup_active_controller + <<< failure message + /home/baila/stoglrobotics/issue-759/src/ros2_control/controller_manager/test/test_cleanup_controller.cpp:108 + Expected equality of these values: + cm_->cleanup_controller(CONTROLLER_NAME_1) + Which is: 1-byte object <01> + controller_interface::return_type::OK + Which is: 1-byte object <00> + >>> +build/controller_manager/test_results/controller_manager/test_controller_manager.gtest.xml: 19 tests, 0 errors, 0 failures, 0 skipped +build/controller_manager/test_results/controller_manager/test_controller_manager_hardware_error_handling.gtest.xml: 6 tests, 0 errors, 0 failures, 0 skipped +build/controller_manager/test_results/controller_manager/test_controller_manager_srvs.gtest.xml: 15 tests, 0 errors, 0 failures, 0 skipped +build/controller_manager/test_results/controller_manager/test_controller_manager_urdf_passing.gtest.xml: 3 tests, 0 errors, 0 failures, 0 skipped +build/controller_manager/test_results/controller_manager/test_controller_manager_with_namespace.gtest.xml: 2 tests, 0 errors, 0 failures, 0 skipped +build/controller_manager/test_results/controller_manager/test_controllers_chaining_with_controller_manager.gtest.xml: 12 tests, 0 errors, 0 failures, 0 skipped +build/controller_manager/test_results/controller_manager/test_hardware_management_srvs.gtest.xml: 4 tests, 0 errors, 0 failures, 0 skipped +build/controller_manager/test_results/controller_manager/test_load_controller.gtest.xml: 39 tests, 0 errors, 0 failures, 0 skipped +build/controller_manager/test_results/controller_manager/test_release_interfaces.gtest.xml: 1 test, 0 errors, 0 failures, 0 skipped +build/controller_manager/test_results/controller_manager/test_spawner_unspawner.gtest.xml: 7 tests, 0 errors, 0 failures, 0 skipped +build/controller_manager_msgs/Testing/20240305-1418/Test.xml: 29 tests, 0 errors, 0 failures, 0 skipped +build/controller_manager_msgs/test_results/controller_manager_msgs/cppcheck_rosidl_generated_c.xunit.xml: 106 tests, 0 errors, 0 failures, 106 skipped +build/controller_manager_msgs/test_results/controller_manager_msgs/cppcheck_rosidl_generated_cpp.xunit.xml: 60 tests, 0 errors, 0 failures, 60 skipped +build/controller_manager_msgs/test_results/controller_manager_msgs/cppcheck_rosidl_generated_py.xunit.xml: 18 tests, 0 errors, 0 failures, 18 skipped +build/controller_manager_msgs/test_results/controller_manager_msgs/cppcheck_rosidl_typesupport_c.xunit.xml: 15 tests, 0 errors, 0 failures, 15 skipped +build/controller_manager_msgs/test_results/controller_manager_msgs/cppcheck_rosidl_typesupport_cpp.xunit.xml: 15 tests, 0 errors, 0 failures, 15 skipped +build/controller_manager_msgs/test_results/controller_manager_msgs/cppcheck_rosidl_typesupport_fastrtps_c.xunit.xml: 30 tests, 0 errors, 0 failures, 30 skipped +build/controller_manager_msgs/test_results/controller_manager_msgs/cppcheck_rosidl_typesupport_fastrtps_cpp.xunit.xml: 30 tests, 0 errors, 0 failures, 30 skipped +build/controller_manager_msgs/test_results/controller_manager_msgs/cppcheck_rosidl_typesupport_introspection_c.xunit.xml: 31 tests, 0 errors, 0 failures, 31 skipped +build/controller_manager_msgs/test_results/controller_manager_msgs/cppcheck_rosidl_typesupport_introspection_cpp.xunit.xml: 30 tests, 0 errors, 0 failures, 30 skipped +build/controller_manager_msgs/test_results/controller_manager_msgs/cpplint_rosidl_generated_c.xunit.xml: 106 tests, 0 errors, 0 failures, 0 skipped +build/controller_manager_msgs/test_results/controller_manager_msgs/cpplint_rosidl_generated_cpp.xunit.xml: 60 tests, 0 errors, 0 failures, 0 skipped +build/controller_manager_msgs/test_results/controller_manager_msgs/cpplint_rosidl_generated_py.xunit.xml: 18 tests, 0 errors, 0 failures, 0 skipped +build/controller_manager_msgs/test_results/controller_manager_msgs/cpplint_rosidl_typesupport_c.xunit.xml: 15 tests, 0 errors, 0 failures, 0 skipped +build/controller_manager_msgs/test_results/controller_manager_msgs/cpplint_rosidl_typesupport_cpp.xunit.xml: 15 tests, 0 errors, 0 failures, 0 skipped +build/controller_manager_msgs/test_results/controller_manager_msgs/cpplint_rosidl_typesupport_fastrtps_c.xunit.xml: 30 tests, 0 errors, 0 failures, 0 skipped +build/controller_manager_msgs/test_results/controller_manager_msgs/cpplint_rosidl_typesupport_fastrtps_cpp.xunit.xml: 30 tests, 0 errors, 0 failures, 0 skipped +build/controller_manager_msgs/test_results/controller_manager_msgs/cpplint_rosidl_typesupport_introspection_c.xunit.xml: 31 tests, 0 errors, 0 failures, 0 skipped +build/controller_manager_msgs/test_results/controller_manager_msgs/cpplint_rosidl_typesupport_introspection_cpp.xunit.xml: 30 tests, 0 errors, 0 failures, 0 skipped +build/controller_manager_msgs/test_results/controller_manager_msgs/flake8_rosidl_generated_py.xunit.xml: 1 test, 0 errors, 0 failures, 0 skipped +build/controller_manager_msgs/test_results/controller_manager_msgs/pep257_rosidl_generated_py.xunit.xml: 18 tests, 0 errors, 0 failures, 0 skipped +build/controller_manager_msgs/test_results/controller_manager_msgs/uncrustify_rosidl_generated_c.xunit.xml: 106 tests, 0 errors, 0 failures, 0 skipped +build/controller_manager_msgs/test_results/controller_manager_msgs/uncrustify_rosidl_generated_cpp.xunit.xml: 60 tests, 0 errors, 0 failures, 0 skipped +build/controller_manager_msgs/test_results/controller_manager_msgs/uncrustify_rosidl_generated_py.xunit.xml: 18 tests, 0 errors, 0 failures, 0 skipped +build/controller_manager_msgs/test_results/controller_manager_msgs/uncrustify_rosidl_typesupport_c.xunit.xml: 15 tests, 0 errors, 0 failures, 0 skipped +build/controller_manager_msgs/test_results/controller_manager_msgs/uncrustify_rosidl_typesupport_cpp.xunit.xml: 15 tests, 0 errors, 0 failures, 0 skipped +build/controller_manager_msgs/test_results/controller_manager_msgs/uncrustify_rosidl_typesupport_fastrtps_c.xunit.xml: 30 tests, 0 errors, 0 failures, 0 skipped +build/controller_manager_msgs/test_results/controller_manager_msgs/uncrustify_rosidl_typesupport_fastrtps_cpp.xunit.xml: 30 tests, 0 errors, 0 failures, 0 skipped +build/controller_manager_msgs/test_results/controller_manager_msgs/uncrustify_rosidl_typesupport_introspection_c.xunit.xml: 31 tests, 0 errors, 0 failures, 0 skipped +build/controller_manager_msgs/test_results/controller_manager_msgs/uncrustify_rosidl_typesupport_introspection_cpp.xunit.xml: 30 tests, 0 errors, 0 failures, 0 skipped +build/hardware_interface/Testing/20240305-1418/Test.xml: 6 tests, 0 errors, 0 failures, 0 skipped +build/hardware_interface/test_results/hardware_interface/test_component_interfaces.gtest.xml: 9 tests, 0 errors, 0 failures, 0 skipped +build/hardware_interface/test_results/hardware_interface/test_component_parser.gtest.xml: 24 tests, 0 errors, 0 failures, 0 skipped +build/hardware_interface/test_results/hardware_interface/test_generic_system.gtest.xml: 20 tests, 0 errors, 0 failures, 0 skipped +build/hardware_interface/test_results/hardware_interface/test_inst_hardwares.gtest.xml: 3 tests, 0 errors, 0 failures, 0 skipped +build/hardware_interface/test_results/hardware_interface/test_joint_handle.gtest.xml: 5 tests, 0 errors, 0 failures, 0 skipped +build/hardware_interface/test_results/hardware_interface/test_macros.gtest.xml: 2 tests, 0 errors, 0 failures, 0 skipped +build/hardware_interface_testing/Testing/20240305-1418/Test.xml: 2 tests, 0 errors, 0 failures, 0 skipped +build/hardware_interface_testing/test_results/hardware_interface_testing/test_resource_manager.gtest.xml: 25 tests, 0 errors, 0 failures, 0 skipped +build/hardware_interface_testing/test_results/hardware_interface_testing/test_resource_manager_prepare_perform_switch.gtest.xml: 5 tests, 0 errors, 0 failures, 0 skipped +build/joint_limits/Testing/20240305-1418/Test.xml: 2 tests, 0 errors, 0 failures, 0 skipped +build/joint_limits/test_results/joint_limits/joint_limits_urdf_test.gtest.xml: 2 tests, 0 errors, 0 failures, 0 skipped +build/joint_limits/test_results/joint_limits/test_joint_limits_rosparam.launch.py.xunit.xml: 2 tests, 0 errors, 0 failures, 0 skipped +build/ros2controlcli/pytest.xml: 1 test, 0 errors, 0 failures, 0 skipped +build/rqt_controller_manager/pytest.xml: 0 tests, 0 errors, 0 failures, 0 skipped +build/transmission_interface/Testing/20240305-1418/Test.xml: 7 tests, 0 errors, 0 failures, 0 skipped +build/transmission_interface/test_results/transmission_interface/test_differential_transmission.gtest.xml: 8 tests, 0 errors, 0 failures, 0 skipped +build/transmission_interface/test_results/transmission_interface/test_differential_transmission_loader.gtest.xml: 6 tests, 0 errors, 0 failures, 0 skipped +build/transmission_interface/test_results/transmission_interface/test_four_bar_linkage_transmission.gtest.xml: 8 tests, 0 errors, 0 failures, 0 skipped +build/transmission_interface/test_results/transmission_interface/test_four_bar_linkage_transmission_loader.gtest.xml: 6 tests, 0 errors, 0 failures, 0 skipped +build/transmission_interface/test_results/transmission_interface/test_simple_transmission.gtest.xml: 11 tests, 0 errors, 0 failures, 0 skipped +build/transmission_interface/test_results/transmission_interface/test_simple_transmission_loader.gtest.xml: 6 tests, 0 errors, 0 failures, 0 skipped +build/transmission_interface/test_results/transmission_interface/test_utils.gtest.xml: 1 test, 0 errors, 0 failures, 0 skipped + +Summary: 1368 tests, 0 errors, 2 failures, 335 skipped From 8dfefc97729d6f40361200be891008d7b1514f23 Mon Sep 17 00:00:00 2001 From: baila Date: Tue, 5 Mar 2024 20:03:08 +0530 Subject: [PATCH 09/10] Removed unwanted file. --- o | 285 -------------------------------------------------------------- 1 file changed, 285 deletions(-) delete mode 100644 o diff --git a/o b/o deleted file mode 100644 index 755c65da9b..0000000000 --- a/o +++ /dev/null @@ -1,285 +0,0 @@ -build/controller_interface/Testing/20240305-1418/Test.xml: 6 tests, 0 errors, 0 failures, 0 skipped -build/controller_interface/test_results/controller_interface/test_chainable_controller_interface.gtest.xml: 6 tests, 0 errors, 0 failures, 0 skipped -build/controller_interface/test_results/controller_interface/test_controller_interface.gtest.xml: 5 tests, 0 errors, 0 failures, 0 skipped -build/controller_interface/test_results/controller_interface/test_controller_with_options.gtest.xml: 5 tests, 0 errors, 0 failures, 0 skipped -build/controller_interface/test_results/controller_interface/test_force_torque_sensor.gtest.xml: 3 tests, 0 errors, 0 failures, 0 skipped -build/controller_interface/test_results/controller_interface/test_imu_sensor.gtest.xml: 1 test, 0 errors, 0 failures, 0 skipped -build/controller_interface/test_results/controller_interface/test_semantic_component_interface.gtest.xml: 3 tests, 0 errors, 0 failures, 0 skipped -build/controller_manager/Testing/20240305-1418/Test.xml: 11 tests, 0 errors, 1 failure, 0 skipped -- test_cleanup_controller - <<< failure message - -- run_test.py: extra environment variables to append: - - AMENT_PREFIX_PATH+=_ - -- run_test.py: invoking following command in '/home/baila/stoglrobotics/issue-759/src/ros2_control/build/controller_manager': - - /home/baila/stoglrobotics/issue-759/src/ros2_control/build/controller_manager/test_cleanup_controller --gtest_output=xml:/home/baila/stoglrobotics/issue-759/src/ros2_control/build/controller_manager/test_results/controller_manager/test_cleanup_controller.gtest.xml - Running main() from gmock_main.cc - [==========] Running 6 tests from 1 test suite. - [----------] Global test environment set-up. - [----------] 6 tests from TestCleanupController - [ RUN ] TestCleanupController.cleanup_unknown_controller - [WARN] [1709648352.713765099] [test_controller_manager]: 'update_rate' parameter not set, using default value. - [INFO] [1709648352.716240433] [test_controller_manager]: Subscribing to '/test_controller_manager/robot_description' topic for robot description. - [INFO] [1709648352.716495380] [test_controller_manager]: Received robot description from topic. - [INFO] [1709648352.716955811] [resource_manager]: Loading hardware 'TestActuatorHardware' - [INFO] [1709648352.718008153] [resource_manager]: Initialize hardware 'TestActuatorHardware' - [INFO] [1709648352.718421785] [resource_manager]: Successful initialization of hardware 'TestActuatorHardware' - [INFO] [1709648352.718731404] [resource_manager]: Loading hardware 'TestSensorHardware' - [INFO] [1709648352.718911796] [resource_manager]: Initialize hardware 'TestSensorHardware' - [INFO] [1709648352.718952394] [resource_manager]: Successful initialization of hardware 'TestSensorHardware' - [INFO] [1709648352.718967880] [resource_manager]: Loading hardware 'TestSystemHardware' - [INFO] [1709648352.719086218] [resource_manager]: Initialize hardware 'TestSystemHardware' - [INFO] [1709648352.719107293] [resource_manager]: Successful initialization of hardware 'TestSystemHardware' - [INFO] [1709648352.719301299] [resource_manager]: 'configure' hardware 'TestSystemHardware' - [INFO] [1709648352.719320280] [resource_manager]: Successful 'configure' of hardware 'TestSystemHardware' - [INFO] [1709648352.719332430] [resource_manager]: 'activate' hardware 'TestSystemHardware' - [INFO] [1709648352.719335366] [resource_manager]: Successful 'activate' of hardware 'TestSystemHardware' - [INFO] [1709648352.719363813] [resource_manager]: 'configure' hardware 'TestSensorHardware' - [INFO] [1709648352.719372036] [resource_manager]: Successful 'configure' of hardware 'TestSensorHardware' - [INFO] [1709648352.719375913] [resource_manager]: 'activate' hardware 'TestSensorHardware' - [INFO] [1709648352.719378538] [resource_manager]: Successful 'activate' of hardware 'TestSensorHardware' - [INFO] [1709648352.719397089] [resource_manager]: 'configure' hardware 'TestActuatorHardware' - [INFO] [1709648352.719404121] [resource_manager]: Successful 'configure' of hardware 'TestActuatorHardware' - [INFO] [1709648352.719409089] [resource_manager]: 'activate' hardware 'TestActuatorHardware' - [INFO] [1709648352.719411703] [resource_manager]: Successful 'activate' of hardware 'TestActuatorHardware' - [INFO] [1709648352.741448290] [test_controller_manager]: Cleanup controller 'unknown_controller_name' - [ERROR] [1709648352.741724532] [test_controller_manager]: Could not cleanup controller with name 'unknown_controller_name' because no controller with this name exists - [ OK ] TestCleanupController.cleanup_unknown_controller (413 ms) - [ RUN ] TestCleanupController.cleanup_controller_failed_init - [WARN] [1709648352.779081582] [test_controller_manager]: 'update_rate' parameter not set, using default value. - [INFO] [1709648352.780118854] [test_controller_manager]: Subscribing to '/test_controller_manager/robot_description' topic for robot description. - [INFO] [1709648352.781130483] [test_controller_manager]: Received robot description from topic. - [INFO] [1709648352.781368280] [resource_manager]: Loading hardware 'TestActuatorHardware' - [INFO] [1709648352.782449371] [resource_manager]: Initialize hardware 'TestActuatorHardware' - [INFO] [1709648352.782724752] [resource_manager]: Successful initialization of hardware 'TestActuatorHardware' - [INFO] [1709648352.782838112] [resource_manager]: Loading hardware 'TestSensorHardware' - [INFO] [1709648352.782921031] [resource_manager]: Initialize hardware 'TestSensorHardware' - [INFO] [1709648352.782950409] [resource_manager]: Successful initialization of hardware 'TestSensorHardware' - [INFO] [1709648352.782962080] [resource_manager]: Loading hardware 'TestSystemHardware' - [INFO] [1709648352.783029512] [resource_manager]: Initialize hardware 'TestSystemHardware' - [INFO] [1709648352.783099559] [resource_manager]: Successful initialization of hardware 'TestSystemHardware' - [INFO] [1709648352.783174605] [resource_manager]: 'configure' hardware 'TestSystemHardware' - [INFO] [1709648352.783181397] [resource_manager]: Successful 'configure' of hardware 'TestSystemHardware' - [INFO] [1709648352.783188098] [resource_manager]: 'activate' hardware 'TestSystemHardware' - [INFO] [1709648352.783190602] [resource_manager]: Successful 'activate' of hardware 'TestSystemHardware' - [INFO] [1709648352.783196481] [resource_manager]: 'configure' hardware 'TestSensorHardware' - [INFO] [1709648352.783199257] [resource_manager]: Successful 'configure' of hardware 'TestSensorHardware' - [INFO] [1709648352.783202362] [resource_manager]: 'activate' hardware 'TestSensorHardware' - [INFO] [1709648352.783204666] [resource_manager]: Successful 'activate' of hardware 'TestSensorHardware' - [INFO] [1709648352.783209063] [resource_manager]: 'configure' hardware 'TestActuatorHardware' - [INFO] [1709648352.783211817] [resource_manager]: Successful 'configure' of hardware 'TestActuatorHardware' - [INFO] [1709648352.783215985] [resource_manager]: 'activate' hardware 'TestActuatorHardware' - [INFO] [1709648352.783218258] [resource_manager]: Successful 'activate' of hardware 'TestActuatorHardware' - [INFO] [1709648352.789308626] [test_controller_manager]: Loading controller 'test_controller_failed_init' - [ERROR] [1709648352.798406709] [test_controller_manager]: Could not initialize the controller named 'test_controller_failed_init' - [INFO] [1709648352.803143770] [test_controller_manager]: Cleanup controller 'test_controller_failed_init' - [ERROR] [1709648352.803258853] [test_controller_manager]: Could not cleanup controller with name 'test_controller_failed_init' because no controller with this name exists - [ OK ] TestCleanupController.cleanup_controller_failed_init (58 ms) - [ RUN ] TestCleanupController.cleanup_non_loaded_controller_fails - [WARN] [1709648352.831705234] [test_controller_manager]: 'update_rate' parameter not set, using default value. - [INFO] [1709648352.832424878] [test_controller_manager]: Subscribing to '/test_controller_manager/robot_description' topic for robot description. - [INFO] [1709648352.832546492] [test_controller_manager]: Received robot description from topic. - [INFO] [1709648352.832739695] [resource_manager]: Loading hardware 'TestActuatorHardware' - [INFO] [1709648352.833474846] [resource_manager]: Initialize hardware 'TestActuatorHardware' - [INFO] [1709648352.833753723] [resource_manager]: Successful initialization of hardware 'TestActuatorHardware' - [INFO] [1709648352.834030396] [resource_manager]: Loading hardware 'TestSensorHardware' - [INFO] [1709648352.834137326] [resource_manager]: Initialize hardware 'TestSensorHardware' - [INFO] [1709648352.834257747] [resource_manager]: Successful initialization of hardware 'TestSensorHardware' - [INFO] [1709648352.834276208] [resource_manager]: Loading hardware 'TestSystemHardware' - [INFO] [1709648352.834356172] [resource_manager]: Initialize hardware 'TestSystemHardware' - [INFO] [1709648352.834372349] [resource_manager]: Successful initialization of hardware 'TestSystemHardware' - [INFO] [1709648352.835561150] [resource_manager]: 'configure' hardware 'TestSystemHardware' - [INFO] [1709648352.835573430] [resource_manager]: Successful 'configure' of hardware 'TestSystemHardware' - [INFO] [1709648352.835580893] [resource_manager]: 'activate' hardware 'TestSystemHardware' - [INFO] [1709648352.835584348] [resource_manager]: Successful 'activate' of hardware 'TestSystemHardware' - [INFO] [1709648352.835591590] [resource_manager]: 'configure' hardware 'TestSensorHardware' - [INFO] [1709648352.835594866] [resource_manager]: Successful 'configure' of hardware 'TestSensorHardware' - [INFO] [1709648352.835598161] [resource_manager]: 'activate' hardware 'TestSensorHardware' - [INFO] [1709648352.835600496] [resource_manager]: Successful 'activate' of hardware 'TestSensorHardware' - [INFO] [1709648352.835605133] [resource_manager]: 'configure' hardware 'TestActuatorHardware' - [INFO] [1709648352.835608198] [resource_manager]: Successful 'configure' of hardware 'TestActuatorHardware' - [INFO] [1709648352.835612826] [resource_manager]: 'activate' hardware 'TestActuatorHardware' - [INFO] [1709648352.835615190] [resource_manager]: Successful 'activate' of hardware 'TestActuatorHardware' - [INFO] [1709648352.844268845] [test_controller_manager]: Cleanup controller 'test_controller1' - [ERROR] [1709648352.844355058] [test_controller_manager]: Could not cleanup controller with name 'test_controller1' because no controller with this name exists - [ OK ] TestCleanupController.cleanup_non_loaded_controller_fails (35 ms) - [ RUN ] TestCleanupController.cleanup_unconfigured_controller - [WARN] [1709648352.879524024] [test_controller_manager]: 'update_rate' parameter not set, using default value. - [INFO] [1709648352.880005109] [test_controller_manager]: Subscribing to '/test_controller_manager/robot_description' topic for robot description. - [INFO] [1709648352.880103474] [test_controller_manager]: Received robot description from topic. - [INFO] [1709648352.880227933] [resource_manager]: Loading hardware 'TestActuatorHardware' - [INFO] [1709648352.880996569] [resource_manager]: Initialize hardware 'TestActuatorHardware' - [INFO] [1709648352.881193329] [resource_manager]: Successful initialization of hardware 'TestActuatorHardware' - [INFO] [1709648352.881328605] [resource_manager]: Loading hardware 'TestSensorHardware' - [INFO] [1709648352.882264743] [resource_manager]: Initialize hardware 'TestSensorHardware' - [INFO] [1709648352.882319635] [resource_manager]: Successful initialization of hardware 'TestSensorHardware' - [INFO] [1709648352.882339098] [resource_manager]: Loading hardware 'TestSystemHardware' - [INFO] [1709648352.884077067] [resource_manager]: Initialize hardware 'TestSystemHardware' - [INFO] [1709648352.884096359] [resource_manager]: Successful initialization of hardware 'TestSystemHardware' - [INFO] [1709648352.884194314] [resource_manager]: 'configure' hardware 'TestSystemHardware' - [INFO] [1709648352.884202277] [resource_manager]: Successful 'configure' of hardware 'TestSystemHardware' - [INFO] [1709648352.884208928] [resource_manager]: 'activate' hardware 'TestSystemHardware' - [INFO] [1709648352.884211913] [resource_manager]: Successful 'activate' of hardware 'TestSystemHardware' - [INFO] [1709648352.884218674] [resource_manager]: 'configure' hardware 'TestSensorHardware' - [INFO] [1709648352.884221660] [resource_manager]: Successful 'configure' of hardware 'TestSensorHardware' - [INFO] [1709648352.884224895] [resource_manager]: 'activate' hardware 'TestSensorHardware' - [INFO] [1709648352.884227139] [resource_manager]: Successful 'activate' of hardware 'TestSensorHardware' - [INFO] [1709648352.884232227] [resource_manager]: 'configure' hardware 'TestActuatorHardware' - [INFO] [1709648352.884236414] [resource_manager]: Successful 'configure' of hardware 'TestActuatorHardware' - [INFO] [1709648352.884242204] [resource_manager]: 'activate' hardware 'TestActuatorHardware' - [INFO] [1709648352.884245028] [resource_manager]: Successful 'activate' of hardware 'TestActuatorHardware' - [INFO] [1709648352.892463199] [test_controller_manager]: Loading controller 'test_controller1' - [INFO] [1709648352.901130901] [test_controller_manager]: Cleanup controller 'test_controller1' - [ OK ] TestCleanupController.cleanup_unconfigured_controller (78 ms) - [ RUN ] TestCleanupController.cleanup_inactive_controller - [WARN] [1709648352.946091267] [test_controller_manager]: 'update_rate' parameter not set, using default value. - [INFO] [1709648352.947639820] [test_controller_manager]: Subscribing to '/test_controller_manager/robot_description' topic for robot description. - [INFO] [1709648352.947801780] [test_controller_manager]: Received robot description from topic. - [INFO] [1709648352.947941685] [resource_manager]: Loading hardware 'TestActuatorHardware' - [INFO] [1709648352.948895892] [resource_manager]: Initialize hardware 'TestActuatorHardware' - [INFO] [1709648352.949098662] [resource_manager]: Successful initialization of hardware 'TestActuatorHardware' - [INFO] [1709648352.949287859] [resource_manager]: Loading hardware 'TestSensorHardware' - [INFO] [1709648352.949439523] [resource_manager]: Initialize hardware 'TestSensorHardware' - [INFO] [1709648352.949527420] [resource_manager]: Successful initialization of hardware 'TestSensorHardware' - [INFO] [1709648352.949545771] [resource_manager]: Loading hardware 'TestSystemHardware' - [INFO] [1709648352.949624303] [resource_manager]: Initialize hardware 'TestSystemHardware' - [INFO] [1709648352.949689992] [resource_manager]: Successful initialization of hardware 'TestSystemHardware' - [INFO] [1709648352.949777259] [resource_manager]: 'configure' hardware 'TestSystemHardware' - [INFO] [1709648352.949832451] [resource_manager]: Successful 'configure' of hardware 'TestSystemHardware' - [INFO] [1709648352.949844451] [resource_manager]: 'activate' hardware 'TestSystemHardware' - [INFO] [1709648352.949863583] [resource_manager]: Successful 'activate' of hardware 'TestSystemHardware' - [INFO] [1709648352.949875012] [resource_manager]: 'configure' hardware 'TestSensorHardware' - [INFO] [1709648352.949878357] [resource_manager]: Successful 'configure' of hardware 'TestSensorHardware' - [INFO] [1709648352.949881804] [resource_manager]: 'activate' hardware 'TestSensorHardware' - [INFO] [1709648352.949884027] [resource_manager]: Successful 'activate' of hardware 'TestSensorHardware' - [INFO] [1709648352.949888865] [resource_manager]: 'configure' hardware 'TestActuatorHardware' - [INFO] [1709648352.949891760] [resource_manager]: Successful 'configure' of hardware 'TestActuatorHardware' - [INFO] [1709648352.949896278] [resource_manager]: 'activate' hardware 'TestActuatorHardware' - [INFO] [1709648352.949898562] [resource_manager]: Successful 'activate' of hardware 'TestActuatorHardware' - [INFO] [1709648352.956828178] [test_controller_manager]: Loading controller 'test_controller1' - [INFO] [1709648352.963166611] [test_controller_manager]: Configuring controller 'test_controller1' - [INFO] [1709648352.963721269] [test_controller_manager]: Cleanup controller 'test_controller1' - [ OK ] TestCleanupController.cleanup_inactive_controller (58 ms) - [ RUN ] TestCleanupController.cleanup_active_controller - [WARN] [1709648353.016982088] [test_controller_manager]: 'update_rate' parameter not set, using default value. - [INFO] [1709648353.017724200] [test_controller_manager]: Subscribing to '/test_controller_manager/robot_description' topic for robot description. - [INFO] [1709648353.019986023] [test_controller_manager]: Received robot description from topic. - [INFO] [1709648353.020122611] [resource_manager]: Loading hardware 'TestActuatorHardware' - [INFO] [1709648353.023900267] [resource_manager]: Initialize hardware 'TestActuatorHardware' - [INFO] [1709648353.024091937] [resource_manager]: Successful initialization of hardware 'TestActuatorHardware' - [INFO] [1709648353.024318978] [resource_manager]: Loading hardware 'TestSensorHardware' - [INFO] [1709648353.024456839] [resource_manager]: Initialize hardware 'TestSensorHardware' - [INFO] [1709648353.024494182] [resource_manager]: Successful initialization of hardware 'TestSensorHardware' - [INFO] [1709648353.024507994] [resource_manager]: Loading hardware 'TestSystemHardware' - [INFO] [1709648353.024578282] [resource_manager]: Initialize hardware 'TestSystemHardware' - [INFO] [1709648353.024593628] [resource_manager]: Successful initialization of hardware 'TestSystemHardware' - [INFO] [1709648353.024675194] [resource_manager]: 'configure' hardware 'TestSystemHardware' - [INFO] [1709648353.024684610] [resource_manager]: Successful 'configure' of hardware 'TestSystemHardware' - [INFO] [1709648353.024691732] [resource_manager]: 'activate' hardware 'TestSystemHardware' - [INFO] [1709648353.024694386] [resource_manager]: Successful 'activate' of hardware 'TestSystemHardware' - [INFO] [1709648353.024700817] [resource_manager]: 'configure' hardware 'TestSensorHardware' - [INFO] [1709648353.024703902] [resource_manager]: Successful 'configure' of hardware 'TestSensorHardware' - [INFO] [1709648353.024707198] [resource_manager]: 'activate' hardware 'TestSensorHardware' - [INFO] [1709648353.024709511] [resource_manager]: Successful 'activate' of hardware 'TestSensorHardware' - [INFO] [1709648353.024713969] [resource_manager]: 'configure' hardware 'TestActuatorHardware' - [INFO] [1709648353.024716875] [resource_manager]: Successful 'configure' of hardware 'TestActuatorHardware' - [INFO] [1709648353.024721342] [resource_manager]: 'activate' hardware 'TestActuatorHardware' - [INFO] [1709648353.024723565] [resource_manager]: Successful 'activate' of hardware 'TestActuatorHardware' - [INFO] [1709648353.031697483] [test_controller_manager]: Loading controller 'test_controller1' - [INFO] [1709648353.040695869] [test_controller_manager]: Configuring controller 'test_controller1' - [INFO] [1709648353.172182596] [test_controller_manager]: Cleanup controller 'test_controller1' - [ERROR] [1709648353.172382230] [test_controller_manager]: Controller 'test_controller1' can not be cleaned-up from 'active' state. - /home/baila/stoglrobotics/issue-759/src/ros2_control/controller_manager/test/test_cleanup_controller.cpp:108: Failure - Expected equality of these values: - cm_->cleanup_controller(CONTROLLER_NAME_1) - Which is: 1-byte object <01> - controller_interface::return_type::OK - Which is: 1-byte object <00> - [ FAILED ] TestCleanupController.cleanup_active_controller (213 ms) - [----------] 6 tests from TestCleanupController (856 ms total) - - [----------] Global test environment tear-down - [==========] 6 tests from 1 test suite ran. (863 ms total) - [ PASSED ] 5 tests. - [ FAILED ] 1 test, listed below: - [ FAILED ] TestCleanupController.cleanup_active_controller - - 1 FAILED TEST - -- run_test.py: return code 1 - -- run_test.py: inject classname prefix into gtest result file '/home/baila/stoglrobotics/issue-759/src/ros2_control/build/controller_manager/test_results/controller_manager/test_cleanup_controller.gtest.xml' - -- run_test.py: verify result file '/home/baila/stoglrobotics/issue-759/src/ros2_control/build/controller_manager/test_results/controller_manager/test_cleanup_controller.gtest.xml' - >>> -build/controller_manager/test_results/controller_manager/test_cleanup_controller.gtest.xml: 6 tests, 0 errors, 1 failure, 0 skipped -- controller_manager.TestCleanupController cleanup_active_controller - <<< failure message - /home/baila/stoglrobotics/issue-759/src/ros2_control/controller_manager/test/test_cleanup_controller.cpp:108 - Expected equality of these values: - cm_->cleanup_controller(CONTROLLER_NAME_1) - Which is: 1-byte object <01> - controller_interface::return_type::OK - Which is: 1-byte object <00> - >>> -build/controller_manager/test_results/controller_manager/test_controller_manager.gtest.xml: 19 tests, 0 errors, 0 failures, 0 skipped -build/controller_manager/test_results/controller_manager/test_controller_manager_hardware_error_handling.gtest.xml: 6 tests, 0 errors, 0 failures, 0 skipped -build/controller_manager/test_results/controller_manager/test_controller_manager_srvs.gtest.xml: 15 tests, 0 errors, 0 failures, 0 skipped -build/controller_manager/test_results/controller_manager/test_controller_manager_urdf_passing.gtest.xml: 3 tests, 0 errors, 0 failures, 0 skipped -build/controller_manager/test_results/controller_manager/test_controller_manager_with_namespace.gtest.xml: 2 tests, 0 errors, 0 failures, 0 skipped -build/controller_manager/test_results/controller_manager/test_controllers_chaining_with_controller_manager.gtest.xml: 12 tests, 0 errors, 0 failures, 0 skipped -build/controller_manager/test_results/controller_manager/test_hardware_management_srvs.gtest.xml: 4 tests, 0 errors, 0 failures, 0 skipped -build/controller_manager/test_results/controller_manager/test_load_controller.gtest.xml: 39 tests, 0 errors, 0 failures, 0 skipped -build/controller_manager/test_results/controller_manager/test_release_interfaces.gtest.xml: 1 test, 0 errors, 0 failures, 0 skipped -build/controller_manager/test_results/controller_manager/test_spawner_unspawner.gtest.xml: 7 tests, 0 errors, 0 failures, 0 skipped -build/controller_manager_msgs/Testing/20240305-1418/Test.xml: 29 tests, 0 errors, 0 failures, 0 skipped -build/controller_manager_msgs/test_results/controller_manager_msgs/cppcheck_rosidl_generated_c.xunit.xml: 106 tests, 0 errors, 0 failures, 106 skipped -build/controller_manager_msgs/test_results/controller_manager_msgs/cppcheck_rosidl_generated_cpp.xunit.xml: 60 tests, 0 errors, 0 failures, 60 skipped -build/controller_manager_msgs/test_results/controller_manager_msgs/cppcheck_rosidl_generated_py.xunit.xml: 18 tests, 0 errors, 0 failures, 18 skipped -build/controller_manager_msgs/test_results/controller_manager_msgs/cppcheck_rosidl_typesupport_c.xunit.xml: 15 tests, 0 errors, 0 failures, 15 skipped -build/controller_manager_msgs/test_results/controller_manager_msgs/cppcheck_rosidl_typesupport_cpp.xunit.xml: 15 tests, 0 errors, 0 failures, 15 skipped -build/controller_manager_msgs/test_results/controller_manager_msgs/cppcheck_rosidl_typesupport_fastrtps_c.xunit.xml: 30 tests, 0 errors, 0 failures, 30 skipped -build/controller_manager_msgs/test_results/controller_manager_msgs/cppcheck_rosidl_typesupport_fastrtps_cpp.xunit.xml: 30 tests, 0 errors, 0 failures, 30 skipped -build/controller_manager_msgs/test_results/controller_manager_msgs/cppcheck_rosidl_typesupport_introspection_c.xunit.xml: 31 tests, 0 errors, 0 failures, 31 skipped -build/controller_manager_msgs/test_results/controller_manager_msgs/cppcheck_rosidl_typesupport_introspection_cpp.xunit.xml: 30 tests, 0 errors, 0 failures, 30 skipped -build/controller_manager_msgs/test_results/controller_manager_msgs/cpplint_rosidl_generated_c.xunit.xml: 106 tests, 0 errors, 0 failures, 0 skipped -build/controller_manager_msgs/test_results/controller_manager_msgs/cpplint_rosidl_generated_cpp.xunit.xml: 60 tests, 0 errors, 0 failures, 0 skipped -build/controller_manager_msgs/test_results/controller_manager_msgs/cpplint_rosidl_generated_py.xunit.xml: 18 tests, 0 errors, 0 failures, 0 skipped -build/controller_manager_msgs/test_results/controller_manager_msgs/cpplint_rosidl_typesupport_c.xunit.xml: 15 tests, 0 errors, 0 failures, 0 skipped -build/controller_manager_msgs/test_results/controller_manager_msgs/cpplint_rosidl_typesupport_cpp.xunit.xml: 15 tests, 0 errors, 0 failures, 0 skipped -build/controller_manager_msgs/test_results/controller_manager_msgs/cpplint_rosidl_typesupport_fastrtps_c.xunit.xml: 30 tests, 0 errors, 0 failures, 0 skipped -build/controller_manager_msgs/test_results/controller_manager_msgs/cpplint_rosidl_typesupport_fastrtps_cpp.xunit.xml: 30 tests, 0 errors, 0 failures, 0 skipped -build/controller_manager_msgs/test_results/controller_manager_msgs/cpplint_rosidl_typesupport_introspection_c.xunit.xml: 31 tests, 0 errors, 0 failures, 0 skipped -build/controller_manager_msgs/test_results/controller_manager_msgs/cpplint_rosidl_typesupport_introspection_cpp.xunit.xml: 30 tests, 0 errors, 0 failures, 0 skipped -build/controller_manager_msgs/test_results/controller_manager_msgs/flake8_rosidl_generated_py.xunit.xml: 1 test, 0 errors, 0 failures, 0 skipped -build/controller_manager_msgs/test_results/controller_manager_msgs/pep257_rosidl_generated_py.xunit.xml: 18 tests, 0 errors, 0 failures, 0 skipped -build/controller_manager_msgs/test_results/controller_manager_msgs/uncrustify_rosidl_generated_c.xunit.xml: 106 tests, 0 errors, 0 failures, 0 skipped -build/controller_manager_msgs/test_results/controller_manager_msgs/uncrustify_rosidl_generated_cpp.xunit.xml: 60 tests, 0 errors, 0 failures, 0 skipped -build/controller_manager_msgs/test_results/controller_manager_msgs/uncrustify_rosidl_generated_py.xunit.xml: 18 tests, 0 errors, 0 failures, 0 skipped -build/controller_manager_msgs/test_results/controller_manager_msgs/uncrustify_rosidl_typesupport_c.xunit.xml: 15 tests, 0 errors, 0 failures, 0 skipped -build/controller_manager_msgs/test_results/controller_manager_msgs/uncrustify_rosidl_typesupport_cpp.xunit.xml: 15 tests, 0 errors, 0 failures, 0 skipped -build/controller_manager_msgs/test_results/controller_manager_msgs/uncrustify_rosidl_typesupport_fastrtps_c.xunit.xml: 30 tests, 0 errors, 0 failures, 0 skipped -build/controller_manager_msgs/test_results/controller_manager_msgs/uncrustify_rosidl_typesupport_fastrtps_cpp.xunit.xml: 30 tests, 0 errors, 0 failures, 0 skipped -build/controller_manager_msgs/test_results/controller_manager_msgs/uncrustify_rosidl_typesupport_introspection_c.xunit.xml: 31 tests, 0 errors, 0 failures, 0 skipped -build/controller_manager_msgs/test_results/controller_manager_msgs/uncrustify_rosidl_typesupport_introspection_cpp.xunit.xml: 30 tests, 0 errors, 0 failures, 0 skipped -build/hardware_interface/Testing/20240305-1418/Test.xml: 6 tests, 0 errors, 0 failures, 0 skipped -build/hardware_interface/test_results/hardware_interface/test_component_interfaces.gtest.xml: 9 tests, 0 errors, 0 failures, 0 skipped -build/hardware_interface/test_results/hardware_interface/test_component_parser.gtest.xml: 24 tests, 0 errors, 0 failures, 0 skipped -build/hardware_interface/test_results/hardware_interface/test_generic_system.gtest.xml: 20 tests, 0 errors, 0 failures, 0 skipped -build/hardware_interface/test_results/hardware_interface/test_inst_hardwares.gtest.xml: 3 tests, 0 errors, 0 failures, 0 skipped -build/hardware_interface/test_results/hardware_interface/test_joint_handle.gtest.xml: 5 tests, 0 errors, 0 failures, 0 skipped -build/hardware_interface/test_results/hardware_interface/test_macros.gtest.xml: 2 tests, 0 errors, 0 failures, 0 skipped -build/hardware_interface_testing/Testing/20240305-1418/Test.xml: 2 tests, 0 errors, 0 failures, 0 skipped -build/hardware_interface_testing/test_results/hardware_interface_testing/test_resource_manager.gtest.xml: 25 tests, 0 errors, 0 failures, 0 skipped -build/hardware_interface_testing/test_results/hardware_interface_testing/test_resource_manager_prepare_perform_switch.gtest.xml: 5 tests, 0 errors, 0 failures, 0 skipped -build/joint_limits/Testing/20240305-1418/Test.xml: 2 tests, 0 errors, 0 failures, 0 skipped -build/joint_limits/test_results/joint_limits/joint_limits_urdf_test.gtest.xml: 2 tests, 0 errors, 0 failures, 0 skipped -build/joint_limits/test_results/joint_limits/test_joint_limits_rosparam.launch.py.xunit.xml: 2 tests, 0 errors, 0 failures, 0 skipped -build/ros2controlcli/pytest.xml: 1 test, 0 errors, 0 failures, 0 skipped -build/rqt_controller_manager/pytest.xml: 0 tests, 0 errors, 0 failures, 0 skipped -build/transmission_interface/Testing/20240305-1418/Test.xml: 7 tests, 0 errors, 0 failures, 0 skipped -build/transmission_interface/test_results/transmission_interface/test_differential_transmission.gtest.xml: 8 tests, 0 errors, 0 failures, 0 skipped -build/transmission_interface/test_results/transmission_interface/test_differential_transmission_loader.gtest.xml: 6 tests, 0 errors, 0 failures, 0 skipped -build/transmission_interface/test_results/transmission_interface/test_four_bar_linkage_transmission.gtest.xml: 8 tests, 0 errors, 0 failures, 0 skipped -build/transmission_interface/test_results/transmission_interface/test_four_bar_linkage_transmission_loader.gtest.xml: 6 tests, 0 errors, 0 failures, 0 skipped -build/transmission_interface/test_results/transmission_interface/test_simple_transmission.gtest.xml: 11 tests, 0 errors, 0 failures, 0 skipped -build/transmission_interface/test_results/transmission_interface/test_simple_transmission_loader.gtest.xml: 6 tests, 0 errors, 0 failures, 0 skipped -build/transmission_interface/test_results/transmission_interface/test_utils.gtest.xml: 1 test, 0 errors, 0 failures, 0 skipped - -Summary: 1368 tests, 0 errors, 2 failures, 335 skipped From 418e47ae0f002f0da73a17eff770b4ede046d17c Mon Sep 17 00:00:00 2001 From: baila Date: Tue, 5 Mar 2024 21:28:42 +0530 Subject: [PATCH 10/10] Correcting clang format and test case --- .../test/test_cleanup_controller.cpp | 26 ++++++++++++------- 1 file changed, 16 insertions(+), 10 deletions(-) diff --git a/controller_manager/test/test_cleanup_controller.cpp b/controller_manager/test/test_cleanup_controller.cpp index d2f52a062b..bed152ac39 100644 --- a/controller_manager/test/test_cleanup_controller.cpp +++ b/controller_manager/test/test_cleanup_controller.cpp @@ -37,17 +37,19 @@ class TestCleanupController : public ControllerManagerFixturecleanup_controller("unknown_controller_name"), controller_interface::return_type::ERROR); + 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); + 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); + ASSERT_EQ( + cm_->cleanup_controller("test_controller_failed_init"), + controller_interface::return_type::ERROR); } TEST_F(TestCleanupController, cleanup_non_loaded_controller_fails) @@ -65,7 +67,8 @@ TEST_F(TestCleanupController, cleanup_unconfigured_controller) ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if->get_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_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if->get_state().id()); } TEST_F(TestCleanupController, cleanup_inactive_controller) @@ -80,7 +83,8 @@ TEST_F(TestCleanupController, cleanup_inactive_controller) EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_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_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if->get_state().id()); } TEST_F(TestCleanupController, cleanup_active_controller) @@ -97,6 +101,8 @@ TEST_F(TestCleanupController, cleanup_active_controller) switch_test_controllers(strvec{CONTROLLER_NAME_1}, strvec{}, STRICT); - 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_state().id()); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if->get_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_state().id()); }