Fix unload of controllers when spawned with --unload-on-kill

controller_manager/controller_manager/spawner.py

@@ -234,7 +234,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
  )
 
@@ -258,16 +258,25 @@ def main(args=None):
  )
  return 1
 
- node.get_logger().info("Deactivated controller")
-
- ret = unload_controller(node, controller_manager_name, controller_name)
- if not ret.ok:
- node.get_logger().error(
- bcolors.FAIL + "Failed to unload controller" + bcolors.ENDC
+ node.get_logger().info(
+ f"Successfully deactivated controllers : {controller_names}"
  )
- return 1
 
- node.get_logger().info("Unloaded controller")
+ 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
+ )
+
+ if unload_status:
+ node.get_logger().info(f"Successfully unloaded controllers : {controller_names}")
+ else:
+ return 1
  return 0
  except KeyboardInterrupt:
  pass

controller_manager/test/test_spawner_unspawner.cpp

@@ -321,6 +321,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_test_fallback_controllers)
 {
  const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") +