diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py index f20c0a3066..a6b7215f5a 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: @@ -265,20 +265,33 @@ def main(args=None): node, controller_manager_name, controller_names, [], True, True, 5.0 ) if not ret.ok: - node.get_logger().error("Failed to deactivate controller") + node.get_logger().error( + bcolors.FAIL + "Failed to deactivate controller" + bcolors.ENDC + ) return 1 - node.get_logger().info("Deactivated controller") + node.get_logger().info( + f"Successfully deactivated controllers : {controller_names}" + ) elif args.stopped: node.get_logger().warn('"--stopped" flag is deprecated use "--inactive" instead') - ret = unload_controller(node, controller_manager_name, controller_name) - if not ret.ok: - node.get_logger().error("Failed to unload controller") - return 1 + 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..2cc742cdfd 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -254,6 +254,25 @@ TEST_F(TestLoadController, unload_on_kill) ASSERT_EQ(cm_->get_loaded_controllers().size(), 0ul); } +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_with_many_controllers) { std::stringstream ss;