|
37 | 37 | from launch_testing.actions import ReadyToTest |
38 | 38 | import launch_testing.markers |
39 | 39 | import launch_ros.actions |
| 40 | +from launch_ros.actions import Node |
40 | 41 |
|
41 | 42 | import rclpy |
42 | 43 | from controller_manager.test_utils import ( |
|
46 | 47 | from controller_manager.launch_utils import generate_controllers_spawner_launch_description |
47 | 48 |
|
48 | 49 |
|
| 50 | + |
49 | 51 | # Executes the given launch file and checks if all nodes can be started |
50 | 52 | @pytest.mark.launch_test |
51 | 53 | def generate_test_description(): |
@@ -102,6 +104,63 @@ def setUp(self): |
102 | 104 | def tearDown(self): |
103 | 105 | self.node.destroy_node() |
104 | 106 |
|
| 107 | + # ------------------------------------------------------------------ |
| 108 | + # Helper methods |
| 109 | + # ------------------------------------------------------------------ |
| 110 | + def _extract_actions(self, result): |
| 111 | + """Return a list of launch actions, regardless of type.""" |
| 112 | + if isinstance(result, list): |
| 113 | + return result |
| 114 | + elif hasattr(result, "entities"): # LaunchDescription in newer ROS2 |
| 115 | + return list(result.entities) |
| 116 | + else: |
| 117 | + return [result] |
| 118 | + |
| 119 | + def _assert_launch_result(self, result, min_len=0): |
| 120 | + """Verify that result is list- or LaunchDescription-like.""" |
| 121 | + self.assertTrue( |
| 122 | + isinstance(result, (list, LaunchDescription)), |
| 123 | + f"Unexpected result type: {type(result)}", |
| 124 | + ) |
| 125 | + actions = self._extract_actions(result) |
| 126 | + self.assertGreaterEqual(len(actions), min_len) |
| 127 | + for act in actions: |
| 128 | + self.assertTrue( |
| 129 | + hasattr(act, "execute") or hasattr(act, "visit"), |
| 130 | + f"Invalid action type: {type(act)}", |
| 131 | + ) |
| 132 | + return actions |
| 133 | + |
| 134 | + # ------------------------------------------------------------------ |
| 135 | + # Launch utility tests |
| 136 | + # ------------------------------------------------------------------ |
| 137 | + def test_generate_controllers_spawner_from_list(self): |
| 138 | + controllers = ["test_controller_1", "test_controller_2"] |
| 139 | + result = generate_controllers_spawner_launch_description(controllers) |
| 140 | + actions = self._assert_launch_result(result, min_len=1) |
| 141 | + self.assertTrue(actions is not None and len(actions) > 0) |
| 142 | + |
| 143 | + def test_generate_controllers_spawner_from_dict(self): |
| 144 | + """Ensure dict-based API works with string param files (old-style).""" |
| 145 | + controllers = { |
| 146 | + "ctrl_A": ["/tmp/dummy.yaml"], |
| 147 | + "ctrl_B": ["/tmp/dummy.yaml"], |
| 148 | + } |
| 149 | + result = generate_controllers_spawner_launch_description_from_dict(controllers) |
| 150 | + actions = self._extract_actions(result) |
| 151 | + self.assertIsInstance(result, LaunchDescription) |
| 152 | + self.assertEqual(len(actions), 3) |
| 153 | + self.assertIsInstance(actions[-1], Node) |
| 154 | + |
| 155 | + def test_generate_load_controller_launch_description(self): |
| 156 | + """Test load controller description with valid single string params.""" |
| 157 | + controllers = ["test_controller_load"] |
| 158 | + result = generate_load_controller_launch_description(controllers) |
| 159 | + actions = self._extract_actions(result) |
| 160 | + self.assertIsInstance(result, LaunchDescription) |
| 161 | + self.assertEqual(len(actions), 3) |
| 162 | + self.assertIsInstance(actions[-1], Node) |
| 163 | + |
105 | 164 | def test_node_start(self): |
106 | 165 | check_node_running(self.node, "controller_manager") |
107 | 166 |
|
|
0 commit comments