From 2ef793ae6a335d8fe48199b6a1774b28726ee161 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 26 Aug 2024 14:10:35 +0200 Subject: [PATCH] Fix unload of controllers when spawned with `--unload-on-kill` (#1717) (cherry picked from commit 9e57adfbc414f92492278fdba10e60716a5995b7) # Conflicts: # controller_manager/controller_manager/spawner.py # controller_manager/test/test_spawner_unspawner.cpp --- .../controller_manager/spawner.py | 24 +++++- .../test/test_spawner_unspawner.cpp | 74 +++++++++++++++++++ 2 files changed, 96 insertions(+), 2 deletions(-) diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py index f20c0a3066..3db0842801 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/controller_manager/controller_manager/spawner.py @@ -244,7 +244,7 @@ def main(args=None): node.get_logger().info( bcolors.OKGREEN - + "Configured and activated all the parsed controllers list!" + + f"Configured and activated all the parsed controllers list : {controller_names}!" + bcolors.ENDC ) if args.stopped: @@ -268,6 +268,7 @@ def main(args=None): node.get_logger().error("Failed to deactivate controller") return 1 +<<<<<<< HEAD node.get_logger().info("Deactivated controller") elif args.stopped: @@ -277,8 +278,27 @@ def main(args=None): if not ret.ok: node.get_logger().error("Failed to unload controller") return 1 +======= + node.get_logger().info( + f"Successfully deactivated controllers : {controller_names}" + ) +>>>>>>> 9e57adf (Fix unload of controllers when spawned with `--unload-on-kill` (#1717)) + + unload_status = True + for controller_name in controller_names: + ret = unload_controller(node, controller_manager_name, controller_name) + if not ret.ok: + unload_status = False + node.get_logger().error( + bcolors.FAIL + + f"Failed to unload controller : {controller_name}" + + bcolors.ENDC + ) - node.get_logger().info("Unloaded controller") + if unload_status: + node.get_logger().info(f"Successfully unloaded controllers : {controller_names}") + else: + return 1 return 0 except KeyboardInterrupt: pass diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index c83f777cfa..2c14e5163f 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -254,6 +254,80 @@ TEST_F(TestLoadController, unload_on_kill) ASSERT_EQ(cm_->get_loaded_controllers().size(), 0ul); } +<<<<<<< HEAD +======= +TEST_F(TestLoadController, unload_on_kill_activate_as_group) +{ + // Launch spawner with unload on kill + // timeout command will kill it after the specified time with signal SIGINT + ControllerManagerRunner cm_runner(this); + cm_->set_parameter(rclcpp::Parameter("ctrl_3.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); + cm_->set_parameter(rclcpp::Parameter("ctrl_2.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); + std::stringstream ss; + ss << "timeout --signal=INT 5 " + << std::string(coveragepy_script) + + " $(ros2 pkg prefix controller_manager)/lib/controller_manager/spawner " + << "ctrl_3 ctrl_2 --activate-as-group -c test_controller_manager --unload-on-kill"; + + EXPECT_NE(std::system(ss.str().c_str()), 0) + << "timeout should have killed spawner and returned non 0 code"; + + ASSERT_EQ(cm_->get_loaded_controllers().size(), 0ul); +} + +TEST_F(TestLoadController, spawner_test_fallback_controllers) +{ + const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") + + "/test/test_controller_spawner_with_fallback_controllers.yaml"; + + cm_->set_parameter(rclcpp::Parameter("ctrl_1.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); + cm_->set_parameter(rclcpp::Parameter("ctrl_2.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); + cm_->set_parameter(rclcpp::Parameter("ctrl_3.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); + + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(call_spawner("ctrl_1 -c test_controller_manager --load-only -p " + test_file_path), 0); + + ASSERT_EQ(cm_->get_loaded_controllers().size(), 1ul); + { + auto ctrl_1 = cm_->get_loaded_controllers()[0]; + ASSERT_EQ(ctrl_1.info.name, "ctrl_1"); + ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_TRUE(ctrl_1.info.fallback_controllers_names.empty()); + ASSERT_EQ( + ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + } + + // Try to spawn now the controller with fallback controllers inside the yaml + EXPECT_EQ( + call_spawner("ctrl_2 ctrl_3 -c test_controller_manager --load-only -p " + test_file_path), 0); + + ASSERT_EQ(cm_->get_loaded_controllers().size(), 3ul); + { + auto ctrl_1 = cm_->get_loaded_controllers()[0]; + ASSERT_EQ(ctrl_1.info.name, "ctrl_1"); + ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_TRUE(ctrl_1.info.fallback_controllers_names.empty()); + ASSERT_EQ( + ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + + auto ctrl_2 = cm_->get_loaded_controllers()[1]; + ASSERT_EQ(ctrl_2.info.name, "ctrl_2"); + ASSERT_EQ(ctrl_2.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_THAT( + ctrl_2.info.fallback_controllers_names, testing::ElementsAre("ctrl_6", "ctrl_7", "ctrl_8")); + ASSERT_EQ( + ctrl_2.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + + auto ctrl_3 = cm_->get_loaded_controllers()[2]; + ASSERT_EQ(ctrl_3.info.name, "ctrl_3"); + ASSERT_EQ(ctrl_3.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_THAT(ctrl_3.info.fallback_controllers_names, testing::ElementsAre("ctrl_9")); + ASSERT_EQ( + ctrl_3.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + } +} + +>>>>>>> 9e57adf (Fix unload of controllers when spawned with `--unload-on-kill` (#1717)) TEST_F(TestLoadController, spawner_with_many_controllers) { std::stringstream ss;