Wait for used controllers in test setup (UniversalRobots#1519)

* Wait for used controllers in test setup

Otherwise initial controller switches may fail

* Fix typo

Author: urfeex

ur_robot_driver/test/integration_test_force_mode.py

@@ -114,6 +114,10 @@ def init_robot(self):
             FollowJointTrajectory,
         )
 
+        self._controller_manager_interface.wait_for_controller("force_mode_controller")
+        self._controller_manager_interface.wait_for_controller("scaled_joint_trajectory_controller")
+        self._controller_manager_interface.wait_for_controller("joint_trajectory_controller")
+
     def setUp(self):
         self._dashboard_interface.start_robot()
         time.sleep(1)

ur_robot_driver/test/integration_test_passthrough_controller.py

@@ -107,6 +107,10 @@ def init_robot(self):
             FollowJointTrajectory,
         )
 
+        # Wait for all controllers needed below, as controller manager services might fail
+        # e.g. when attempting to deactivate an unknown controller
+        self._controller_manager_interface.wait_for_controller("scaled_joint_trajectory_controller")
+
     def setUp(self):
         self._dashboard_interface.start_robot()
         time.sleep(1)

ur_robot_driver/test/integration_test_scaled_joint_controller.py

@@ -88,6 +88,10 @@ def init_robot(self):
             FollowJointTrajectory,
         )
 
+        # Wait for all controllers needed below, as controller manager services might fail
+        # e.g. when attempting to deactivate an unknown controller
+        self._controller_manager_interface.wait_for_controller("passthrough_trajectory_controller")
+
     def setUp(self):
         self._dashboard_interface.start_robot()
         time.sleep(1)