From e7aa06e8bfa7b502dd3500174bc033ae7b2b260d Mon Sep 17 00:00:00 2001 From: Robert Kwan Date: Sun, 26 Oct 2025 22:04:37 -0700 Subject: [PATCH 01/13] Merge back from jazzy branch - add tests for launch_utils of controller manager (#2147) --- .../test/test_ros2_control_node_launch.py | 64 ++++++++++++++++++- 1 file changed, 62 insertions(+), 2 deletions(-) diff --git a/controller_manager/test/test_ros2_control_node_launch.py b/controller_manager/test/test_ros2_control_node_launch.py index 9c9f8892ed..cca3c7ad8d 100644 --- a/controller_manager/test/test_ros2_control_node_launch.py +++ b/controller_manager/test/test_ros2_control_node_launch.py @@ -43,8 +43,11 @@ check_controllers_running, check_node_running, ) -from controller_manager.launch_utils import generate_controllers_spawner_launch_description - +from controller_manager.launch_utils import ( + generate_controllers_spawner_launch_description, + generate_controllers_spawner_launch_description_from_dict, + generate_load_controller_launch_description, +) # Executes the given launch file and checks if all nodes can be started @pytest.mark.launch_test @@ -102,6 +105,63 @@ def setUp(self): def tearDown(self): self.node.destroy_node() + # ------------------------------------------------------------------ + # Helper methods + # ------------------------------------------------------------------ + def _extract_actions(self, result): + """Return a list of launch actions, regardless of type.""" + if isinstance(result, list): + return result + elif hasattr(result, "entities"): # LaunchDescription in newer ROS2 + return list(result.entities) + else: + return [result] + + def _assert_launch_result(self, result, min_len=0): + """Verify that result is list- or LaunchDescription-like.""" + self.assertTrue( + isinstance(result, (list, LaunchDescription)), + f"Unexpected result type: {type(result)}", + ) + actions = self._extract_actions(result) + self.assertGreaterEqual(len(actions), min_len) + for act in actions: + self.assertTrue( + hasattr(act, "execute") or hasattr(act, "visit"), + f"Invalid action type: {type(act)}", + ) + return actions + + # ------------------------------------------------------------------ + # Launch utility tests + # ------------------------------------------------------------------ + def test_generate_controllers_spawner_from_list(self): + controllers = ["test_controller_1", "test_controller_2"] + result = generate_controllers_spawner_launch_description(controllers) + actions = self._assert_launch_result(result, min_len=1) + self.assertTrue(actions is not None and len(actions) > 0) + + def test_generate_controllers_spawner_from_dict(self): + """Ensure dict-based API works with string param files (old-style).""" + controllers = { + "ctrl_A": ["/tmp/dummy.yaml"], + "ctrl_B": ["/tmp/dummy.yaml"], + } + result = generate_controllers_spawner_launch_description_from_dict(controllers) + actions = self._extract_actions(result) + self.assertIsInstance(result, LaunchDescription) + self.assertEqual(len(actions), 3) + self.assertIsInstance(actions[-1], Node) + + def test_generate_load_controller_launch_description(self): + """Test load controller description with valid single string params.""" + controllers = ["test_controller_load"] + result = generate_load_controller_launch_description(controllers) + actions = self._extract_actions(result) + self.assertIsInstance(result, LaunchDescription) + self.assertEqual(len(actions), 3) + self.assertIsInstance(actions[-1], Node) + def test_node_start(self): check_node_running(self.node, "controller_manager") From 8e7d6bc828095c3b8e24076400c3dec4ed535276 Mon Sep 17 00:00:00 2001 From: Robert Kwan Date: Mon, 27 Oct 2025 22:27:48 -0700 Subject: [PATCH 02/13] Fix missing line to Add tests for launch_utils of controller manager (#2147) --- controller_manager/test/test_ros2_control_node_launch.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/controller_manager/test/test_ros2_control_node_launch.py b/controller_manager/test/test_ros2_control_node_launch.py index cca3c7ad8d..46a4f813c0 100644 --- a/controller_manager/test/test_ros2_control_node_launch.py +++ b/controller_manager/test/test_ros2_control_node_launch.py @@ -37,6 +37,7 @@ from launch_testing.actions import ReadyToTest import launch_testing.markers import launch_ros.actions +from launch_ros.actions import Node import rclpy from controller_manager.test_utils import ( @@ -49,6 +50,7 @@ generate_load_controller_launch_description, ) + # Executes the given launch file and checks if all nodes can be started @pytest.mark.launch_test def generate_test_description(): @@ -133,7 +135,7 @@ def _assert_launch_result(self, result, min_len=0): return actions # ------------------------------------------------------------------ - # Launch utility tests + # Launch utility tests # ------------------------------------------------------------------ def test_generate_controllers_spawner_from_list(self): controllers = ["test_controller_1", "test_controller_2"] From 05e3d32fe24ddc87e9cfa1e6d6f74e48bd6a3cb0 Mon Sep 17 00:00:00 2001 From: Robert Kwan Date: Mon, 27 Oct 2025 22:27:48 -0700 Subject: [PATCH 03/13] Fix missing line to Add tests for launch_utils of controller manager (#2147) --- controller_manager/test/test_ros2_control_node_launch.py | 1 + 1 file changed, 1 insertion(+) diff --git a/controller_manager/test/test_ros2_control_node_launch.py b/controller_manager/test/test_ros2_control_node_launch.py index 46a4f813c0..bcc1c04a33 100644 --- a/controller_manager/test/test_ros2_control_node_launch.py +++ b/controller_manager/test/test_ros2_control_node_launch.py @@ -51,6 +51,7 @@ ) + # Executes the given launch file and checks if all nodes can be started @pytest.mark.launch_test def generate_test_description(): From 52bcf35abd30989518296c03fb6c22b403ac4783 Mon Sep 17 00:00:00 2001 From: Robert Kwan Date: Sun, 9 Nov 2025 01:41:10 -0800 Subject: [PATCH 04/13] Add more tests for launch_utils of controller manager (#2147) --- controller_manager/CMakeLists.txt | 24 ++ controller_manager/test/test_controller3.yaml | 8 + .../test/test_controller_load.yaml | 8 + .../test/test_joint_state_broadcast.yaml | 7 + .../test_launch_utils_integration_dict.py | 240 ++++++++++++++++++ .../test_launch_utils_integration_list.py | 232 +++++++++++++++++ .../test_launch_utils_integration_load.py | 167 ++++++++++++ .../test/test_launch_utils_unit.py | 97 +++++++ .../test/test_ros2_control_node_combined.yaml | 22 ++ 9 files changed, 805 insertions(+) create mode 100644 controller_manager/test/test_controller3.yaml create mode 100644 controller_manager/test/test_controller_load.yaml create mode 100644 controller_manager/test/test_joint_state_broadcast.yaml create mode 100644 controller_manager/test/test_launch_utils_integration_dict.py create mode 100644 controller_manager/test/test_launch_utils_integration_list.py create mode 100644 controller_manager/test/test_launch_utils_integration_load.py create mode 100644 controller_manager/test/test_launch_utils_unit.py create mode 100644 controller_manager/test/test_ros2_control_node_combined.yaml diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index 1bd47cd923..71039885c6 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -244,10 +244,34 @@ if(BUILD_TESTING) ) find_package(ament_cmake_pytest REQUIRED) + find_package(launch_testing_ament_cmake REQUIRED) + find_package(rclpy REQUIRED) install(FILES test/test_ros2_control_node.yaml + test/test_controller_load.yaml + test/test_ros2_control_node_combined.yaml 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) + ament_add_pytest_test(test_launch_utils_unit test/test_launch_utils_unit.py + APPEND_ENV AMENT_PREFIX_PATH=${ament_index_build_path} + PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR}:${CMAKE_CURRENT_SOURCE_DIR}:${PYTHONPATH} + ) + # ---------------------------------------------------------------------------- + # Integration tests for individual launch_utils entry points + # ---------------------------------------------------------------------------- + ament_add_pytest_test(test_launch_utils_integration_list + test/test_launch_utils_integration_list.py + TIMEOUT 60 + ) + ament_add_pytest_test(test_launch_utils_integration_dict + test/test_launch_utils_integration_dict.py + TIMEOUT 60 + ) + ament_add_pytest_test(test_launch_utils_integration_load + test/test_launch_utils_integration_load.py + TIMEOUT 60 + ) + endif() install( diff --git a/controller_manager/test/test_controller3.yaml b/controller_manager/test/test_controller3.yaml new file mode 100644 index 0000000000..4cb904882f --- /dev/null +++ b/controller_manager/test/test_controller3.yaml @@ -0,0 +1,8 @@ +controller_manager: + ros__parameters: + update_rate: 100 # Hz + +controller3: + ros__parameters: + type: "controller_manager/test_controller" + joint_names: ["joint3"] diff --git a/controller_manager/test/test_controller_load.yaml b/controller_manager/test/test_controller_load.yaml new file mode 100644 index 0000000000..ae5745e888 --- /dev/null +++ b/controller_manager/test/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"] diff --git a/controller_manager/test/test_joint_state_broadcast.yaml b/controller_manager/test/test_joint_state_broadcast.yaml new file mode 100644 index 0000000000..9001187574 --- /dev/null +++ b/controller_manager/test/test_joint_state_broadcast.yaml @@ -0,0 +1,7 @@ +controller_manager: + ros__parameters: + update_rate: 100 # Hz + +joint_state_broadcaster: + ros__parameters: + type: "joint_state_broadcaster/JointStateBroadcaster" diff --git a/controller_manager/test/test_launch_utils_integration_dict.py b/controller_manager/test/test_launch_utils_integration_dict.py new file mode 100644 index 0000000000..d16391d675 --- /dev/null +++ b/controller_manager/test/test_launch_utils_integration_dict.py @@ -0,0 +1,240 @@ +#!/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 +import os +import tempfile +import time + +from ament_index_python.packages import get_package_share_directory, get_package_prefix +from launch import LaunchDescription +import launch_testing +from launch_testing.actions import ReadyToTest +import launch_testing.markers +import launch_ros.actions + +import rclpy + +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. + """ + + # Create temporary directory for all test files + temp_dir = tempfile.mkdtemp() + print(f"Creating test files in: {temp_dir}") + + # Get URDF, without involving xacro + urdf = os.path.join( + get_package_share_directory("ros2_control_test_assets"), + "urdf", + "test_hardware_components.urdf", + ) + with open(urdf) as infp: + robot_description_content = infp.read() + robot_description = {"robot_description": robot_description_content} + + robot_controllers = os.path.join( + get_package_prefix("controller_manager"), "test", "test_ros2_control_node_combined.yaml" + ) + + # Verify both files exist + assert os.path.isfile(robot_controllers), f"Controller config not created: {robot_controllers}" + assert os.path.isfile(urdf), f"URDF not created: {urdf}" + + robot_state_pub_node = launch_ros.actions.Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[robot_description], + ) + + # ===== START CONTROLLER MANAGER (ros2_control_node) ===== + control_node = launch_ros.actions.Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[robot_controllers], # Use the combined config file + output="both", + ) + + # 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": [robot_controllers], + "ctrl_with_parameters_and_type": [robot_controllers], + "controller3": [robot_controllers], + } + controller_list = list(ctrl_dict.keys()) + + # ===== GENERATE SPAWNER LAUNCH DESCRIPTION ===== + print(f"Spawning controllers: {controller_list}") + + # Correct function name and call + spawner_ld = generate_controllers_spawner_launch_description_from_dict( + controller_info_dict=ctrl_dict, + ) + + # ===== CREATE LAUNCH DESCRIPTION ===== + ld = LaunchDescription( + [robot_state_pub_node, control_node, ReadyToTest()] + spawner_ld.entities + ) + + # Return tuple with launch description and test context + return ld, { + "controller_list": controller_list, # Key name updated to match the test function + "robot_controllers": str(robot_controllers), + "urdf_file": urdf, + "temp_dir": temp_dir, + } + + +# 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 test_spawner_nodes_launched(self, proc_info): + """Ensure processes are running.""" + process_names = proc_info.process_names() + self.assertGreater(len(process_names), 0) + print("\n[TEST] Active processes: {process_names}") + + def test_controllers_loaded(self, proc_info, controller_list): + """Test that controllers were loaded (poll until they appear).""" + node = rclpy.create_node("test_controller_query_node") + + try: + from controller_manager_msgs.srv import ListControllers + + client = node.create_client(ListControllers, "/controller_manager/list_controllers") + + print("\n[TEST] Waiting for controller_manager service...") + wait_for_svc_timeout = 30.0 + if not client.wait_for_service(timeout_sec=wait_for_svc_timeout): + process_names = proc_info.process_names() + self.fail( + f"Controller manager service not available after {wait_for_svc_timeout}s.\n" + f"Active processes: {process_names}" + ) + + # Poll for controllers to be registered + print("[TEST] Service available, polling for controllers (timeout 30s)...") + deadline = node.get_clock().now() + rclpy.duration.Duration(seconds=30.0) + seen = [] + while node.get_clock().now() < deadline: + req = ListControllers.Request() + fut = client.call_async(req) + rclpy.spin_until_future_complete(node, fut, timeout_sec=2.0) + if fut.done() and fut.result() is not None: + response = fut.result() + seen = [c.name for c in response.controller] + if all(ctrl in seen for ctrl in controller_list): + print(f"[TEST] Loaded controllers: {seen}") + break + # small sleep to avoid tight-loop + time.sleep(0.2) + else: + # timeout expired + self.fail( + f"Timeout waiting for controllers to be loaded. " + f"Expected: {controller_list}, saw: {seen}" + ) + + # Final assert (defensive) + for controller in controller_list: + self.assertIn( + controller, + seen, + f"Controller '{controller}' was not loaded. Available: {seen}", + ) + + print(f"[TEST] ? All {len(controller_list)} controllers loaded successfully") + + finally: + node.destroy_node() + + 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.""" + print("\n[POST-SHUTDOWN] Process exit codes:") + for process_name in proc_info.process_names(): + info = proc_info[process_name] + print(f" {process_name}: {info.returncode}") + + for process_name in proc_info.process_names(): + info = proc_info[process_name] + + if "ros2_control_node" in process_name: + self.assertEqual( + info.returncode, 0, f"{process_name} exited with {info.returncode}" + ) + elif "spawner" in process_name: + # Spawner should complete successfully (0) or be terminated + self.assertIn( + info.returncode, + [0, -2, -15], + f"Spawner {process_name} exited with {info.returncode}", + ) + else: + self.assertIn( + info.returncode, [0, -2, -15], f"{process_name} exited with {info.returncode}" + ) + + print("[POST-SHUTDOWN] ? All processes exited as expected") + + def test_cleanup_temp_files(self, temp_dir, robot_controllers, urdf_file): + """Clean up temporary test files.""" + import shutil + + print(f"\n[CLEANUP] Removing temporary directory: {temp_dir}") + + # The original clean-up logic was commented out, enabling it for safety + try: + if os.path.exists(temp_dir): + shutil.rmtree(temp_dir) + + print("[CLEANUP] ? Temporary files removed") + except Exception as e: + print(f"[CLEANUP] Warning: Cleanup failed: {e}") diff --git a/controller_manager/test/test_launch_utils_integration_list.py b/controller_manager/test/test_launch_utils_integration_list.py new file mode 100644 index 0000000000..ab49a17ef1 --- /dev/null +++ b/controller_manager/test/test_launch_utils_integration_list.py @@ -0,0 +1,232 @@ +#!/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 +import os +import time +import tempfile + +from ament_index_python.packages import get_package_share_directory, get_package_prefix +from launch import LaunchDescription +import launch_testing +from launch_testing.actions import ReadyToTest +import launch_testing.markers +import launch_ros.actions + +import rclpy + +from controller_manager.launch_utils import ( + generate_controllers_spawner_launch_description, +) + + +@pytest.mark.launch_test +def generate_test_description(): + """ + Generate launch description for testing. + """ + + # Create temporary directory for all test files + temp_dir = tempfile.mkdtemp() + print(f"Creating test files in: {temp_dir}") + + # Get URDF, without involving xacro + urdf = os.path.join( + get_package_share_directory("ros2_control_test_assets"), + "urdf", + "test_hardware_components.urdf", + ) + with open(urdf) as infp: + robot_description_content = infp.read() + robot_description = {"robot_description": robot_description_content} + + robot_controllers = os.path.join( + get_package_prefix("controller_manager"), "test", "test_ros2_control_node_combined.yaml" + ) + + # Verify both files exist + assert os.path.isfile(robot_controllers), f"Controller config not created: {robot_controllers}" + assert os.path.isfile(urdf), f"URDF not created: {urdf}" + + robot_state_pub_node = launch_ros.actions.Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[robot_description], + ) + + # ===== START CONTROLLER MANAGER ===== + control_node = launch_ros.actions.Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[robot_controllers], + output="both", + ) + + # ===== DEFINE CONTROLLERS TO SPAWN ===== + controller_list = ["joint_state_broadcaster", "ctrl_with_parameters_and_type", "controller3"] + + # ===== GENERATE SPAWNER ===== + print(f"Spawning controllers: {controller_list}") + print(f"Using config file: {robot_controllers}") + + spawner_ld = generate_controllers_spawner_launch_description( + controller_names=controller_list.copy(), + controller_params_files=[robot_controllers], + ) + + # ===== CREATE LAUNCH DESCRIPTION ===== + ld = LaunchDescription( + [robot_state_pub_node, control_node, ReadyToTest()] + spawner_ld.entities + ) + + # Return tuple with test context + return ld, { + "controller_list": controller_list, + "robot_controllers": robot_controllers, + "urdf_file": urdf, + "temp_dir": temp_dir, + } + + +# 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 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_loaded(self, proc_info, controller_list): + """Test that controllers were loaded (poll until they appear).""" + node = rclpy.create_node("test_controller_query_node") + + try: + from controller_manager_msgs.srv import ListControllers + + client = node.create_client(ListControllers, "/controller_manager/list_controllers") + + print("\n[TEST] Waiting for controller_manager service...") + wait_for_svc_timeout = 30.0 + if not client.wait_for_service(timeout_sec=wait_for_svc_timeout): + process_names = proc_info.process_names() + self.fail( + f"Controller manager service not available after {wait_for_svc_timeout}s.\n" + f"Active processes: {process_names}" + ) + + # Poll for controllers to be registered + print("[TEST] Service available, polling for controllers (timeout 30s)...") + deadline = node.get_clock().now() + rclpy.duration.Duration(seconds=30.0) + seen = [] + while node.get_clock().now() < deadline: + req = ListControllers.Request() + fut = client.call_async(req) + rclpy.spin_until_future_complete(node, fut, timeout_sec=2.0) + if fut.done() and fut.result() is not None: + response = fut.result() + seen = [c.name for c in response.controller] + if all(ctrl in seen for ctrl in controller_list): + print(f"[TEST] Loaded controllers: {seen}") + break + # small sleep to avoid tight-loop + time.sleep(0.2) + else: + # timeout expired + self.fail( + f"Timeout waiting for controllers to be loaded. " + f"Expected: {controller_list}, saw: {seen}" + ) + + # Final assert (defensive) + for controller in controller_list: + self.assertIn( + controller, + seen, + f"Controller '{controller}' was not loaded. Available: {seen}", + ) + + print(f"[TEST] ? All {len(controller_list)} controllers loaded successfully") + + finally: + node.destroy_node() + + 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.""" + print("\n[POST-SHUTDOWN] Process exit codes:") + for process_name in proc_info.process_names(): + info = proc_info[process_name] + print(f" {process_name}: {info.returncode}") + + for process_name in proc_info.process_names(): + info = proc_info[process_name] + + if "ros2_control_node" in process_name: + self.assertEqual( + info.returncode, 0, f"{process_name} exited with {info.returncode}" + ) + elif "spawner" in process_name: + # Spawner should complete successfully (0) or be terminated + self.assertIn( + info.returncode, + [0, -2, -15], + f"Spawner {process_name} exited with {info.returncode}", + ) + else: + self.assertIn( + info.returncode, [0, -2, -15], f"{process_name} exited with {info.returncode}" + ) + + print("[POST-SHUTDOWN] ? All processes exited as expected") + + def test_cleanup_temp_files(self, temp_dir, robot_controllers, urdf_file): + """Clean up temporary test files.""" + import shutil + + print(f"\n[CLEANUP] Removing temporary directory: {temp_dir}") + + try: + if os.path.exists(temp_dir): + shutil.rmtree(temp_dir) + + print("[CLEANUP] ? Temporary files removed") + except Exception as e: + print(f"[CLEANUP] Warning: Cleanup failed: {e}") diff --git a/controller_manager/test/test_launch_utils_integration_load.py b/controller_manager/test/test_launch_utils_integration_load.py new file mode 100644 index 0000000000..418342e1e6 --- /dev/null +++ b/controller_manager/test/test_launch_utils_integration_load.py @@ -0,0 +1,167 @@ +#!/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 os + +import pytest +import unittest +import os +import tempfile + +from ament_index_python.packages import get_package_share_directory, get_package_prefix +from launch import LaunchDescription +import launch_testing +from launch_testing.actions import ReadyToTest +import launch_testing.asserts +import launch_testing.markers +import launch_ros.actions + +import rclpy + +from controller_manager.launch_utils import generate_load_controller_launch_description + + +def get_loaded_controllers(node, timeout=30.0): + """Query /controller_manager/list_controllers until controllers are available.""" + from controller_manager_msgs.srv import ListControllers + import time + + client = node.create_client(ListControllers, "/controller_manager/list_controllers") + + if not client.wait_for_service(timeout_sec=timeout): + raise RuntimeError("Controller manager service not available") + + seen = [] + start_time = time.time() + while time.time() - start_time < timeout: + req = ListControllers.Request() + fut = client.call_async(req) + rclpy.spin_until_future_complete(node, fut, timeout_sec=2.0) + + if fut.done() and fut.result() is not None: + response = fut.result() + seen = [c.name for c in response.controller] + if seen: + break + + time.sleep(0.2) + + return seen + + +@pytest.mark.launch_test +def generate_test_description(): + + # Create temporary directory for all test files + temp_dir = tempfile.mkdtemp() + print(f"Creating test files in: {temp_dir}") + + # Get URDF, without involving xacro + urdf = os.path.join( + get_package_share_directory("ros2_control_test_assets"), + "urdf", + "test_hardware_components.urdf", + ) + with open(urdf) as infp: + robot_description_content = infp.read() + robot_description = {"robot_description": robot_description_content} + + robot_controllers = os.path.join( + get_package_prefix("controller_manager"), "test", "test_controller_load.yaml" + ) + + # Verify both files exist + assert os.path.isfile(robot_controllers), f"Controller config not created: {robot_controllers}" + + robot_state_pub_node = launch_ros.actions.Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[robot_description], + ) + + # ===== START CONTROLLER MANAGER ===== + control_node = launch_ros.actions.Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[robot_controllers], + output="both", + ) + + print(f"Using config file: {robot_controllers}") + + spawner_action = generate_load_controller_launch_description( + controller_name="test_controller_load", + controller_params_file=[robot_controllers], + ) + + ld = LaunchDescription( + [ + robot_state_pub_node, + control_node, + *spawner_action.entities, # Unpack the entities from the returned LaunchDescription + ReadyToTest(), + ] + ) + + return ld, { + "temp_dir": temp_dir, + "controller_name": "test_controller_load", + "urdf_file": urdf, + } + + +class TestControllerLoad(unittest.TestCase): + def test_controller_loaded(self, launch_service, proc_output, controller_name): + # Create a temporary ROS 2 node for calling the service + rclpy.init() + test_node = rclpy.create_node("test_controller_client") + + # Poll the ListControllers service to ensure the target controller is present + try: + loaded_controllers = get_loaded_controllers(test_node, timeout=30.0) + + # CRITICAL ASSERTION: The test passes only if the controller name is in the list + assert ( + controller_name in loaded_controllers + ), f"Controller '{controller_name}' not found. Loaded: {loaded_controllers}" + + finally: + test_node.destroy_node() + rclpy.shutdown() + + 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 TestCleanup: + def test_ros_nodes_exit_cleanly(self, proc_info): + # The control_node should exit cleanly after the whole launch file finishes + launch_testing.asserts.assertExitCodes(proc_info) + + # Ensures the temporary directory is removed after the test is done + def test_cleanup_temp_files(self, temp_dir): + import shutil + + try: + shutil.rmtree(temp_dir) + except Exception as e: + print(f"Cleanup failed for directory {temp_dir}: {e}") diff --git a/controller_manager/test/test_launch_utils_unit.py b/controller_manager/test/test_launch_utils_unit.py new file mode 100644 index 0000000000..9661a21a4d --- /dev/null +++ b/controller_manager/test/test_launch_utils_unit.py @@ -0,0 +1,97 @@ +#!/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. +"""Unit tests for launch_utils.py functions.""" + +import unittest +from launch import LaunchDescription +from launch_ros.actions import Node + +from controller_manager.launch_utils import ( + generate_controllers_spawner_launch_description, + generate_controllers_spawner_launch_description_from_dict, + generate_load_controller_launch_description, +) + + +class TestLaunchUtils(unittest.TestCase): + """ + Test suite for launch_utils.py functions. + + Tests three entry points: + 1. generate_controllers_spawner_launch_description (from list) + 2. generate_controllers_spawner_launch_description_from_dict (from dict) + 3. generate_load_controller_launch_description (load controllers) + """ + + # ------------------------------------------------------------------ + # Helper methods + # ------------------------------------------------------------------ + def _extract_actions(self, result): + """Return a list of launch actions, regardless of type.""" + if isinstance(result, list): + return result + elif hasattr(result, "entities"): # LaunchDescription in newer ROS2 + return list(result.entities) + else: + return [result] + + def _assert_launch_result(self, result, min_len=0): + """Verify that result is list- or LaunchDescription-like.""" + self.assertTrue( + isinstance(result, (list, LaunchDescription)), + f"Unexpected result type: {type(result)}", + ) + actions = self._extract_actions(result) + self.assertGreaterEqual(len(actions), min_len) + for act in actions: + self.assertTrue( + hasattr(act, "execute") or hasattr(act, "visit"), + f"Invalid action type: {type(act)}", + ) + return actions + + # ------------------------------------------------------------------ + # Launch utility tests + # ------------------------------------------------------------------ + def test_generate_controllers_spawner_from_list(self): + controllers = ["test_controller_1", "test_controller_2"] + result = generate_controllers_spawner_launch_description(controllers) + actions = self._assert_launch_result(result, min_len=1) + self.assertTrue(actions is not None and len(actions) > 0) + + def test_generate_controllers_spawner_from_dict(self): + """Ensure dict-based API works with string param files (old-style).""" + controllers = { + "ctrl_A": ["/tmp/dummy.yaml"], + "ctrl_B": ["/tmp/dummy.yaml"], + } + result = generate_controllers_spawner_launch_description_from_dict(controllers) + actions = self._extract_actions(result) + self.assertIsInstance(result, LaunchDescription) + self.assertEqual(len(actions), 3) + self.assertIsInstance(actions[-1], Node) + + def test_generate_load_controller_launch_description(self): + """Test load controller description with valid single string params.""" + controllers = ["test_controller_load"] + result = generate_load_controller_launch_description(controllers) + actions = self._extract_actions(result) + self.assertIsInstance(result, LaunchDescription) + self.assertEqual(len(actions), 3) + self.assertIsInstance(actions[-1], Node) + + +if __name__ == "__main__": + unittest.main() diff --git a/controller_manager/test/test_ros2_control_node_combined.yaml b/controller_manager/test/test_ros2_control_node_combined.yaml new file mode 100644 index 0000000000..a3b7545999 --- /dev/null +++ b/controller_manager/test/test_ros2_control_node_combined.yaml @@ -0,0 +1,22 @@ +controller_manager: + ros__parameters: + update_rate: 100 # Hz + +joint_state_broadcaster: + ros__parameters: + type: "joint_state_broadcaster/JointStateBroadcaster" + +forward_position_controller: + ros__parameters: + type: position_controllers/JointGroupPositionController + joints: ["joint1"] + +ctrl_with_parameters_and_type: + ros__parameters: + type: "controller_manager/test_controller" + joint_names: ["joint1"] + +controller3: + ros__parameters: + type: "controller_manager/test_controller" + joint_names: ["joint3"] From d161825c9a17e5338eb61269ec2b4390f5dd13bb Mon Sep 17 00:00:00 2001 From: Robert Kwan Date: Sun, 9 Nov 2025 11:47:19 -0800 Subject: [PATCH 05/13] Fixed pre-commit error in master branch for #2147 --- controller_manager/test/test_ros2_control_node_launch.py | 1 - 1 file changed, 1 deletion(-) diff --git a/controller_manager/test/test_ros2_control_node_launch.py b/controller_manager/test/test_ros2_control_node_launch.py index bcc1c04a33..46a4f813c0 100644 --- a/controller_manager/test/test_ros2_control_node_launch.py +++ b/controller_manager/test/test_ros2_control_node_launch.py @@ -51,7 +51,6 @@ ) - # Executes the given launch file and checks if all nodes can be started @pytest.mark.launch_test def generate_test_description(): From b36e7eae141d5598dc7474a3b1cae8c983e66635 Mon Sep 17 00:00:00 2001 From: Jordan Palacios Date: Fri, 14 Nov 2025 14:49:17 +0100 Subject: [PATCH 06/13] Improving differential_transmission configure checks (#2812) --- .../differential_transmission.hpp | 44 ++++++++++++++++--- .../test/differential_transmission_test.cpp | 31 +++++++++++++ 2 files changed, 69 insertions(+), 6 deletions(-) diff --git a/transmission_interface/include/transmission_interface/differential_transmission.hpp b/transmission_interface/include/transmission_interface/differential_transmission.hpp index 5dc61dc795..6c7bdb7aea 100644 --- a/transmission_interface/include/transmission_interface/differential_transmission.hpp +++ b/transmission_interface/include/transmission_interface/differential_transmission.hpp @@ -344,42 +344,74 @@ void DifferentialTransmission::configure( get_handles_info())); } - if (joint_position_.size() != actuator_position_.size()) + if ( + !joint_position_.empty() && !actuator_position_.empty() && + joint_position_.size() != actuator_position_.size()) { throw Exception( fmt::format( FMT_COMPILE("Pair-wise mismatch on position interfaces. \n{}"), get_handles_info())); } - if (joint_velocity_.size() != actuator_velocity_.size()) + if ( + !joint_velocity_.empty() && !actuator_velocity_.empty() && + joint_velocity_.size() != actuator_velocity_.size()) { throw Exception( fmt::format( FMT_COMPILE("Pair-wise mismatch on velocity interfaces. \n{}"), get_handles_info())); } - if (joint_effort_.size() != actuator_effort_.size()) + if ( + !joint_effort_.empty() && !actuator_effort_.empty() && + joint_effort_.size() != actuator_effort_.size()) { throw Exception( fmt::format( FMT_COMPILE("Pair-wise mismatch on effort interfaces. \n{}"), get_handles_info())); } - if (joint_torque_.size() != actuator_torque_.size()) + if ( + !joint_torque_.empty() && !actuator_torque_.empty() && + joint_torque_.size() != actuator_torque_.size()) { throw Exception( fmt::format( FMT_COMPILE("Pair-wise mismatch on torque interfaces. \n{}"), get_handles_info())); } - if (joint_force_.size() != actuator_force_.size()) + if ( + !joint_force_.empty() && !actuator_force_.empty() && + joint_force_.size() != actuator_force_.size()) { throw Exception( fmt::format(FMT_COMPILE("Pair-wise mismatch on force interfaces. \n{}"), get_handles_info())); } - if (joint_absolute_position_.size() != actuator_absolute_position_.size()) + if ( + !joint_absolute_position_.empty() && !actuator_absolute_position_.empty() && + joint_absolute_position_.size() != actuator_absolute_position_.size()) { throw Exception( fmt::format( FMT_COMPILE("Pair-wise mismatch on absolute position interfaces. \n{}"), get_handles_info())); } + + // Check at least one pair-wise interface is available + if (!((!joint_position_.empty() && !actuator_position_.empty() && + joint_position_.size() == actuator_position_.size()) || + (!joint_velocity_.empty() && !actuator_velocity_.empty() && + joint_velocity_.size() == actuator_velocity_.size()) || + (!joint_effort_.empty() && !actuator_effort_.empty() && + joint_effort_.size() == actuator_effort_.size()) || + (!joint_torque_.empty() && !actuator_torque_.empty() && + joint_torque_.size() == actuator_torque_.size()) || + (!joint_force_.empty() && !actuator_force_.empty() && + joint_force_.size() == actuator_force_.size()) || + (!joint_absolute_position_.empty() && !actuator_absolute_position_.empty() && + joint_absolute_position_.size() == actuator_absolute_position_.size()))) + { + throw Exception( + fmt::format( + FMT_COMPILE("No pair-wise interface available between joints and actuators. \n{}"), + get_handles_info())); + } } inline void DifferentialTransmission::actuator_to_joint() diff --git a/transmission_interface/test/differential_transmission_test.cpp b/transmission_interface/test/differential_transmission_test.cpp index 96a6b8e002..06dcb78385 100644 --- a/transmission_interface/test/differential_transmission_test.cpp +++ b/transmission_interface/test/differential_transmission_test.cpp @@ -133,6 +133,37 @@ TEST(ConfigureTest, FailsWithBadHandles) testConfigureWithBadHandles(HW_IF_ABSOLUTE_POSITION); } +TEST(ConfigureTest, SuccessWithOneGoodHandle) +{ + DifferentialTransmission trans({1.0, 1.0}, {1.0, 1.0}); + double dummy; + + auto a1_p_handle = ActuatorHandle("act1", HW_IF_POSITION, &dummy); + auto a2_p_handle = ActuatorHandle("act2", HW_IF_POSITION, &dummy); + auto a1_f_handle = ActuatorHandle("act1", HW_IF_FORCE, &dummy); + auto a2_f_handle = ActuatorHandle("act2", HW_IF_FORCE, &dummy); + auto j1_p_handle = JointHandle("joint1", HW_IF_POSITION, &dummy); + auto j2_p_handle = JointHandle("joint2", HW_IF_POSITION, &dummy); + + // No exception should be thrown even though there is no force interface in the joints + ASSERT_NO_THROW(trans.configure( + {j1_p_handle, j2_p_handle}, {a1_p_handle, a2_p_handle, a1_f_handle, a2_f_handle})); +} + +TEST(ConfigureTest, FailWhenGoodHandles) +{ + DifferentialTransmission trans({1.0, 1.0}, {1.0, 1.0}); + double dummy; + + auto a1_f_handle = ActuatorHandle("act1", HW_IF_FORCE, &dummy); + auto a2_f_handle = ActuatorHandle("act2", HW_IF_FORCE, &dummy); + auto j1_p_handle = JointHandle("joint1", HW_IF_POSITION, &dummy); + auto j2_p_handle = JointHandle("joint2", HW_IF_POSITION, &dummy); + + // No pair-wise interfaces available + EXPECT_THROW(trans.configure({j1_p_handle, j2_p_handle}, {a1_f_handle, a2_f_handle}), Exception); +} + class TransmissionSetup : public ::testing::Test { protected: From bae478820a24f8b6360493030f99395f95ab7eea Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Fri, 14 Nov 2025 19:45:49 +0100 Subject: [PATCH 07/13] Use kilted branch for ros2_controllers (#2813) --- .github/workflows/rolling-abi-compatibility.yml | 1 - .github/workflows/rolling-binary-build.yml | 1 - .github/workflows/rolling-debian-build.yml | 1 - .github/workflows/rolling-rhel-binary-build.yml | 1 - .github/workflows/rolling-semi-binary-build.yml | 1 - .github/workflows/rolling-semi-binary-downstream-build.yml | 1 - .github/workflows/rolling-source-build.yml | 1 - ros_controls.kilted.repos | 2 +- 8 files changed, 1 insertion(+), 8 deletions(-) diff --git a/.github/workflows/rolling-abi-compatibility.yml b/.github/workflows/rolling-abi-compatibility.yml index fe865b8e88..a9fb553d49 100644 --- a/.github/workflows/rolling-abi-compatibility.yml +++ b/.github/workflows/rolling-abi-compatibility.yml @@ -14,7 +14,6 @@ on: - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_control-not-released.rolling.repos' - - 'ros2_control-not-released.kilted.repos' concurrency: # cancel previous runs of the same workflow, except for pushes on given branches diff --git a/.github/workflows/rolling-binary-build.yml b/.github/workflows/rolling-binary-build.yml index 05c3e09b97..7d7b19d7c6 100644 --- a/.github/workflows/rolling-binary-build.yml +++ b/.github/workflows/rolling-binary-build.yml @@ -17,7 +17,6 @@ on: - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_control-not-released.rolling.repos' - - 'ros2_control-not-released.kilted.repos' push: *event schedule: # Run every morning to detect flakiness and broken dependencies diff --git a/.github/workflows/rolling-debian-build.yml b/.github/workflows/rolling-debian-build.yml index 01d59bbc28..02dd6037dc 100644 --- a/.github/workflows/rolling-debian-build.yml +++ b/.github/workflows/rolling-debian-build.yml @@ -14,7 +14,6 @@ on: - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_control.rolling.repos' - - 'ros2_control.kilted.repos' schedule: # Run every day to detect flakiness and broken dependencies - cron: '03 1 * * MON-FRI' diff --git a/.github/workflows/rolling-rhel-binary-build.yml b/.github/workflows/rolling-rhel-binary-build.yml index 3d39bd3262..7f06d3f4a2 100644 --- a/.github/workflows/rolling-rhel-binary-build.yml +++ b/.github/workflows/rolling-rhel-binary-build.yml @@ -14,7 +14,6 @@ on: - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_control.rolling.repos' - - 'ros2_control.kilted.repos' schedule: # Run every day to detect flakiness and broken dependencies - cron: '03 1 * * MON-FRI' diff --git a/.github/workflows/rolling-semi-binary-build.yml b/.github/workflows/rolling-semi-binary-build.yml index b8256d39f5..bab72a944b 100644 --- a/.github/workflows/rolling-semi-binary-build.yml +++ b/.github/workflows/rolling-semi-binary-build.yml @@ -17,7 +17,6 @@ on: - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_control.rolling.repos' - - 'ros2_control.kilted.repos' push: *event schedule: # Run every morning to detect flakiness and broken dependencies diff --git a/.github/workflows/rolling-semi-binary-downstream-build.yml b/.github/workflows/rolling-semi-binary-downstream-build.yml index 13735752a9..d185976c7b 100644 --- a/.github/workflows/rolling-semi-binary-downstream-build.yml +++ b/.github/workflows/rolling-semi-binary-downstream-build.yml @@ -17,7 +17,6 @@ on: - '**/package.xml' - '**/CMakeLists.txt' - 'ros_controls.rolling.repos' - - 'ros_controls.kilted.repos' concurrency: group: ${{ github.workflow }}-${{ github.ref }} diff --git a/.github/workflows/rolling-source-build.yml b/.github/workflows/rolling-source-build.yml index 4c07772dcd..7f2cfe7a92 100644 --- a/.github/workflows/rolling-source-build.yml +++ b/.github/workflows/rolling-source-build.yml @@ -14,7 +14,6 @@ on: - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_control.rolling.repos' - - 'ros2_control.kilted.repos' pull_request: branches: - master diff --git a/ros_controls.kilted.repos b/ros_controls.kilted.repos index e6a4eafee1..b196c6acd0 100644 --- a/ros_controls.kilted.repos +++ b/ros_controls.kilted.repos @@ -10,7 +10,7 @@ repositories: ros2_controllers: type: git url: https://github.com/ros-controls/ros2_controllers.git - version: master + version: kilted control_toolbox: type: git url: https://github.com/ros-controls/control_toolbox.git From ef44894466f920c2a6f1423a0484eddea47c1f65 Mon Sep 17 00:00:00 2001 From: Robert Kwan Date: Sat, 15 Nov 2025 02:11:56 -0800 Subject: [PATCH 08/13] Changes - Add tests for launch_utils of controller manager #2147 PR(#2768) --- controller_manager/CMakeLists.txt | 24 ++--------- controller_manager/test/test_controller3.yaml | 8 ---- .../test/test_joint_state_broadcast.yaml | 7 ---- .../test/test_launch_utils/CMakeLists.txt | 42 +++++++++++++++++++ .../test_controller_load.yaml | 0 .../test_launch_utils_integration_dict.py | 41 +++++++++--------- .../test_launch_utils_integration_list.py | 33 ++++++++------- .../test_launch_utils_integration_load.py | 24 +++++------ .../test_launch_utils_unit.py | 0 .../test_ros2_control_node_combined.yaml | 4 +- .../test/test_ros2_control_node_launch.py | 33 +-------------- 11 files changed, 98 insertions(+), 118 deletions(-) delete mode 100644 controller_manager/test/test_controller3.yaml delete mode 100644 controller_manager/test/test_joint_state_broadcast.yaml create mode 100644 controller_manager/test/test_launch_utils/CMakeLists.txt rename controller_manager/test/{ => test_launch_utils}/test_controller_load.yaml (100%) rename controller_manager/test/{ => test_launch_utils}/test_launch_utils_integration_dict.py (89%) rename controller_manager/test/{ => test_launch_utils}/test_launch_utils_integration_list.py (90%) rename controller_manager/test/{ => test_launch_utils}/test_launch_utils_integration_load.py (90%) rename controller_manager/test/{ => test_launch_utils}/test_launch_utils_unit.py (100%) rename controller_manager/test/{ => test_launch_utils}/test_ros2_control_node_combined.yaml (91%) diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index f144b7b482..8f3c811b27 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -248,30 +248,12 @@ if(BUILD_TESTING) find_package(launch_testing_ament_cmake REQUIRED) find_package(rclpy REQUIRED) install(FILES test/test_ros2_control_node.yaml - test/test_controller_load.yaml - test/test_ros2_control_node_combined.yaml 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) - ament_add_pytest_test(test_launch_utils_unit test/test_launch_utils_unit.py - APPEND_ENV AMENT_PREFIX_PATH=${ament_index_build_path} - PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR}:${CMAKE_CURRENT_SOURCE_DIR}:${PYTHONPATH} - ) - # ---------------------------------------------------------------------------- - # Integration tests for individual launch_utils entry points - # ---------------------------------------------------------------------------- - ament_add_pytest_test(test_launch_utils_integration_list - test/test_launch_utils_integration_list.py - TIMEOUT 60 - ) - ament_add_pytest_test(test_launch_utils_integration_dict - test/test_launch_utils_integration_dict.py - TIMEOUT 60 - ) - ament_add_pytest_test(test_launch_utils_integration_load - test/test_launch_utils_integration_load.py - TIMEOUT 60 - ) + + # Now include the launch_utils subfolder + add_subdirectory(test/test_launch_utils) endif() diff --git a/controller_manager/test/test_controller3.yaml b/controller_manager/test/test_controller3.yaml deleted file mode 100644 index 4cb904882f..0000000000 --- a/controller_manager/test/test_controller3.yaml +++ /dev/null @@ -1,8 +0,0 @@ -controller_manager: - ros__parameters: - update_rate: 100 # Hz - -controller3: - ros__parameters: - type: "controller_manager/test_controller" - joint_names: ["joint3"] diff --git a/controller_manager/test/test_joint_state_broadcast.yaml b/controller_manager/test/test_joint_state_broadcast.yaml deleted file mode 100644 index 9001187574..0000000000 --- a/controller_manager/test/test_joint_state_broadcast.yaml +++ /dev/null @@ -1,7 +0,0 @@ -controller_manager: - ros__parameters: - update_rate: 100 # Hz - -joint_state_broadcaster: - ros__parameters: - type: "joint_state_broadcaster/JointStateBroadcaster" diff --git a/controller_manager/test/test_launch_utils/CMakeLists.txt b/controller_manager/test/test_launch_utils/CMakeLists.txt new file mode 100644 index 0000000000..36d6892f05 --- /dev/null +++ b/controller_manager/test/test_launch_utils/CMakeLists.txt @@ -0,0 +1,42 @@ +# 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 YAML test files +# +install( + FILES + test_controller_load.yaml + test_ros2_control_node_combined.yaml + DESTINATION 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 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 + TIMEOUT 60 +) +ament_add_pytest_test(test_launch_utils_integration_dict + test_launch_utils_integration_dict.py + TIMEOUT 60 +) +ament_add_pytest_test(test_launch_utils_integration_load + test_launch_utils_integration_load.py + TIMEOUT 60 +) diff --git a/controller_manager/test/test_controller_load.yaml b/controller_manager/test/test_launch_utils/test_controller_load.yaml similarity index 100% rename from controller_manager/test/test_controller_load.yaml rename to controller_manager/test/test_launch_utils/test_controller_load.yaml diff --git a/controller_manager/test/test_launch_utils_integration_dict.py b/controller_manager/test/test_launch_utils/test_launch_utils_integration_dict.py similarity index 89% rename from controller_manager/test/test_launch_utils_integration_dict.py rename to controller_manager/test/test_launch_utils/test_launch_utils_integration_dict.py index d16391d675..b62b3cf6b3 100644 --- a/controller_manager/test/test_launch_utils_integration_dict.py +++ b/controller_manager/test/test_launch_utils/test_launch_utils_integration_dict.py @@ -15,15 +15,14 @@ import pytest import unittest -import os import tempfile import time +from pathlib import Path from ament_index_python.packages import get_package_share_directory, get_package_prefix from launch import LaunchDescription import launch_testing from launch_testing.actions import ReadyToTest -import launch_testing.markers import launch_ros.actions import rclpy @@ -45,23 +44,27 @@ def generate_test_description(): temp_dir = tempfile.mkdtemp() print(f"Creating test files in: {temp_dir}") - # Get URDF, without involving xacro - urdf = os.path.join( - get_package_share_directory("ros2_control_test_assets"), - "urdf", - "test_hardware_components.urdf", + # URDF path (pathlib version, no xacro) + urdf = ( + Path(get_package_share_directory("ros2_control_test_assets")) + / "urdf" + / "test_hardware_components.urdf" ) + with open(urdf) as infp: robot_description_content = infp.read() robot_description = {"robot_description": robot_description_content} - robot_controllers = os.path.join( - get_package_prefix("controller_manager"), "test", "test_ros2_control_node_combined.yaml" + # Path to combined YAML + robot_controllers = ( + Path(get_package_prefix("controller_manager")) + / "test" + / "test_ros2_control_node_combined.yaml" ) - # Verify both files exist - assert os.path.isfile(robot_controllers), f"Controller config not created: {robot_controllers}" - assert os.path.isfile(urdf), f"URDF not created: {urdf}" + # Verify files exist (Path method) + assert robot_controllers.is_file(), f"Controller config not found: {robot_controllers}" + assert urdf.is_file(), f"URDF not found: {urdf}" robot_state_pub_node = launch_ros.actions.Node( package="robot_state_publisher", @@ -74,16 +77,16 @@ def generate_test_description(): control_node = launch_ros.actions.Node( package="controller_manager", executable="ros2_control_node", - parameters=[robot_controllers], # Use the combined config file + parameters=[str(robot_controllers)], # Use the combined config file output="both", ) # 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": [robot_controllers], - "ctrl_with_parameters_and_type": [robot_controllers], - "controller3": [robot_controllers], + "joint_state_broadcaster": [str(robot_controllers)], + "controller1": [str(robot_controllers)], + "controller2": [str(robot_controllers)], } controller_list = list(ctrl_dict.keys()) @@ -103,7 +106,7 @@ def generate_test_description(): # Return tuple with launch description and test context return ld, { "controller_list": controller_list, # Key name updated to match the test function - "robot_controllers": str(robot_controllers), + "robot_controllers": robot_controllers, "urdf_file": urdf, "temp_dir": temp_dir, } @@ -224,7 +227,7 @@ def test_exit_codes(self, proc_info): print("[POST-SHUTDOWN] ? All processes exited as expected") - def test_cleanup_temp_files(self, temp_dir, robot_controllers, urdf_file): + def test_cleanup_temp_files(self, temp_dir): """Clean up temporary test files.""" import shutil @@ -232,7 +235,7 @@ def test_cleanup_temp_files(self, temp_dir, robot_controllers, urdf_file): # The original clean-up logic was commented out, enabling it for safety try: - if os.path.exists(temp_dir): + if temp_dir.exists(): shutil.rmtree(temp_dir) print("[CLEANUP] ? Temporary files removed") diff --git a/controller_manager/test/test_launch_utils_integration_list.py b/controller_manager/test/test_launch_utils/test_launch_utils_integration_list.py similarity index 90% rename from controller_manager/test/test_launch_utils_integration_list.py rename to controller_manager/test/test_launch_utils/test_launch_utils_integration_list.py index ab49a17ef1..336fa10bef 100644 --- a/controller_manager/test/test_launch_utils_integration_list.py +++ b/controller_manager/test/test_launch_utils/test_launch_utils_integration_list.py @@ -15,15 +15,14 @@ import pytest import unittest -import os import time import tempfile +from pathlib import Path from ament_index_python.packages import get_package_share_directory, get_package_prefix from launch import LaunchDescription import launch_testing from launch_testing.actions import ReadyToTest -import launch_testing.markers import launch_ros.actions import rclpy @@ -44,22 +43,24 @@ def generate_test_description(): print(f"Creating test files in: {temp_dir}") # Get URDF, without involving xacro - urdf = os.path.join( - get_package_share_directory("ros2_control_test_assets"), - "urdf", - "test_hardware_components.urdf", + urdf = ( + Path(get_package_share_directory("ros2_control_test_assets")) + / "urdf" + / "test_hardware_components.urdf" ) with open(urdf) as infp: robot_description_content = infp.read() robot_description = {"robot_description": robot_description_content} - robot_controllers = os.path.join( - get_package_prefix("controller_manager"), "test", "test_ros2_control_node_combined.yaml" + robot_controllers = ( + Path(get_package_prefix("controller_manager")) + / "test" + / "test_ros2_control_node_combined.yaml" ) - # Verify both files exist - assert os.path.isfile(robot_controllers), f"Controller config not created: {robot_controllers}" - assert os.path.isfile(urdf), f"URDF not created: {urdf}" + # Verify files exist + assert robot_controllers.is_file(), f"Controller config missing: {robot_controllers}" + assert urdf.is_file(), f"URDF missing: {urdf}" robot_state_pub_node = launch_ros.actions.Node( package="robot_state_publisher", @@ -72,12 +73,12 @@ def generate_test_description(): control_node = launch_ros.actions.Node( package="controller_manager", executable="ros2_control_node", - parameters=[robot_controllers], + parameters=[str(robot_controllers)], output="both", ) # ===== DEFINE CONTROLLERS TO SPAWN ===== - controller_list = ["joint_state_broadcaster", "ctrl_with_parameters_and_type", "controller3"] + controller_list = ["joint_state_broadcaster", "controller1", "controller2"] # ===== GENERATE SPAWNER ===== print(f"Spawning controllers: {controller_list}") @@ -85,7 +86,7 @@ def generate_test_description(): spawner_ld = generate_controllers_spawner_launch_description( controller_names=controller_list.copy(), - controller_params_files=[robot_controllers], + controller_params_files=[str(robot_controllers)], ) # ===== CREATE LAUNCH DESCRIPTION ===== @@ -217,14 +218,14 @@ def test_exit_codes(self, proc_info): print("[POST-SHUTDOWN] ? All processes exited as expected") - def test_cleanup_temp_files(self, temp_dir, robot_controllers, urdf_file): + def test_cleanup_temp_files(self, temp_dir): """Clean up temporary test files.""" import shutil print(f"\n[CLEANUP] Removing temporary directory: {temp_dir}") try: - if os.path.exists(temp_dir): + if temp_dir.exists(): shutil.rmtree(temp_dir) print("[CLEANUP] ? Temporary files removed") diff --git a/controller_manager/test/test_launch_utils_integration_load.py b/controller_manager/test/test_launch_utils/test_launch_utils_integration_load.py similarity index 90% rename from controller_manager/test/test_launch_utils_integration_load.py rename to controller_manager/test/test_launch_utils/test_launch_utils_integration_load.py index 418342e1e6..b4fc07d680 100644 --- a/controller_manager/test/test_launch_utils_integration_load.py +++ b/controller_manager/test/test_launch_utils/test_launch_utils_integration_load.py @@ -15,15 +15,13 @@ import pytest import unittest -import os import tempfile - +from pathlib import Path from ament_index_python.packages import get_package_share_directory, get_package_prefix from launch import LaunchDescription import launch_testing from launch_testing.actions import ReadyToTest import launch_testing.asserts -import launch_testing.markers import launch_ros.actions import rclpy @@ -67,21 +65,21 @@ def generate_test_description(): print(f"Creating test files in: {temp_dir}") # Get URDF, without involving xacro - urdf = os.path.join( - get_package_share_directory("ros2_control_test_assets"), - "urdf", - "test_hardware_components.urdf", + urdf = ( + Path(get_package_share_directory("ros2_control_test_assets")) + / "urdf" + / "test_hardware_components.urdf" ) with open(urdf) as infp: robot_description_content = infp.read() robot_description = {"robot_description": robot_description_content} - robot_controllers = os.path.join( - get_package_prefix("controller_manager"), "test", "test_controller_load.yaml" + robot_controllers = ( + Path(get_package_prefix("controller_manager")) / "test" / "test_controller_load.yaml" ) # Verify both files exist - assert os.path.isfile(robot_controllers), f"Controller config not created: {robot_controllers}" + assert robot_controllers.is_file(), f"Controller config not created: {robot_controllers}" robot_state_pub_node = launch_ros.actions.Node( package="robot_state_publisher", @@ -94,7 +92,7 @@ def generate_test_description(): control_node = launch_ros.actions.Node( package="controller_manager", executable="ros2_control_node", - parameters=[robot_controllers], + parameters=[str(robot_controllers)], output="both", ) @@ -102,7 +100,7 @@ def generate_test_description(): spawner_action = generate_load_controller_launch_description( controller_name="test_controller_load", - controller_params_file=[robot_controllers], + controller_params_file=[str(robot_controllers)], ) ld = LaunchDescription( @@ -117,7 +115,7 @@ def generate_test_description(): return ld, { "temp_dir": temp_dir, "controller_name": "test_controller_load", - "urdf_file": urdf, + "urdf_file": str(urdf), } diff --git a/controller_manager/test/test_launch_utils_unit.py b/controller_manager/test/test_launch_utils/test_launch_utils_unit.py similarity index 100% rename from controller_manager/test/test_launch_utils_unit.py rename to controller_manager/test/test_launch_utils/test_launch_utils_unit.py diff --git a/controller_manager/test/test_ros2_control_node_combined.yaml b/controller_manager/test/test_launch_utils/test_ros2_control_node_combined.yaml similarity index 91% rename from controller_manager/test/test_ros2_control_node_combined.yaml rename to controller_manager/test/test_launch_utils/test_ros2_control_node_combined.yaml index a3b7545999..2ae3abcfa5 100644 --- a/controller_manager/test/test_ros2_control_node_combined.yaml +++ b/controller_manager/test/test_launch_utils/test_ros2_control_node_combined.yaml @@ -11,12 +11,12 @@ forward_position_controller: type: position_controllers/JointGroupPositionController joints: ["joint1"] -ctrl_with_parameters_and_type: +controller1: ros__parameters: type: "controller_manager/test_controller" joint_names: ["joint1"] -controller3: +controller2: ros__parameters: type: "controller_manager/test_controller" joint_names: ["joint3"] diff --git a/controller_manager/test/test_ros2_control_node_launch.py b/controller_manager/test/test_ros2_control_node_launch.py index 46a4f813c0..a8defcf80e 100644 --- a/controller_manager/test/test_ros2_control_node_launch.py +++ b/controller_manager/test/test_ros2_control_node_launch.py @@ -37,7 +37,7 @@ from launch_testing.actions import ReadyToTest import launch_testing.markers import launch_ros.actions -from launch_ros.actions import Node + import rclpy from controller_manager.test_utils import ( @@ -50,7 +50,6 @@ generate_load_controller_launch_description, ) - # Executes the given launch file and checks if all nodes can be started @pytest.mark.launch_test def generate_test_description(): @@ -134,36 +133,6 @@ def _assert_launch_result(self, result, min_len=0): ) return actions - # ------------------------------------------------------------------ - # Launch utility tests - # ------------------------------------------------------------------ - def test_generate_controllers_spawner_from_list(self): - controllers = ["test_controller_1", "test_controller_2"] - result = generate_controllers_spawner_launch_description(controllers) - actions = self._assert_launch_result(result, min_len=1) - self.assertTrue(actions is not None and len(actions) > 0) - - def test_generate_controllers_spawner_from_dict(self): - """Ensure dict-based API works with string param files (old-style).""" - controllers = { - "ctrl_A": ["/tmp/dummy.yaml"], - "ctrl_B": ["/tmp/dummy.yaml"], - } - result = generate_controllers_spawner_launch_description_from_dict(controllers) - actions = self._extract_actions(result) - self.assertIsInstance(result, LaunchDescription) - self.assertEqual(len(actions), 3) - self.assertIsInstance(actions[-1], Node) - - def test_generate_load_controller_launch_description(self): - """Test load controller description with valid single string params.""" - controllers = ["test_controller_load"] - result = generate_load_controller_launch_description(controllers) - actions = self._extract_actions(result) - self.assertIsInstance(result, LaunchDescription) - self.assertEqual(len(actions), 3) - self.assertIsInstance(actions[-1], Node) - def test_node_start(self): check_node_running(self.node, "controller_manager") From e5768bacf27db5923a678cc6cac14abffcf69a7b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Noel=20Jim=C3=A9nez=20Garc=C3=ADa?= Date: Wed, 19 Nov 2025 12:15:53 +0100 Subject: [PATCH 09/13] Use tinyxml2 package instead of deprecated tinyxml2_vendor (#2833) --- hardware_interface/CMakeLists.txt | 2 -- hardware_interface/package.xml | 2 +- 2 files changed, 1 insertion(+), 3 deletions(-) diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index e10f7ec16f..a696e605d6 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -14,7 +14,6 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS rcutils realtime_tools TinyXML2 - tinyxml2_vendor joint_limits urdf pal_statistics @@ -50,7 +49,6 @@ target_link_libraries(hardware_interface PUBLIC rcpputils::rcpputils ${joint_limits_TARGETS} ${TinyXML2_LIBRARIES} - ${tinyxml2_vendor_LIBRARIES} ${pal_statistics_LIBRARIES} ${control_msgs_TARGETS} ${lifecycle_msgs_TARGETS} diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index 6d643c20d1..153e737f7f 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -23,7 +23,7 @@ rcpputils realtime_tools sdformat_urdf - tinyxml2_vendor + tinyxml2 urdf fmt From 70a8ecafe13fc9677ecfdaf5154e80ee3b3b0aea Mon Sep 17 00:00:00 2001 From: James Cowsert <148249109+J-Cowsert@users.noreply.github.com> Date: Wed, 19 Nov 2025 05:21:35 -0600 Subject: [PATCH 10/13] Avoid C++20 structured binding capture (#2832) --- hardware_interface/src/resource_manager.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 0d300eed05..d2128dceb3 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -1003,8 +1003,10 @@ class ResourceStorage { if (interface) { - for (auto & [hw_name, limiters] : joint_limiters_interface_) + for (auto & entry : joint_limiters_interface_) { + auto & limiters = entry.second; + // If the prefix is a joint name, then bind the limiter to the command interface if (limiters.find(interface->get_prefix_name()) != limiters.end()) { From f5c420f7bd53dd2c4d9cd61916ed350a2d5ab065 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 19 Nov 2025 21:13:58 +0100 Subject: [PATCH 11/13] Fix dependencies of controller_manager (#2836) --- controller_manager/CMakeLists.txt | 7 ++++--- controller_manager/package.xml | 29 +++++++++++++++-------------- 2 files changed, 19 insertions(+), 17 deletions(-) diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index 8f3c811b27..ac254288f4 100644 --- a/controller_manager/CMakeLists.txt +++ b/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) diff --git a/controller_manager/package.xml b/controller_manager/package.xml index d514b8cce2..8945530afa 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -19,34 +19,35 @@ controller_interface controller_manager_msgs diagnostic_updater + fmt + generate_parameter_library hardware_interface - launch - launch_ros + libstatistics_collector + lifecycle_msgs pluginlib rclcpp - rcpputils realtime_tools - ros2_control_test_assets - ros2param - ros2run std_msgs - libstatistics_collector - generate_parameter_library - fmt + + launch_ros + launch_testing_ros + launch python3-filelock + python3-yaml + rcl_interfaces + rclpy + ros2param + sensor_msgs ament_cmake_gmock ament_cmake_pytest + example_interfaces hardware_interface_testing - launch_testing_ros launch_testing - launch python3-coverage - rclpy robot_state_publisher ros2_control_test_assets - sensor_msgs - example_interfaces + ros2pkg ament_cmake From b786d535676775ccd1fd4012867f71b23116e07b Mon Sep 17 00:00:00 2001 From: Dhruv Patel Date: Fri, 21 Nov 2025 01:31:37 +0100 Subject: [PATCH 12/13] Use fmt for correct int64_t format specifier across platforms (#2817) --------- Signed-off-by: Dhruv Patel Co-authored-by: Sai Kishor Kothakota Co-authored-by: Sai Kishor Kothakota --- .../src/controller_interface_base.cpp | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index 50bef315f1..94779f2c0b 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/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 { From d078b5d36ea03cae5375d53520878bca60c070be Mon Sep 17 00:00:00 2001 From: Robert Kwan Date: Fri, 21 Nov 2025 02:05:30 -0800 Subject: [PATCH 13/13] Modify tests for launch_utils of controller manager (#2147) --- controller_manager/CMakeLists.txt | 2 - .../test/test_launch_utils/CMakeLists.txt | 15 +- .../test_launch_utils_integration_dict.py | 205 +++++------------ .../test_launch_utils_integration_list.py | 210 ++++++------------ .../test_launch_utils_integration_load.py | 145 ++++++------ .../test_ros2_control_node_combined.yaml | 24 +- 6 files changed, 215 insertions(+), 386 deletions(-) diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index ac254288f4..1b935c20f3 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -246,8 +246,6 @@ if(BUILD_TESTING) ) find_package(ament_cmake_pytest REQUIRED) - find_package(launch_testing_ament_cmake REQUIRED) - find_package(rclpy REQUIRED) install(FILES test/test_ros2_control_node.yaml DESTINATION test) ament_add_pytest_test(test_ros2_control_node test/test_ros2_control_node_launch.py) diff --git a/controller_manager/test/test_launch_utils/CMakeLists.txt b/controller_manager/test/test_launch_utils/CMakeLists.txt index 36d6892f05..3c2aa465b5 100644 --- a/controller_manager/test/test_launch_utils/CMakeLists.txt +++ b/controller_manager/test/test_launch_utils/CMakeLists.txt @@ -4,13 +4,21 @@ 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 test + DESTINATION share/${PROJECT_NAME}/test ) # Make sure test files get installed into the test tree @@ -20,7 +28,7 @@ install( test_launch_utils_integration_list.py test_launch_utils_integration_dict.py test_launch_utils_integration_load.py - DESTINATION test + DESTINATION share/${PROJECT_NAME}/test ) # Register each test with ament @@ -30,13 +38,10 @@ ament_add_pytest_test(test_launch_utils_unit test_launch_utils_unit.py ) ament_add_pytest_test(test_launch_utils_integration_list test_launch_utils_integration_list.py - TIMEOUT 60 ) ament_add_pytest_test(test_launch_utils_integration_dict test_launch_utils_integration_dict.py - TIMEOUT 60 ) ament_add_pytest_test(test_launch_utils_integration_load test_launch_utils_integration_load.py - TIMEOUT 60 ) diff --git a/controller_manager/test/test_launch_utils/test_launch_utils_integration_dict.py b/controller_manager/test/test_launch_utils/test_launch_utils_integration_dict.py index b62b3cf6b3..75dfe1f95a 100644 --- a/controller_manager/test/test_launch_utils/test_launch_utils_integration_dict.py +++ b/controller_manager/test/test_launch_utils/test_launch_utils_integration_dict.py @@ -15,18 +15,18 @@ import pytest import unittest -import tempfile -import time -from pathlib import Path - -from ament_index_python.packages import get_package_share_directory, get_package_prefix 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, ) @@ -40,76 +40,69 @@ def generate_test_description(): THIS VERSION CREATES ALL NEEDED FILES DYNAMICALLY AND USES THE COMBINED CONFIG. """ - # Create temporary directory for all test files - temp_dir = tempfile.mkdtemp() - print(f"Creating test files in: {temp_dir}") - # URDF path (pathlib version, no xacro) - urdf = ( - Path(get_package_share_directory("ros2_control_test_assets")) + urdf_subst = ( + PathSubstitution(FindPackageShare("ros2_control_test_assets")) / "urdf" / "test_hardware_components.urdf" ) - with open(urdf) as infp: + 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 = ( - Path(get_package_prefix("controller_manager")) + robot_controllers_subst = ( + PathSubstitution(FindPackageShare("controller_manager")) / "test" / "test_ros2_control_node_combined.yaml" ) - # Verify files exist (Path method) - assert robot_controllers.is_file(), f"Controller config not found: {robot_controllers}" - assert urdf.is_file(), f"URDF not found: {urdf}" - - robot_state_pub_node = launch_ros.actions.Node( - package="robot_state_publisher", - executable="robot_state_publisher", - output="both", - parameters=[robot_description], - ) - - # ===== START CONTROLLER MANAGER (ros2_control_node) ===== - control_node = launch_ros.actions.Node( - package="controller_manager", - executable="ros2_control_node", - parameters=[str(robot_controllers)], # Use the combined config file - output="both", - ) + 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": [str(robot_controllers)], - "controller1": [str(robot_controllers)], - "controller2": [str(robot_controllers)], + "joint_state_broadcaster": [], + "controller1": [], + "controller2": [], } controller_list = list(ctrl_dict.keys()) # ===== GENERATE SPAWNER LAUNCH DESCRIPTION ===== print(f"Spawning controllers: {controller_list}") - # Correct function name and call - spawner_ld = generate_controllers_spawner_launch_description_from_dict( - controller_info_dict=ctrl_dict, - ) - # ===== CREATE LAUNCH DESCRIPTION ===== - ld = LaunchDescription( - [robot_state_pub_node, control_node, ReadyToTest()] + spawner_ld.entities - ) - - # Return tuple with launch description and test context - return ld, { - "controller_list": controller_list, # Key name updated to match the test function - "robot_controllers": robot_controllers, - "urdf_file": urdf, - "temp_dir": temp_dir, - } + 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 @@ -124,65 +117,18 @@ def setUpClass(cls): 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("\n[TEST] Active processes: {process_names}") - - def test_controllers_loaded(self, proc_info, controller_list): - """Test that controllers were loaded (poll until they appear).""" - node = rclpy.create_node("test_controller_query_node") - - try: - from controller_manager_msgs.srv import ListControllers - - client = node.create_client(ListControllers, "/controller_manager/list_controllers") - - print("\n[TEST] Waiting for controller_manager service...") - wait_for_svc_timeout = 30.0 - if not client.wait_for_service(timeout_sec=wait_for_svc_timeout): - process_names = proc_info.process_names() - self.fail( - f"Controller manager service not available after {wait_for_svc_timeout}s.\n" - f"Active processes: {process_names}" - ) - - # Poll for controllers to be registered - print("[TEST] Service available, polling for controllers (timeout 30s)...") - deadline = node.get_clock().now() + rclpy.duration.Duration(seconds=30.0) - seen = [] - while node.get_clock().now() < deadline: - req = ListControllers.Request() - fut = client.call_async(req) - rclpy.spin_until_future_complete(node, fut, timeout_sec=2.0) - if fut.done() and fut.result() is not None: - response = fut.result() - seen = [c.name for c in response.controller] - if all(ctrl in seen for ctrl in controller_list): - print(f"[TEST] Loaded controllers: {seen}") - break - # small sleep to avoid tight-loop - time.sleep(0.2) - else: - # timeout expired - self.fail( - f"Timeout waiting for controllers to be loaded. " - f"Expected: {controller_list}, saw: {seen}" - ) - - # Final assert (defensive) - for controller in controller_list: - self.assertIn( - controller, - seen, - f"Controller '{controller}' was not loaded. Available: {seen}", - ) - - print(f"[TEST] ? All {len(controller_list)} controllers loaded successfully") - - finally: - node.destroy_node() + 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).""" @@ -192,7 +138,7 @@ def test_spawner_exit_code(self, proc_info): # 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") + print(f"[TEST] Launch has {len(process_names)} active processes") @launch_testing.post_shutdown_test() @@ -201,43 +147,8 @@ class TestProcessOutput(unittest.TestCase): def test_exit_codes(self, proc_info): """Verify all processes exited successfully.""" - print("\n[POST-SHUTDOWN] Process exit codes:") - for process_name in proc_info.process_names(): - info = proc_info[process_name] - print(f" {process_name}: {info.returncode}") - - for process_name in proc_info.process_names(): - info = proc_info[process_name] - - if "ros2_control_node" in process_name: - self.assertEqual( - info.returncode, 0, f"{process_name} exited with {info.returncode}" - ) - elif "spawner" in process_name: - # Spawner should complete successfully (0) or be terminated - self.assertIn( - info.returncode, - [0, -2, -15], - f"Spawner {process_name} exited with {info.returncode}", - ) - else: - self.assertIn( - info.returncode, [0, -2, -15], f"{process_name} exited with {info.returncode}" - ) - - print("[POST-SHUTDOWN] ? All processes exited as expected") - - def test_cleanup_temp_files(self, temp_dir): - """Clean up temporary test files.""" - import shutil - - print(f"\n[CLEANUP] Removing temporary directory: {temp_dir}") - - # The original clean-up logic was commented out, enabling it for safety - try: - if temp_dir.exists(): - shutil.rmtree(temp_dir) - - print("[CLEANUP] ? Temporary files removed") - except Exception as e: - print(f"[CLEANUP] Warning: Cleanup failed: {e}") + launch_testing.asserts.assertExitCodes( + proc_info, + # All other processes (ros2_control_node, etc.) must exit cleanly (0) + allowable_exit_codes=[0, 1, -2, -15], + ) diff --git a/controller_manager/test/test_launch_utils/test_launch_utils_integration_list.py b/controller_manager/test/test_launch_utils/test_launch_utils_integration_list.py index 336fa10bef..03fce12480 100644 --- a/controller_manager/test/test_launch_utils/test_launch_utils_integration_list.py +++ b/controller_manager/test/test_launch_utils/test_launch_utils_integration_list.py @@ -15,18 +15,18 @@ import pytest import unittest -import time -import tempfile -from pathlib import Path - -from ament_index_python.packages import get_package_share_directory, get_package_prefix from launch import LaunchDescription import launch_testing from launch_testing.actions import ReadyToTest import launch_ros.actions +from launch.substitutions import PathSubstitution, PathJoinSubstitution +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, ) @@ -38,69 +38,72 @@ def generate_test_description(): Generate launch description for testing. """ - # Create temporary directory for all test files - temp_dir = tempfile.mkdtemp() - print(f"Creating test files in: {temp_dir}") - - # Get URDF, without involving xacro - urdf = ( - Path(get_package_share_directory("ros2_control_test_assets")) + # URDF path (pathlib version, no xacro) + urdf_subst = ( + PathSubstitution(FindPackageShare("ros2_control_test_assets")) / "urdf" / "test_hardware_components.urdf" ) - with open(urdf) as infp: - robot_description_content = infp.read() - robot_description = {"robot_description": robot_description_content} - robot_controllers = ( - Path(get_package_prefix("controller_manager")) - / "test" - / "test_ros2_control_node_combined.yaml" - ) + context = LaunchContext() - # Verify files exist - assert robot_controllers.is_file(), f"Controller config missing: {robot_controllers}" - assert urdf.is_file(), f"URDF missing: {urdf}" + urdf_path_str = urdf_subst.perform(context) - robot_state_pub_node = launch_ros.actions.Node( - package="robot_state_publisher", - executable="robot_state_publisher", - output="both", - parameters=[robot_description], - ) + # 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} - # ===== START CONTROLLER MANAGER ===== - control_node = launch_ros.actions.Node( - package="controller_manager", - executable="ros2_control_node", - parameters=[str(robot_controllers)], - output="both", + # Path to combined YAML + # robot_controllers = ( + # PathSubstitution(FindPackageShare("controller_manager")) + # / "test" + # / "test_ros2_control_node_combined.yaml" + # ) + + robot_controllers_subst = PathJoinSubstitution( + [ + 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) + # ===== DEFINE CONTROLLERS TO SPAWN ===== controller_list = ["joint_state_broadcaster", "controller1", "controller2"] # ===== GENERATE SPAWNER ===== print(f"Spawning controllers: {controller_list}") - print(f"Using config file: {robot_controllers}") - - spawner_ld = generate_controllers_spawner_launch_description( - controller_names=controller_list.copy(), - controller_params_files=[str(robot_controllers)], - ) # ===== CREATE LAUNCH DESCRIPTION ===== - ld = LaunchDescription( - [robot_state_pub_node, control_node, ReadyToTest()] + spawner_ld.entities - ) - - # Return tuple with test context - return ld, { - "controller_list": controller_list, - "robot_controllers": robot_controllers, - "urdf_file": urdf, - "temp_dir": temp_dir, - } + 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( + controller_names=controller_list.copy(), + controller_params_files=[robot_controllers_path], + ), + ReadyToTest(), + ] + ), {"controller_list": controller_list} # Active tests @@ -115,65 +118,18 @@ def setUpClass(cls): 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_loaded(self, proc_info, controller_list): - """Test that controllers were loaded (poll until they appear).""" - node = rclpy.create_node("test_controller_query_node") - - try: - from controller_manager_msgs.srv import ListControllers - - client = node.create_client(ListControllers, "/controller_manager/list_controllers") - - print("\n[TEST] Waiting for controller_manager service...") - wait_for_svc_timeout = 30.0 - if not client.wait_for_service(timeout_sec=wait_for_svc_timeout): - process_names = proc_info.process_names() - self.fail( - f"Controller manager service not available after {wait_for_svc_timeout}s.\n" - f"Active processes: {process_names}" - ) - - # Poll for controllers to be registered - print("[TEST] Service available, polling for controllers (timeout 30s)...") - deadline = node.get_clock().now() + rclpy.duration.Duration(seconds=30.0) - seen = [] - while node.get_clock().now() < deadline: - req = ListControllers.Request() - fut = client.call_async(req) - rclpy.spin_until_future_complete(node, fut, timeout_sec=2.0) - if fut.done() and fut.result() is not None: - response = fut.result() - seen = [c.name for c in response.controller] - if all(ctrl in seen for ctrl in controller_list): - print(f"[TEST] Loaded controllers: {seen}") - break - # small sleep to avoid tight-loop - time.sleep(0.2) - else: - # timeout expired - self.fail( - f"Timeout waiting for controllers to be loaded. " - f"Expected: {controller_list}, saw: {seen}" - ) - - # Final assert (defensive) - for controller in controller_list: - self.assertIn( - controller, - seen, - f"Controller '{controller}' was not loaded. Available: {seen}", - ) - - print(f"[TEST] ? All {len(controller_list)} controllers loaded successfully") - - finally: - node.destroy_node() + 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).""" @@ -192,42 +148,8 @@ class TestProcessOutput(unittest.TestCase): def test_exit_codes(self, proc_info): """Verify all processes exited successfully.""" - print("\n[POST-SHUTDOWN] Process exit codes:") - for process_name in proc_info.process_names(): - info = proc_info[process_name] - print(f" {process_name}: {info.returncode}") - - for process_name in proc_info.process_names(): - info = proc_info[process_name] - - if "ros2_control_node" in process_name: - self.assertEqual( - info.returncode, 0, f"{process_name} exited with {info.returncode}" - ) - elif "spawner" in process_name: - # Spawner should complete successfully (0) or be terminated - self.assertIn( - info.returncode, - [0, -2, -15], - f"Spawner {process_name} exited with {info.returncode}", - ) - else: - self.assertIn( - info.returncode, [0, -2, -15], f"{process_name} exited with {info.returncode}" - ) - - print("[POST-SHUTDOWN] ? All processes exited as expected") - - def test_cleanup_temp_files(self, temp_dir): - """Clean up temporary test files.""" - import shutil - - print(f"\n[CLEANUP] Removing temporary directory: {temp_dir}") - - try: - if temp_dir.exists(): - shutil.rmtree(temp_dir) - - print("[CLEANUP] ? Temporary files removed") - except Exception as e: - print(f"[CLEANUP] Warning: Cleanup failed: {e}") + launch_testing.asserts.assertExitCodes( + proc_info, + # All other processes (ros2_control_node, etc.) must exit cleanly (0) + allowable_exit_codes=[0, 1, -15], + ) diff --git a/controller_manager/test/test_launch_utils/test_launch_utils_integration_load.py b/controller_manager/test/test_launch_utils/test_launch_utils_integration_load.py index b4fc07d680..1b61e63c0e 100644 --- a/controller_manager/test/test_launch_utils/test_launch_utils_integration_load.py +++ b/controller_manager/test/test_launch_utils/test_launch_utils_integration_load.py @@ -15,14 +15,16 @@ import pytest import unittest -import tempfile -from pathlib import Path -from ament_index_python.packages import get_package_share_directory, get_package_prefix from launch import LaunchDescription import launch_testing from launch_testing.actions import ReadyToTest -import launch_testing.asserts + +# import launch_testing.asserts import launch_ros.actions +from launch.substitutions import PathSubstitution +from launch_ros.substitutions import FindPackageShare +from launch.launch_context import LaunchContext + import rclpy @@ -60,83 +62,85 @@ def get_loaded_controllers(node, timeout=30.0): @pytest.mark.launch_test def generate_test_description(): - # Create temporary directory for all test files - temp_dir = tempfile.mkdtemp() - print(f"Creating test files in: {temp_dir}") - - # Get URDF, without involving xacro - urdf = ( - Path(get_package_share_directory("ros2_control_test_assets")) + # URDF path (pathlib version, no xacro) + urdf_subst = ( + PathSubstitution(FindPackageShare("ros2_control_test_assets")) / "urdf" / "test_hardware_components.urdf" ) - with open(urdf) as infp: - robot_description_content = infp.read() - robot_description = {"robot_description": robot_description_content} - robot_controllers = ( - Path(get_package_prefix("controller_manager")) / "test" / "test_controller_load.yaml" - ) + context = LaunchContext() - # Verify both files exist - assert robot_controllers.is_file(), f"Controller config not created: {robot_controllers}" + urdf_path_str = urdf_subst.perform(context) - robot_state_pub_node = launch_ros.actions.Node( - package="robot_state_publisher", - executable="robot_state_publisher", - output="both", - parameters=[robot_description], - ) + # 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} - # ===== START CONTROLLER MANAGER ===== - control_node = launch_ros.actions.Node( - package="controller_manager", - executable="ros2_control_node", - parameters=[str(robot_controllers)], - output="both", + # Path to combined YAML + robot_controllers = ( + PathSubstitution(FindPackageShare("controller_manager")) + / "test" + / "test_ros2_control_node_combined.yaml" ) - print(f"Using config file: {robot_controllers}") + context = LaunchContext() + robot_controllers_path = robot_controllers.perform(context) - spawner_action = generate_load_controller_launch_description( - controller_name="test_controller_load", - controller_params_file=[str(robot_controllers)], - ) + print("Resolved controller YAML:", robot_controllers_path) - ld = LaunchDescription( + return LaunchDescription( [ - robot_state_pub_node, - control_node, - *spawner_action.entities, # Unpack the entities from the returned 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_load_controller_launch_description( + controller_name="controller1", + controller_params_file=[robot_controllers_path], + ), ReadyToTest(), ] - ) - - return ld, { - "temp_dir": temp_dir, - "controller_name": "test_controller_load", - "urdf_file": str(urdf), + ), { + "controller_name": "controller1", } class TestControllerLoad(unittest.TestCase): - def test_controller_loaded(self, launch_service, proc_output, controller_name): - # Create a temporary ROS 2 node for calling the service + + @classmethod + def setUpClass(cls): rclpy.init() - test_node = rclpy.create_node("test_controller_client") - # Poll the ListControllers service to ensure the target controller is present - try: - loaded_controllers = get_loaded_controllers(test_node, timeout=30.0) + @classmethod + def tearDownClass(cls): + rclpy.shutdown() + + def setUp(self): + self.node = rclpy.create_node("test_controller_client") - # CRITICAL ASSERTION: The test passes only if the controller name is in the list - assert ( - controller_name in loaded_controllers - ), f"Controller '{controller_name}' not found. Loaded: {loaded_controllers}" + def test_controller_loaded(self, launch_service, proc_output, controller_name): + + # Poll the ListControllers service to ensure the target controller is present + loaded_controllers = get_loaded_controllers(self.node, timeout=30.0) - finally: - test_node.destroy_node() - rclpy.shutdown() + # CRITICAL ASSERTION: The test passes only if the controller name is in the list + assert ( + controller_name in loaded_controllers + ), f"Controller '{controller_name}' not found. Loaded: {loaded_controllers}" def test_spawner_exit_code(self, proc_info): """Test that spawner process ran (may have completed already).""" @@ -150,16 +154,13 @@ def test_spawner_exit_code(self, proc_info): @launch_testing.post_shutdown_test() -class TestCleanup: - def test_ros_nodes_exit_cleanly(self, proc_info): - # The control_node should exit cleanly after the whole launch file finishes - launch_testing.asserts.assertExitCodes(proc_info) - - # Ensures the temporary directory is removed after the test is done - def test_cleanup_temp_files(self, temp_dir): - import shutil - - try: - shutil.rmtree(temp_dir) - except Exception as e: - print(f"Cleanup failed for directory {temp_dir}: {e}") +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], + ) diff --git a/controller_manager/test/test_launch_utils/test_ros2_control_node_combined.yaml b/controller_manager/test/test_launch_utils/test_ros2_control_node_combined.yaml index 2ae3abcfa5..56f3eefdae 100644 --- a/controller_manager/test/test_launch_utils/test_ros2_control_node_combined.yaml +++ b/controller_manager/test/test_launch_utils/test_ros2_control_node_combined.yaml @@ -2,21 +2,13 @@ controller_manager: ros__parameters: update_rate: 100 # Hz -joint_state_broadcaster: - ros__parameters: - type: "joint_state_broadcaster/JointStateBroadcaster" + joint_state_broadcaster: + type: "joint_state_broadcaster/JointStateBroadcaster" -forward_position_controller: - ros__parameters: - type: position_controllers/JointGroupPositionController - joints: ["joint1"] + controller1: + type: "controller_manager/test_controller" + joint_names: ["joint1"] -controller1: - ros__parameters: - type: "controller_manager/test_controller" - joint_names: ["joint1"] - -controller2: - ros__parameters: - type: "controller_manager/test_controller" - joint_names: ["joint3"] + controller2: + type: "controller_manager/test_controller" + joint_names: ["joint2"]