From 8cadf7ca07658f1220408b04e8fa094dd6cc9fac Mon Sep 17 00:00:00 2001 From: Wiktor Bajor Date: Sat, 11 Jan 2025 19:19:39 +0100 Subject: [PATCH 1/4] Call shutdown instead of cleanup during controller unloading --- .../controller_manager/controller_manager.hpp | 7 +++ controller_manager/src/controller_manager.cpp | 51 ++++++++++--------- 2 files changed, 34 insertions(+), 24 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 7acbe20166..cb9c245076 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -317,6 +317,13 @@ class ControllerManager : public rclcpp::Node void initialize_parameters(); + /** + * Call shutdown and move given controller to the finalized state. + * + * \param[in] controller controller to be shutdown. + */ + void shutdown_controller(controller_manager::ControllerSpec & controller) const; + /** * Clear request lists used when switching controllers. The lists are shared between "callback" * and "control loop" threads. diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index fd59c89ea8..63cd92ed98 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -695,7 +695,7 @@ controller_interface::return_type ControllerManager::unload_controller( return controller_interface::return_type::ERROR; } - RCLCPP_DEBUG(get_logger(), "Cleanup controller"); + RCLCPP_DEBUG(get_logger(), "Shutdown controller"); controller_chain_spec_cleanup(controller_chain_spec_, controller_name); // TODO(destogl): remove reference interface if chainable; i.e., add a separate method for // cleaning-up controllers? @@ -705,29 +705,7 @@ controller_interface::return_type ControllerManager::unload_controller( get_logger(), "Controller '%s' is cleaned-up before unloading!", controller_name.c_str()); // TODO(destogl): remove reference interface if chainable; i.e., add a separate method for // cleaning-up controllers? - try - { - const auto new_state = controller.c->get_node()->cleanup(); - if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) - { - RCLCPP_WARN( - get_logger(), "Failed to clean-up the controller '%s' before unloading!", - controller_name.c_str()); - } - } - catch (const std::exception & e) - { - RCLCPP_ERROR( - get_logger(), - "Caught exception of type : %s while cleaning up the controller '%s' before unloading: %s", - typeid(e).name(), controller_name.c_str(), e.what()); - } - catch (...) - { - RCLCPP_ERROR( - get_logger(), "Failed to clean-up the controller '%s' before unloading", - controller_name.c_str()); - } + shutdown_controller(controller); } executor_->remove_node(controller.c->get_node()->get_node_base_interface()); to.erase(found_it); @@ -745,6 +723,31 @@ controller_interface::return_type ControllerManager::unload_controller( return controller_interface::return_type::OK; } +void ControllerManager::shutdown_controller(controller_manager::ControllerSpec & controller) const +try +{ + const auto new_state = controller.c->get_node()->shutdown(); + if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) + { + RCLCPP_WARN( + get_logger(), "Failed to shut-down the controller '%s' before unloading!", + controller.info.name.c_str()); + } +} +catch (const std::exception & e) +{ + RCLCPP_ERROR( + get_logger(), + "Caught exception of type : %s while shut down the controller '%s' before unloading: %s", + typeid(e).name(), controller.info.name.c_str(), e.what()); +} +catch (...) +{ + RCLCPP_ERROR( + get_logger(), "Failed to shut-down the controller '%s' before unloading", + controller.info.name.c_str()); +} + std::vector ControllerManager::get_loaded_controllers() const { std::lock_guard guard(rt_controllers_wrapper_.controllers_lock_); From d906915bd25d2f3d5883bf627b1c57b79cbbad32 Mon Sep 17 00:00:00 2001 From: Wiktor Bajor Date: Sat, 11 Jan 2025 22:31:35 +0100 Subject: [PATCH 2/4] Align tests --- controller_manager/src/controller_manager.cpp | 2 +- .../test/test_controller/test_controller.cpp | 9 +++++++++ .../test/test_controller/test_controller.hpp | 3 +++ .../test/test_controller_manager.cpp | 4 ++-- .../test/test_controller_manager_srvs.cpp | 14 +++++++------- .../test_controller_with_interfaces.cpp | 6 ++++++ .../test_controller_with_interfaces.hpp | 3 +++ 7 files changed, 31 insertions(+), 10 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 63cd92ed98..89cbadc22f 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -702,7 +702,7 @@ controller_interface::return_type ControllerManager::unload_controller( if (is_controller_inactive(*controller.c)) { RCLCPP_DEBUG( - get_logger(), "Controller '%s' is cleaned-up before unloading!", controller_name.c_str()); + get_logger(), "Controller '%s' is shut-down before unloading!", controller_name.c_str()); // TODO(destogl): remove reference interface if chainable; i.e., add a separate method for // cleaning-up controllers? shutdown_controller(controller); diff --git a/controller_manager/test/test_controller/test_controller.cpp b/controller_manager/test/test_controller/test_controller.cpp index ee8377f2a3..d3663467d5 100644 --- a/controller_manager/test/test_controller/test_controller.cpp +++ b/controller_manager/test/test_controller/test_controller.cpp @@ -160,6 +160,15 @@ CallbackReturn TestController::on_cleanup(const rclcpp_lifecycle::State & /*prev return CallbackReturn::SUCCESS; } +CallbackReturn TestController::on_shutdown(const rclcpp_lifecycle::State &) +{ + if (should_down_calls) + { + (*should_down_calls)++; + } + return CallbackReturn::SUCCESS; +} + void TestController::set_command_interface_configuration( const controller_interface::InterfaceConfiguration & cfg) { diff --git a/controller_manager/test/test_controller/test_controller.hpp b/controller_manager/test/test_controller/test_controller.hpp index 0728be877b..a90478eda0 100644 --- a/controller_manager/test/test_controller/test_controller.hpp +++ b/controller_manager/test/test_controller/test_controller.hpp @@ -54,6 +54,8 @@ class TestController : public controller_interface::ControllerInterface CallbackReturn on_cleanup(const rclcpp_lifecycle::State & previous_state) override; + CallbackReturn on_shutdown(const rclcpp_lifecycle::State & previous_state) override; + void set_command_interface_configuration( const controller_interface::InterfaceConfiguration & cfg); @@ -69,6 +71,7 @@ class TestController : public controller_interface::ControllerInterface // Variable where we store when cleanup was called, pointer because the controller // is usually destroyed after cleanup size_t * cleanup_calls = nullptr; + size_t * should_down_calls = nullptr; controller_interface::InterfaceConfiguration cmd_iface_cfg_; controller_interface::InterfaceConfiguration state_iface_cfg_; diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index 49c02d28ac..4e7065b094 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -228,7 +228,7 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle) EXPECT_EQ(controller_interface::return_type::OK, unload_future.get()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, test_controller->get_lifecycle_state().id()); EXPECT_EQ(1, test_controller.use_count()); } @@ -429,7 +429,7 @@ TEST_P(TestControllerManagerWithStrictness, async_controller_lifecycle) EXPECT_EQ(controller_interface::return_type::OK, unload_future.get()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, test_controller->get_lifecycle_state().id()); EXPECT_EQ(1, test_controller.use_count()); } diff --git a/controller_manager/test/test_controller_manager_srvs.cpp b/controller_manager/test/test_controller_manager_srvs.cpp index 26a5299a07..f7ef521556 100644 --- a/controller_manager/test/test_controller_manager_srvs.cpp +++ b/controller_manager/test/test_controller_manager_srvs.cpp @@ -395,14 +395,14 @@ TEST_F(TestControllerManagerSrvs, reload_controller_libraries_srv) ASSERT_GT(test_controller.use_count(), 1) << "Controller manager should have have a copy of this shared ptr"; - cleanup_calls = 0; - test_controller->cleanup_calls = &cleanup_calls; + size_t should_down_calls = 0; + test_controller->should_down_calls = &should_down_calls; test_controller.reset(); // destroy our copy of the controller request->force_kill = false; result = call_service_and_wait(*client, request, srv_executor, true); ASSERT_TRUE(result->ok); - ASSERT_EQ(cleanup_calls, 1u); + ASSERT_EQ(should_down_calls, 1u); ASSERT_EQ(test_controller.use_count(), 0) << "No more references to the controller after reloading."; test_controller.reset(); @@ -428,8 +428,8 @@ TEST_F(TestControllerManagerSrvs, reload_controller_libraries_srv) << "Controller manager should still have have a copy of " "this shared ptr, no unloading was performed"; - cleanup_calls = 0; - test_controller->cleanup_calls = &cleanup_calls; + should_down_calls = 0; + test_controller->should_down_calls = &should_down_calls; test_controller.reset(); // destroy our copy of the controller // Force stop active controller @@ -439,7 +439,7 @@ TEST_F(TestControllerManagerSrvs, reload_controller_libraries_srv) ASSERT_EQ(test_controller_weak.use_count(), 0) << "No more references to the controller after reloading."; - ASSERT_EQ(cleanup_calls, 1u) + ASSERT_EQ(should_down_calls, 1u) << "Controller should have been stopped and cleaned up with force_kill = true"; } @@ -578,7 +578,7 @@ TEST_F(TestControllerManagerSrvs, configure_controller_srv) unload_request->name = test_controller::TEST_CONTROLLER_NAME; ASSERT_TRUE(call_service_and_wait(*unload_client, unload_request, srv_executor, true)->ok); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, test_controller->get_lifecycle_state().id()); EXPECT_EQ(0u, cm_->get_loaded_controllers().size()); } diff --git a/controller_manager/test/test_controller_with_interfaces/test_controller_with_interfaces.cpp b/controller_manager/test/test_controller_with_interfaces/test_controller_with_interfaces.cpp index d5695e41e5..efe903be6f 100644 --- a/controller_manager/test/test_controller_with_interfaces/test_controller_with_interfaces.cpp +++ b/controller_manager/test/test_controller_with_interfaces/test_controller_with_interfaces.cpp @@ -45,6 +45,12 @@ TestControllerWithInterfaces::on_cleanup(const rclcpp_lifecycle::State & /*previ return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +TestControllerWithInterfaces::on_shutdown(const rclcpp_lifecycle::State & /*previous_state*/) +{ + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + } // namespace test_controller_with_interfaces #include "pluginlib/class_list_macros.hpp" diff --git a/controller_manager/test/test_controller_with_interfaces/test_controller_with_interfaces.hpp b/controller_manager/test/test_controller_with_interfaces/test_controller_with_interfaces.hpp index 751cb4f3a8..8227b62cb2 100644 --- a/controller_manager/test/test_controller_with_interfaces/test_controller_with_interfaces.hpp +++ b/controller_manager/test/test_controller_with_interfaces/test_controller_with_interfaces.hpp @@ -54,6 +54,9 @@ class TestControllerWithInterfaces : public controller_interface::ControllerInte rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_cleanup( const rclcpp_lifecycle::State & previous_state) override; + + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_shutdown( + const rclcpp_lifecycle::State & previous_state) override; }; } // namespace test_controller_with_interfaces From 51d065950616e3366b7e2bc2cfb32ab6a9a8ec31 Mon Sep 17 00:00:00 2001 From: Wiktor Bajor Date: Sun, 12 Jan 2025 12:17:24 +0100 Subject: [PATCH 3/4] Fixes after review --- .../controller_manager/controller_manager.hpp | 2 +- controller_manager/src/controller_manager.cpp | 8 ++++---- .../test/test_controller/test_controller.cpp | 4 ++-- .../test/test_controller/test_controller.hpp | 6 +++--- .../test/test_controller_manager_srvs.cpp | 12 ++++++------ 5 files changed, 16 insertions(+), 16 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index cb9c245076..ba4bd2acaa 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -318,7 +318,7 @@ class ControllerManager : public rclcpp::Node void initialize_parameters(); /** - * Call shutdown and move given controller to the finalized state. + * Call shutdown to change the given controller lifecycle node to the finalized state. * * \param[in] controller controller to be shutdown. */ diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 89cbadc22f..59be195cfa 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -702,7 +702,7 @@ controller_interface::return_type ControllerManager::unload_controller( if (is_controller_inactive(*controller.c)) { RCLCPP_DEBUG( - get_logger(), "Controller '%s' is shut-down before unloading!", controller_name.c_str()); + get_logger(), "Controller '%s' is shutdown before unloading!", controller_name.c_str()); // TODO(destogl): remove reference interface if chainable; i.e., add a separate method for // cleaning-up controllers? shutdown_controller(controller); @@ -730,7 +730,7 @@ try if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) { RCLCPP_WARN( - get_logger(), "Failed to shut-down the controller '%s' before unloading!", + get_logger(), "Failed to shutdown the controller '%s' before unloading!", controller.info.name.c_str()); } } @@ -738,13 +738,13 @@ catch (const std::exception & e) { RCLCPP_ERROR( get_logger(), - "Caught exception of type : %s while shut down the controller '%s' before unloading: %s", + "Caught exception of type : %s while shutdown the controller '%s' before unloading: %s", typeid(e).name(), controller.info.name.c_str(), e.what()); } catch (...) { RCLCPP_ERROR( - get_logger(), "Failed to shut-down the controller '%s' before unloading", + get_logger(), "Failed to shutdown the controller '%s' before unloading", controller.info.name.c_str()); } diff --git a/controller_manager/test/test_controller/test_controller.cpp b/controller_manager/test/test_controller/test_controller.cpp index d3663467d5..306799a66e 100644 --- a/controller_manager/test/test_controller/test_controller.cpp +++ b/controller_manager/test/test_controller/test_controller.cpp @@ -162,9 +162,9 @@ CallbackReturn TestController::on_cleanup(const rclcpp_lifecycle::State & /*prev CallbackReturn TestController::on_shutdown(const rclcpp_lifecycle::State &) { - if (should_down_calls) + if (shutdown_calls) { - (*should_down_calls)++; + (*shutdown_calls)++; } return CallbackReturn::SUCCESS; } diff --git a/controller_manager/test/test_controller/test_controller.hpp b/controller_manager/test/test_controller/test_controller.hpp index a90478eda0..a28dfa420e 100644 --- a/controller_manager/test/test_controller/test_controller.hpp +++ b/controller_manager/test/test_controller/test_controller.hpp @@ -68,10 +68,10 @@ class TestController : public controller_interface::ControllerInterface rclcpp::Service::SharedPtr service_; unsigned int internal_counter = 0; bool simulate_cleanup_failure = false; - // Variable where we store when cleanup was called, pointer because the controller - // is usually destroyed after cleanup + // Variable where we store when shutdown was called, pointer because the controller + // is usually destroyed after shutdown size_t * cleanup_calls = nullptr; - size_t * should_down_calls = nullptr; + size_t * shutdown_calls = nullptr; controller_interface::InterfaceConfiguration cmd_iface_cfg_; controller_interface::InterfaceConfiguration state_iface_cfg_; diff --git a/controller_manager/test/test_controller_manager_srvs.cpp b/controller_manager/test/test_controller_manager_srvs.cpp index f7ef521556..4a7a704fac 100644 --- a/controller_manager/test/test_controller_manager_srvs.cpp +++ b/controller_manager/test/test_controller_manager_srvs.cpp @@ -395,14 +395,14 @@ TEST_F(TestControllerManagerSrvs, reload_controller_libraries_srv) ASSERT_GT(test_controller.use_count(), 1) << "Controller manager should have have a copy of this shared ptr"; - size_t should_down_calls = 0; - test_controller->should_down_calls = &should_down_calls; + size_t shutdown_calls = 0; + test_controller->shutdown_calls = &shutdown_calls; test_controller.reset(); // destroy our copy of the controller request->force_kill = false; result = call_service_and_wait(*client, request, srv_executor, true); ASSERT_TRUE(result->ok); - ASSERT_EQ(should_down_calls, 1u); + ASSERT_EQ(shutdown_calls, 1u); ASSERT_EQ(test_controller.use_count(), 0) << "No more references to the controller after reloading."; test_controller.reset(); @@ -428,8 +428,8 @@ TEST_F(TestControllerManagerSrvs, reload_controller_libraries_srv) << "Controller manager should still have have a copy of " "this shared ptr, no unloading was performed"; - should_down_calls = 0; - test_controller->should_down_calls = &should_down_calls; + shutdown_calls = 0; + test_controller->shutdown_calls = &shutdown_calls; test_controller.reset(); // destroy our copy of the controller // Force stop active controller @@ -439,7 +439,7 @@ TEST_F(TestControllerManagerSrvs, reload_controller_libraries_srv) ASSERT_EQ(test_controller_weak.use_count(), 0) << "No more references to the controller after reloading."; - ASSERT_EQ(should_down_calls, 1u) + ASSERT_EQ(shutdown_calls, 1u) << "Controller should have been stopped and cleaned up with force_kill = true"; } From 6366451b9f9a59e3a1dd84db1c384eb7f2b63e62 Mon Sep 17 00:00:00 2001 From: Wiktor Bajor Date: Sun, 12 Jan 2025 18:44:47 +0100 Subject: [PATCH 4/4] Enable unloading controller in unconfigured state. --- controller_manager/src/controller_manager.cpp | 47 +++++++++++-------- .../test/test_controller_manager_srvs.cpp | 4 +- 2 files changed, 30 insertions(+), 21 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 59be195cfa..5691188fa3 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -58,6 +58,13 @@ static const rmw_qos_profile_t qos_services = { false}; #endif +inline bool is_controller_unconfigured( + const controller_interface::ControllerInterfaceBase & controller) +{ + return controller.get_lifecycle_state().id() == + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED; +} + inline bool is_controller_inactive(const controller_interface::ControllerInterfaceBase & controller) { return controller.get_lifecycle_state().id() == @@ -699,7 +706,7 @@ controller_interface::return_type ControllerManager::unload_controller( controller_chain_spec_cleanup(controller_chain_spec_, controller_name); // TODO(destogl): remove reference interface if chainable; i.e., add a separate method for // cleaning-up controllers? - if (is_controller_inactive(*controller.c)) + if (is_controller_inactive(*controller.c) || is_controller_unconfigured(*controller.c)) { RCLCPP_DEBUG( get_logger(), "Controller '%s' is shutdown before unloading!", controller_name.c_str()); @@ -724,29 +731,31 @@ controller_interface::return_type ControllerManager::unload_controller( } void ControllerManager::shutdown_controller(controller_manager::ControllerSpec & controller) const -try { - const auto new_state = controller.c->get_node()->shutdown(); - if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) + try { - RCLCPP_WARN( - get_logger(), "Failed to shutdown the controller '%s' before unloading!", + const auto new_state = controller.c->get_node()->shutdown(); + if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) + { + RCLCPP_WARN( + get_logger(), "Failed to shutdown the controller '%s' before unloading!", + controller.info.name.c_str()); + } + } + catch (const std::exception & e) + { + RCLCPP_ERROR( + get_logger(), + "Caught exception of type : %s while shutdown the controller '%s' before unloading: %s", + typeid(e).name(), controller.info.name.c_str(), e.what()); + } + catch (...) + { + RCLCPP_ERROR( + get_logger(), "Failed to shutdown the controller '%s' before unloading", controller.info.name.c_str()); } } -catch (const std::exception & e) -{ - RCLCPP_ERROR( - get_logger(), - "Caught exception of type : %s while shutdown the controller '%s' before unloading: %s", - typeid(e).name(), controller.info.name.c_str(), e.what()); -} -catch (...) -{ - RCLCPP_ERROR( - get_logger(), "Failed to shutdown the controller '%s' before unloading", - controller.info.name.c_str()); -} std::vector ControllerManager::get_loaded_controllers() const { diff --git a/controller_manager/test/test_controller_manager_srvs.cpp b/controller_manager/test/test_controller_manager_srvs.cpp index 4a7a704fac..3a1ea5877b 100644 --- a/controller_manager/test/test_controller_manager_srvs.cpp +++ b/controller_manager/test/test_controller_manager_srvs.cpp @@ -491,7 +491,7 @@ TEST_F(TestControllerManagerSrvs, unload_controller_srv) result = call_service_and_wait(*client, request, srv_executor, true); ASSERT_TRUE(result->ok); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, test_controller->get_lifecycle_state().id()); EXPECT_EQ(0u, cm_->get_loaded_controllers().size()); } @@ -526,7 +526,7 @@ TEST_F(TestControllerManagerSrvs, robot_description_on_load_and_unload_controlle unload_request->name = test_controller::TEST_CONTROLLER_NAME; auto result = call_service_and_wait(*unload_client, unload_request, srv_executor, true); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, test_controller->get_lifecycle_state().id()); EXPECT_EQ(0u, cm_->get_loaded_controllers().size());