Add tests for launch_utils of controller manager (#2147)

controller_interface/src/controller_interface_base.cpp

@@ -165,10 +165,13 @@ const rclcpp_lifecycle::State & ControllerInterfaceBase::configure()
     if (ctrl_itf_params_.update_rate != 0u && update_rate > ctrl_itf_params_.update_rate)
     {
       RCLCPP_WARN(
-        get_node()->get_logger(),
-        "The update rate of the controller : '%ld Hz' cannot be higher than the update rate of the "
-        "controller manager : '%d Hz'. Setting it to the update rate of the controller manager.",
-        update_rate, ctrl_itf_params_.update_rate);
+        get_node()->get_logger(), "%s",
+        fmt::format(
+          "The update rate of the controller : '{} Hz' cannot be higher than the update rate of "
+          "the "
+          "controller manager : '{} Hz'. Setting it to the update rate of the controller manager.",
+          update_rate, ctrl_itf_params_.update_rate)
+          .c_str());
     }
     else
     {

controller_manager/CMakeLists.txt

@@ -9,14 +9,15 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
   controller_interface
   controller_manager_msgs
   diagnostic_updater
+  fmt
+  generate_parameter_library
   hardware_interface
+  libstatistics_collector
+  lifecycle_msgs
   pluginlib
   rclcpp
   realtime_tools
   std_msgs
-  libstatistics_collector
-  generate_parameter_library
-  fmt
 )
 
 find_package(ament_cmake REQUIRED)
@@ -249,6 +250,10 @@ if(BUILD_TESTING)
     DESTINATION test)
   ament_add_pytest_test(test_ros2_control_node test/test_ros2_control_node_launch.py)
   ament_add_pytest_test(test_test_utils test/test_test_utils.py)
+
+  # Now include the launch_utils subfolder
+  add_subdirectory(test/test_launch_utils)
+
 endif()
 
 install(

controller_manager/package.xml

@@ -19,34 +19,35 @@
   <depend>controller_interface</depend>
   <depend>controller_manager_msgs</depend>
   <depend>diagnostic_updater</depend>
+  <depend>fmt</depend>
+  <depend>generate_parameter_library</depend>
   <depend>hardware_interface</depend>
-  <depend>launch</depend>
-  <depend>launch_ros</depend>
+  <depend>libstatistics_collector</depend>
+  <depend>lifecycle_msgs</depend>
   <depend>pluginlib</depend>
   <depend>rclcpp</depend>
-  <depend>rcpputils</depend>
   <depend>realtime_tools</depend>
-  <depend>ros2_control_test_assets</depend>
-  <depend>ros2param</depend>
-  <depend>ros2run</depend>
   <depend>std_msgs</depend>
-  <depend>libstatistics_collector</depend>
-  <depend>generate_parameter_library</depend>
-  <depend>fmt</depend>
+
+  <exec_depend>launch_ros</exec_depend>
+  <exec_depend>launch_testing_ros</exec_depend>
+  <exec_depend>launch</exec_depend>
   <exec_depend>python3-filelock</exec_depend>
+  <exec_depend>python3-yaml</exec_depend>
+  <exec_depend>rcl_interfaces</exec_depend>
+  <exec_depend>rclpy</exec_depend>
+  <exec_depend>ros2param</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
 
   <test_depend>ament_cmake_gmock</test_depend>
   <test_depend>ament_cmake_pytest</test_depend>
+  <test_depend>example_interfaces</test_depend>
   <test_depend>hardware_interface_testing</test_depend>
-  <test_depend>launch_testing_ros</test_depend>
   <test_depend>launch_testing</test_depend>
-  <test_depend>launch</test_depend>
   <test_depend>python3-coverage</test_depend>
-  <test_depend>rclpy</test_depend>
   <test_depend>robot_state_publisher</test_depend>
   <test_depend>ros2_control_test_assets</test_depend>
-  <test_depend>sensor_msgs</test_depend>
-  <test_depend>example_interfaces</test_depend>
+  <test_depend>ros2pkg</test_depend>
 
   <export>
     <build_type>ament_cmake</build_type>

controller_manager/test/test_launch_utils/CMakeLists.txt

@@ -0,0 +1,47 @@
+# This subdirectory handles pytest-based launch tests for controller_manager
+
+find_package(ament_cmake_pytest REQUIRED)
+find_package(launch_testing_ament_cmake REQUIRED)
+find_package(rclpy REQUIRED)
+
+# Install the URDF files into the 'share' directory
+#install(
+#  DIRECTORY urdf
+#  DESTINATION share/${PROJECT_NAME}
+#  FILES_MATCHING
+#  PATTERN "*.urdf"
+#)
+
+# Install YAML test files
+#
+install(
+  FILES
+    test_controller_load.yaml
+    test_ros2_control_node_combined.yaml
+  DESTINATION share/${PROJECT_NAME}/test
+)
+
+# Make sure test files get installed into the test tree
+install(
+  FILES
+    test_launch_utils_unit.py
+    test_launch_utils_integration_list.py
+    test_launch_utils_integration_dict.py
+    test_launch_utils_integration_load.py
+  DESTINATION share/${PROJECT_NAME}/test
+)
+
+# Register each test with ament
+ament_add_pytest_test(test_launch_utils_unit test_launch_utils_unit.py
+    APPEND_ENV AMENT_PREFIX_PATH=${ament_index_build_path}
+    PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR}:${CMAKE_CURRENT_SOURCE_DIR}:${PYTHONPATH}
+)
+ament_add_pytest_test(test_launch_utils_integration_list
+  test_launch_utils_integration_list.py
+)
+ament_add_pytest_test(test_launch_utils_integration_dict
+  test_launch_utils_integration_dict.py
+)
+ament_add_pytest_test(test_launch_utils_integration_load
+  test_launch_utils_integration_load.py
+)

controller_manager/test/test_launch_utils/test_controller_load.yaml

@@ -0,0 +1,8 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 100  # Hz
+
+test_controller_load:
+  ros__parameters:
+    type: "controller_manager/test_controller"
+    joint_names: ["joint1"]

controller_manager/test/test_launch_utils/test_launch_utils_integration_dict.py

@@ -0,0 +1,154 @@
+#!/usr/bin/env python3
+# Copyright 2025 Robert Kwan
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import pytest
+import unittest
+from launch import LaunchDescription
+import launch_testing
+from launch_testing.actions import ReadyToTest
+import launch_ros.actions
+from launch.substitutions import PathSubstitution
+from launch_ros.substitutions import FindPackageShare
+from launch.launch_context import LaunchContext
+
+import rclpy
+
+from controller_manager.test_utils import check_controllers_running
+
+from controller_manager.launch_utils import (
+    generate_controllers_spawner_launch_description_from_dict,
+)
+
+
+@pytest.mark.launch_test
+def generate_test_description():
+    """
+    Generate launch description for testing.
+
+    THIS VERSION CREATES ALL NEEDED FILES DYNAMICALLY AND USES THE COMBINED CONFIG.
+    """
+
+    # URDF path (pathlib version, no xacro)
+    urdf_subst = (
+        PathSubstitution(FindPackageShare("ros2_control_test_assets"))
+        / "urdf"
+        / "test_hardware_components.urdf"
+    )
+
+    context = LaunchContext()
+
+    urdf_path_str = urdf_subst.perform(context)
+
+    # DEBUG: You can print the resolved path here to verify:
+    print(f"Resolved URDF Path: {urdf_path_str}")
+
+    with open(urdf_path_str) as infp:
+        robot_description_content = infp.read()
+    robot_description = {"robot_description": robot_description_content}
+
+    # Path to combined YAML
+    robot_controllers_subst = (
+        PathSubstitution(FindPackageShare("controller_manager"))
+        / "test"
+        / "test_ros2_control_node_combined.yaml"
+    )
+
+    robot_controllers_path = robot_controllers_subst.perform(context)
+    print("Resolved controller YAML:", robot_controllers_path)
+
+    # The dictionary keys are the controller names to be spawned/started.
+    # Values can be empty lists since config is provided via the main YAML.
+    ctrl_dict = {
+        "joint_state_broadcaster": [],
+        "controller1": [],
+        "controller2": [],
+    }
+    controller_list = list(ctrl_dict.keys())
+
+    # ===== GENERATE SPAWNER LAUNCH DESCRIPTION =====
+    print(f"Spawning controllers: {controller_list}")
+
+    # ===== CREATE LAUNCH DESCRIPTION =====
+    return LaunchDescription(
+        [
+            launch_ros.actions.Node(
+                package="robot_state_publisher",
+                executable="robot_state_publisher",
+                namespace="",
+                output="both",
+                parameters=[robot_description],
+            ),
+            launch_ros.actions.Node(
+                package="controller_manager",
+                executable="ros2_control_node",
+                namespace="",
+                parameters=[robot_description, robot_controllers_path],
+                output="both",
+            ),
+            generate_controllers_spawner_launch_description_from_dict(
+                controller_info_dict=ctrl_dict, extra_spawner_args=["--activate"]
+            ),
+            ReadyToTest(),
+        ]
+    ), {"controller_list": controller_list}
+
+
+# Active tests
+class TestControllerSpawnerList(unittest.TestCase):
+    """Active tests that run while the launch is active."""
+
+    @classmethod
+    def setUpClass(cls):
+        rclpy.init()
+
+    @classmethod
+    def tearDownClass(cls):
+        rclpy.shutdown()
+
+    def setUp(self):
+        self.node = rclpy.create_node("test_controller_spawner")
+
+    def test_spawner_nodes_launched(self, proc_info):
+        """Ensure processes are running."""
+        process_names = proc_info.process_names()
+        self.assertGreater(len(process_names), 0)
+        print(f"\n[TEST] Active processes: {process_names}")
+
+    def test_controllers_start(self, controller_list):
+        cnames = controller_list.copy()
+        check_controllers_running(self.node, cnames, state="active")
+
+    def test_spawner_exit_code(self, proc_info):
+        """Test that spawner process ran (may have completed already)."""
+        process_names = proc_info.process_names()
+        print(f"\n[TEST] Checking for spawner in: {process_names}")
+
+        # The spawner may have already completed successfully and exited
+        # So we just verify that we have processes running
+        self.assertGreater(len(process_names), 0)
+        print(f"[TEST] Launch has {len(process_names)} active processes")
+
+
+@launch_testing.post_shutdown_test()
+class TestProcessOutput(unittest.TestCase):
+    """Post-shutdown tests."""
+
+    def test_exit_codes(self, proc_info):
+        """Verify all processes exited successfully."""
+        launch_testing.asserts.assertExitCodes(
+            proc_info,
+            # All other processes (ros2_control_node, etc.) must exit cleanly (0)
+            allowable_exit_codes=[0, 1, -2, -15],
+        )