From e23d1e039ca7dec35e8fc99a2fe2ae252711e4ae Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 9 Feb 2024 11:25:06 +0100 Subject: [PATCH 1/3] check for state of the controller node before cleanup (#1363) (cherry picked from commit 6f57faf712c02956263b7c67282a5bdee01fc6d4) # Conflicts: # controller_manager/test/test_controller_manager_srvs.cpp --- controller_manager/src/controller_manager.cpp | 15 ++++- .../test/test_controller_manager_srvs.cpp | 60 +++++++++++++++++++ 2 files changed, 74 insertions(+), 1 deletion(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index d416755135..2db5e1ab79 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -667,7 +667,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 1c13972ae0..1995dfd7d6 100644 --- a/controller_manager/test/test_controller_manager_srvs.cpp +++ b/controller_manager/test/test_controller_manager_srvs.cpp @@ -462,9 +462,57 @@ 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()); +} + +<<<<<<< HEAD +======= +TEST_F(TestControllerManagerSrvs, robot_description_on_load_and_unload_controller) +{ + rclcpp::executors::SingleThreadedExecutor srv_executor; + rclcpp::Node::SharedPtr srv_node = std::make_shared("srv_client"); + srv_executor.add_node(srv_node); + rclcpp::Client::SharedPtr unload_client = + srv_node->create_client( + "test_controller_manager/unload_controller"); + + 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()); + + // check the robot description + ASSERT_EQ(ros2_control_test_assets::minimal_robot_urdf, test_controller->get_robot_description()); + + // Now change the robot description and then see that the controller maintains the old URDF until + // it is unloaded and loaded again + auto msg = std_msgs::msg::String(); + msg.data = ros2_control_test_assets::minimal_robot_missing_state_keys_urdf; + cm_->robot_description_callback(msg); + ASSERT_EQ(ros2_control_test_assets::minimal_robot_urdf, test_controller->get_robot_description()); + + // now unload and load the controller and see if the controller gets the new robot description + 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 + cm_->add_controller( + test_controller, test_controller::TEST_CONTROLLER_NAME, + test_controller::TEST_CONTROLLER_CLASS_NAME); + EXPECT_EQ(1u, cm_->get_loaded_controllers().size()); + ASSERT_EQ( + ros2_control_test_assets::minimal_robot_missing_state_keys_urdf, + test_controller->get_robot_description()); } +>>>>>>> 6f57faf (check for state of the controller node before cleanup (#1363)) TEST_F(TestControllerManagerSrvs, configure_controller_srv) { rclcpp::executors::SingleThreadedExecutor srv_executor; @@ -473,6 +521,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; @@ -491,6 +542,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) From 30f2248f41a44c2f2951d7c53a5f63b91e322708 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 27 Mar 2024 18:04:51 +0100 Subject: [PATCH 2/3] Fix merge conflicts Co-authored-by: Sai Kishor Kothakota --- controller_manager/test/test_controller_manager_srvs.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/controller_manager/test/test_controller_manager_srvs.cpp b/controller_manager/test/test_controller_manager_srvs.cpp index 1995dfd7d6..b98e280996 100644 --- a/controller_manager/test/test_controller_manager_srvs.cpp +++ b/controller_manager/test/test_controller_manager_srvs.cpp @@ -467,8 +467,6 @@ TEST_F(TestControllerManagerSrvs, unload_controller_srv) EXPECT_EQ(0u, cm_->get_loaded_controllers().size()); } -<<<<<<< HEAD -======= TEST_F(TestControllerManagerSrvs, robot_description_on_load_and_unload_controller) { rclcpp::executors::SingleThreadedExecutor srv_executor; @@ -512,7 +510,6 @@ TEST_F(TestControllerManagerSrvs, robot_description_on_load_and_unload_controlle test_controller->get_robot_description()); } ->>>>>>> 6f57faf (check for state of the controller node before cleanup (#1363)) TEST_F(TestControllerManagerSrvs, configure_controller_srv) { rclcpp::executors::SingleThreadedExecutor srv_executor; From 90b076528b6e62afffeef8712a1c3fb32e321371 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Thu, 28 Mar 2024 10:19:57 +0100 Subject: [PATCH 3/3] Remove test applicable only to rolling Co-authored-by: Sai Kishor Kothakota --- .../test/test_controller_manager_srvs.cpp | 43 ------------------- 1 file changed, 43 deletions(-) diff --git a/controller_manager/test/test_controller_manager_srvs.cpp b/controller_manager/test/test_controller_manager_srvs.cpp index b98e280996..58c6920648 100644 --- a/controller_manager/test/test_controller_manager_srvs.cpp +++ b/controller_manager/test/test_controller_manager_srvs.cpp @@ -467,49 +467,6 @@ TEST_F(TestControllerManagerSrvs, unload_controller_srv) EXPECT_EQ(0u, cm_->get_loaded_controllers().size()); } -TEST_F(TestControllerManagerSrvs, robot_description_on_load_and_unload_controller) -{ - rclcpp::executors::SingleThreadedExecutor srv_executor; - rclcpp::Node::SharedPtr srv_node = std::make_shared("srv_client"); - srv_executor.add_node(srv_node); - rclcpp::Client::SharedPtr unload_client = - srv_node->create_client( - "test_controller_manager/unload_controller"); - - 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()); - - // check the robot description - ASSERT_EQ(ros2_control_test_assets::minimal_robot_urdf, test_controller->get_robot_description()); - - // Now change the robot description and then see that the controller maintains the old URDF until - // it is unloaded and loaded again - auto msg = std_msgs::msg::String(); - msg.data = ros2_control_test_assets::minimal_robot_missing_state_keys_urdf; - cm_->robot_description_callback(msg); - ASSERT_EQ(ros2_control_test_assets::minimal_robot_urdf, test_controller->get_robot_description()); - - // now unload and load the controller and see if the controller gets the new robot description - 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 - cm_->add_controller( - test_controller, test_controller::TEST_CONTROLLER_NAME, - test_controller::TEST_CONTROLLER_CLASS_NAME); - EXPECT_EQ(1u, cm_->get_loaded_controllers().size()); - ASSERT_EQ( - ros2_control_test_assets::minimal_robot_missing_state_keys_urdf, - test_controller->get_robot_description()); -} - TEST_F(TestControllerManagerSrvs, configure_controller_srv) { rclcpp::executors::SingleThreadedExecutor srv_executor;