From 6f57faf712c02956263b7c67282a5bdee01fc6d4 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 9 Feb 2024 11:25:06 +0100 Subject: [PATCH] check for state of the controller node before cleanup (#1363) --- controller_manager/src/controller_manager.cpp | 15 ++++++++++++++- .../test/test_controller_manager_srvs.cpp | 16 ++++++++++++++++ 2 files changed, 30 insertions(+), 1 deletion(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 4becabb0a4..1911bd6ff8 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -664,7 +664,20 @@ controller_interface::return_type ControllerManager::unload_controller( 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(); + if (is_controller_inactive(*controller.c)) + { + RCLCPP_DEBUG( + 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? + 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()); + } + } executor_->remove_node(controller.c->get_node()->get_node_base_interface()); to.erase(found_it); diff --git a/controller_manager/test/test_controller_manager_srvs.cpp b/controller_manager/test/test_controller_manager_srvs.cpp index 2cd5645bd8..cdec4806c1 100644 --- a/controller_manager/test/test_controller_manager_srvs.cpp +++ b/controller_manager/test/test_controller_manager_srvs.cpp @@ -456,6 +456,8 @@ 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, test_controller->get_state().id()); EXPECT_EQ(0u, cm_->get_loaded_controllers().size()); } @@ -488,6 +490,8 @@ TEST_F(TestControllerManagerSrvs, robot_description_on_load_and_unload_controlle auto unload_request = std::make_shared(); 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, test_controller->get_state().id()); EXPECT_EQ(0u, cm_->get_loaded_controllers().size()); // now load it and check if it got the new robot description @@ -508,6 +512,9 @@ TEST_F(TestControllerManagerSrvs, configure_controller_srv) rclcpp::Client::SharedPtr client = srv_node->create_client( "test_controller_manager/configure_controller"); + rclcpp::Client::SharedPtr unload_client = + srv_node->create_client( + "test_controller_manager/unload_controller"); auto request = std::make_shared(); request->name = test_controller::TEST_CONTROLLER_NAME; @@ -526,6 +533,15 @@ TEST_F(TestControllerManagerSrvs, configure_controller_srv) EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, cm_->get_loaded_controllers()[0].c->get_state().id()); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_state().id()); + + // now unload the controller and check the state + auto unload_request = std::make_shared(); + 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, test_controller->get_state().id()); + EXPECT_EQ(0u, cm_->get_loaded_controllers().size()); } TEST_F(TestControllerManagerSrvs, list_sorted_chained_controllers)