Fix flaky controller switch test

We were starting the tests right after we started the driver. However,
the test requires that all controllers are actually available. This
commit adds a check whether all required controller are actually loaded.

If not all required controllers are found in a specific time, the test
will throw an exception and therefore fail.

Author: urfeex

ur_robot_driver/test/integration_test_controller_switch.py

@@ -48,6 +48,16 @@
     generate_driver_test_description,
 )
 
+ALL_CONTROLLERS = [
+    "scaled_joint_trajectory_controller",
+    "joint_trajectory_controller",
+    "forward_position_controller",
+    "forward_velocity_controller",
+    "passthrough_trajectory_controller",
+    "force_mode_controller",
+    "freedrive_mode_controller",
+]
+
 
 @pytest.mark.launch_test
 @launch_testing.parametrize(
@@ -78,6 +88,9 @@ def init_robot(self):
         self._dashboard_interface = DashboardInterface(self.node)
         self._controller_manager_interface = ControllerManagerInterface(self.node)
         self._io_status_controller_interface = IoStatusInterface(self.node)
+        self._controller_manager_interface.wait_for_service(timeout_sec=30.0)
+        for controller in ALL_CONTROLLERS:
+            self._controller_manager_interface.wait_for_controller(controller)
 
     def setUp(self):
         self._dashboard_interface.start_robot()
@@ -89,15 +102,7 @@ def test_activating_multiple_controllers_same_interface_fails(self):
         self.assertTrue(
             self._controller_manager_interface.switch_controller(
                 strictness=SwitchController.Request.BEST_EFFORT,
-                deactivate_controllers=[
-                    "scaled_joint_trajectory_controller",
-                    "joint_trajectory_controller",
-                    "forward_position_controller",
-                    "forward_velocity_controller",
-                    "passthrough_trajectory_controller",
-                    "force_mode_controller",
-                    "freedrive_mode_controller",
-                ],
+                deactivate_controllers=ALL_CONTROLLERS,
             ).ok
         )
 
@@ -126,15 +131,7 @@ def test_activating_multiple_controllers_different_interface_fails(self):
         self.assertTrue(
             self._controller_manager_interface.switch_controller(
                 strictness=SwitchController.Request.BEST_EFFORT,
-                deactivate_controllers=[
-                    "scaled_joint_trajectory_controller",
-                    "joint_trajectory_controller",
-                    "forward_position_controller",
-                    "forward_velocity_controller",
-                    "force_mode_controller",
-                    "passthrough_trajectory_controller",
-                    "freedrive_mode_controller",
-                ],
+                deactivate_controllers=ALL_CONTROLLERS,
             ).ok
         )
         self.assertFalse(
@@ -261,7 +258,9 @@ def test_activating_controller_with_running_position_controller_fails(self):
             ).ok
         )
 
-    def test_activating_controller_with_running_passthrough_trajectory_controller_fails(self):
+    def test_activating_controller_with_running_passthrough_trajectory_controller_fails(
+        self,
+    ):
         # Having a position-based controller active, no other controller should be able to
         # activate.
         self.assertTrue(
@@ -332,14 +331,7 @@ def test_force_mode_and_trajectory_passthrough_controller_are_compatible(self):
         self.assertTrue(
             self._controller_manager_interface.switch_controller(
                 strictness=SwitchController.Request.BEST_EFFORT,
-                deactivate_controllers=[
-                    "scaled_joint_trajectory_controller",
-                    "joint_trajectory_controller",
-                    "forward_position_controller",
-                    "forward_velocity_controller",
-                    "passthrough_trajectory_controller",
-                    "force_mode_controller",
-                ],
+                deactivate_controllers=ALL_CONTROLLERS,
             ).ok
         )
 
@@ -411,15 +403,7 @@ def test_tool_contact_compatibility(self):
         self.assertTrue(
             self._controller_manager_interface.switch_controller(
                 strictness=SwitchController.Request.BEST_EFFORT,
-                deactivate_controllers=[
-                    "scaled_joint_trajectory_controller",
-                    "joint_trajectory_controller",
-                    "forward_position_controller",
-                    "forward_velocity_controller",
-                    "passthrough_trajectory_controller",
-                    "force_mode_controller",
-                    "tool_contact_controller",
-                ],
+                deactivate_controllers=ALL_CONTROLLERS,
             ).ok
         )
 

ur_robot_driver/test/test_common.py

@@ -256,14 +256,19 @@ class ControllerManagerInterface(
     },
     services={"list_controllers": ListControllers},
 ):
-    def wait_for_controller(self, controller_name, target_state="active"):
-        while True:
+    def wait_for_controller(self, controller_name, target_state=None, timeout=TIMEOUT_WAIT_SERVICE):
+        start_time = time.time()
+        while time.time() - start_time < timeout:
             controllers = self.list_controllers().controller
             for controller in controllers:
-                if (controller.name == controller_name) and (controller.state == target_state):
-                    return
-
+                if controller.name == controller_name:
+                    if (target_state is None) or (controller.state == target_state):
+                        return
             time.sleep(1)
+        raise Exception(
+            "Controller '%s' not found or not in state '%s' within %fs"
+            % (controller_name, target_state, timeout)
+        )
 
 
 class IoStatusInterface(

ur_robot_driver/test/urscript_interface.py

@@ -81,7 +81,9 @@ def setUp(self):
         time.sleep(1)
         self.assertTrue(self._io_status_controller_interface.resend_robot_program().success)
 
-        self._controller_manager_interface.wait_for_controller("io_and_status_controller")
+        self._controller_manager_interface.wait_for_controller(
+            "io_and_status_controller", target_state="active"
+        )
 
     def test_set_io(self):
         """Test setting an IO using a direct program call."""