From fc9898f560aa13dce06c854b82e6f121fd8d3e37 Mon Sep 17 00:00:00 2001
From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com>
Date: Mon, 16 Dec 2024 22:13:54 +0100
Subject: [PATCH] Use launch_utils instead of a spawner per controller
 (backport #666) (#667)

---
 example_13/CMakeLists.txt                     |   9 +-
 .../bringup/launch/three_robots.launch.py     | 105 ++++++------------
 example_13/package.xml                        |   7 +-
 example_13/test/test_three_robots_launch.py   |   4 +-
 4 files changed, 52 insertions(+), 73 deletions(-)

diff --git a/example_13/CMakeLists.txt b/example_13/CMakeLists.txt
index 0f8ae8643..1ea254d7e 100644
--- a/example_13/CMakeLists.txt
+++ b/example_13/CMakeLists.txt
@@ -26,11 +26,18 @@ install(
 )
 
 if(BUILD_TESTING)
+  find_package(ament_cmake_ros REQUIRED)
+  find_package(launch_testing_ament_cmake REQUIRED)
   find_package(ament_cmake_pytest REQUIRED)
 
   ament_add_pytest_test(example_13_urdf_xacro test/test_urdf_xacro.py)
   ament_add_pytest_test(view_example_13_launch test/test_view_robot_launch.py)
-  ament_add_pytest_test(run_example_13_launch test/test_three_robots_launch.py)
+
+  function(add_ros_isolated_launch_test path)
+    set(RUNNER "${ament_cmake_ros_DIR}/run_test_isolated.py")
+    add_launch_test("${path}" RUNNER "${RUNNER}" ${ARGN})
+  endfunction()
+  add_ros_isolated_launch_test(test/test_three_robots_launch.py)
 endif()
 
 ## EXPORTS
diff --git a/example_13/bringup/launch/three_robots.launch.py b/example_13/bringup/launch/three_robots.launch.py
index 92a309b48..1785a1e1a 100644
--- a/example_13/bringup/launch/three_robots.launch.py
+++ b/example_13/bringup/launch/three_robots.launch.py
@@ -22,6 +22,7 @@
 
 from launch_ros.actions import Node
 from launch_ros.substitutions import FindPackageShare
+from controller_manager.launch_utils import generate_controllers_spawner_launch_description
 
 
 def generate_launch_description():
@@ -87,7 +88,10 @@ def generate_launch_description():
     control_node = Node(
         package="controller_manager",
         executable="ros2_control_node",
-        parameters=[robot_description, robot_controllers],
+        parameters=[robot_controllers],
+        remappings=[
+            ("~/robot_description", "/robot_description"),
+        ],
     )
     robot_state_pub_node = Node(
         package="robot_state_publisher",
@@ -104,73 +108,44 @@ def generate_launch_description():
         condition=IfCondition(gui),
     )
 
-    # Separate robot state publishers for each robot
-
-    # Global joint state broadcaster
-    joint_state_broadcaster_spawner = Node(
-        package="controller_manager",
-        executable="spawner",
-        arguments=["joint_state_broadcaster"],
+    # global broadcaster and initially active controllers from RRBot
+    general_ctrl_spawner = generate_controllers_spawner_launch_description(
+        [
+            # Global joint state broadcaster
+            "joint_state_broadcaster",
+            # RRBot controllers
+            "rrbot_joint_state_broadcaster",
+            "rrbot_position_controller",
+            # External FTS broadcaster
+            "rrbot_external_fts_broadcaster",
+        ],
+        controller_params_files=[robot_controllers],
     )
 
-    # RRBot controllers
-    rrbot_joint_state_broadcaster_spawner = Node(
-        package="controller_manager",
-        executable="spawner",
-        arguments=["rrbot_joint_state_broadcaster"],
-    )
-    rrbot_position_controller_spawner = Node(
-        package="controller_manager",
-        executable="spawner",
-        arguments=["rrbot_position_controller"],
-    )
-    # External FTS broadcaster
-    rrbot_external_fts_broadcaster_spawner = Node(
-        package="controller_manager",
-        executable="spawner",
-        arguments=["rrbot_external_fts_broadcaster"],
+    # RRBot with sensors controllers, initially active
+    rrbot_sensor_ctrl_spawner_active = generate_controllers_spawner_launch_description(
+        ["rrbot_with_sensor_joint_state_broadcaster", "rrbot_with_sensor_fts_broadcaster"],
+        controller_params_files=[robot_controllers],
     )
 
-    # RRBot controllers
-    rrbot_with_sensor_joint_state_broadcaster_spawner = Node(
-        package="controller_manager",
-        executable="spawner",
-        arguments=["rrbot_with_sensor_joint_state_broadcaster"],
-    )
-    rrbot_with_sensor_position_controller_spawner = Node(
-        package="controller_manager",
-        executable="spawner",
-        arguments=[
+    # RRBot with sensors controllers, initially inactive
+    rrbot_sensor_ctrl_spawner_inactive = generate_controllers_spawner_launch_description(
+        [
             "rrbot_with_sensor_position_controller",
-            "--inactive",
         ],
-    )
-    rrbot_with_sensor_fts_broadcaster_spawner = Node(
-        package="controller_manager",
-        executable="spawner",
-        arguments=["rrbot_with_sensor_fts_broadcaster"],
+        controller_params_files=[robot_controllers],
+        extra_spawner_args=["--inactive"],
     )
 
-    # ThreeDofBot controllers
-    threedofbot_joint_state_broadcaster_spawner = Node(
-        package="controller_manager",
-        executable="spawner",
-        arguments=[
+    # ThreeDofBot controllers, initially inactive
+    threedofbot_ctrl_spawner = generate_controllers_spawner_launch_description(
+        [
             "threedofbot_joint_state_broadcaster",
-            "-c",
-            "/controller_manager",
-            "--inactive",
+            "threedofbot_position_controller",
+            "threedofbot_pid_gain_controller",
         ],
-    )
-    threedofbot_position_controller_spawner = Node(
-        package="controller_manager",
-        executable="spawner",
-        arguments=["threedofbot_position_controller", "--inactive"],
-    )
-    threedofbot_pid_gain_controller_spawner = Node(
-        package="controller_manager",
-        executable="spawner",
-        arguments=["threedofbot_pid_gain_controller", "--inactive"],
+        controller_params_files=[robot_controllers],
+        extra_spawner_args=["--inactive"],
     )
 
     # Command publishers
@@ -197,16 +172,10 @@ def generate_launch_description():
         control_node,
         robot_state_pub_node,
         rviz_node,
-        joint_state_broadcaster_spawner,
-        rrbot_joint_state_broadcaster_spawner,
-        rrbot_position_controller_spawner,
-        rrbot_external_fts_broadcaster_spawner,
-        rrbot_with_sensor_joint_state_broadcaster_spawner,
-        rrbot_with_sensor_position_controller_spawner,
-        rrbot_with_sensor_fts_broadcaster_spawner,
-        threedofbot_joint_state_broadcaster_spawner,
-        threedofbot_position_controller_spawner,
-        threedofbot_pid_gain_controller_spawner,
+        general_ctrl_spawner,
+        rrbot_sensor_ctrl_spawner_active,
+        rrbot_sensor_ctrl_spawner_inactive,
+        threedofbot_ctrl_spawner,
         rrbot_position_command_publisher,
         rrbot_with_sensor_position_command_publisher,
         threedofbot_position_command_publisher,
diff --git a/example_13/package.xml b/example_13/package.xml
index 0cc8f4a59..9e0af6f60 100644
--- a/example_13/package.xml
+++ b/example_13/package.xml
@@ -31,8 +31,13 @@
   <exec_depend>xacro</exec_depend>
 
   <test_depend>ament_cmake_pytest</test_depend>
-  <test_depend>launch_testing_ros</test_depend>
+  <test_depend>ament_cmake_ros</test_depend>
+  <test_depend>launch_ros</test_depend>
+  <test_depend>launch_testing_ament_cmake</test_depend>
+  <test_depend>launch_testing</test_depend>
+  <test_depend>launch</test_depend>
   <test_depend>liburdfdom-tools</test_depend>
+  <test_depend>rclpy</test_depend>
   <test_depend>ros2_control_demo_testing</test_depend>
   <test_depend>xacro</test_depend>
 
diff --git a/example_13/test/test_three_robots_launch.py b/example_13/test/test_three_robots_launch.py
index 230b6ca95..444647255 100644
--- a/example_13/test/test_three_robots_launch.py
+++ b/example_13/test/test_three_robots_launch.py
@@ -29,7 +29,6 @@
 # Author: Christoph Froehlich
 
 import os
-import pytest
 import unittest
 import subprocess
 
@@ -48,8 +47,7 @@
 )
 
 
-# Executes the given launch file and checks if all nodes can be started
-@pytest.mark.rostest
+# Executes the given launch file
 def generate_test_description():
     launch_include = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(