Activate necessary controllers

Author: urfeex

ur_robot_driver/test/integration_test_trajectory_until.py

@@ -105,11 +105,14 @@ def setUp(self):
     # Tests
     #
 
-    def test_trajectory_with_tool_contact_no_trigger_succeeds(self, tf_prefix):
+    def test_trajectory_with_tool_contact_no_trigger_succeeds(
+        self, tf_prefix, initial_joint_controller
+    ):
+        self._controller_manager_interface.wait_for_controller(initial_joint_controller)
         self.assertTrue(
             self._controller_manager_interface.switch_controller(
                 strictness=SwitchController.Request.BEST_EFFORT,
-                activate_controllers=["tool_contact_controller"],
+                activate_controllers=["tool_contact_controller", initial_joint_controller],
             ).ok
         )
         trajectory = JointTrajectory()
@@ -133,18 +136,13 @@ def test_trajectory_with_tool_contact_no_trigger_succeeds(self, tf_prefix):
         self.assertEqual(
             result.until_condition_result, FollowJointTrajectoryUntil.Result.NOT_TRIGGERED
         )
-        self.assertTrue(
-            self._controller_manager_interface.switch_controller(
-                strictness=SwitchController.Request.BEST_EFFORT,
-                deactivate_controllers=["tool_contact_controller"],
-            ).ok
-        )
 
-    def test_trajectory_until_can_cancel(self, tf_prefix):
+    def test_trajectory_until_can_cancel(self, tf_prefix, initial_joint_controller):
+        self._controller_manager_interface.wait_for_controller(initial_joint_controller)
         self.assertTrue(
             self._controller_manager_interface.switch_controller(
                 strictness=SwitchController.Request.BEST_EFFORT,
-                activate_controllers=["tool_contact_controller"],
+                activate_controllers=["tool_contact_controller", initial_joint_controller],
             ).ok
         )
         trajectory = JointTrajectory()
@@ -162,9 +160,3 @@ def test_trajectory_until_can_cancel(self, tf_prefix):
         self.assertTrue(goal_handle.accepted)
         result = self._trajectory_until_interface.cancel_goal(goal_handle)
         self.assertEqual(result.return_code, CancelGoal.ERROR_NONE)
-        self.assertTrue(
-            self._controller_manager_interface.switch_controller(
-                strictness=SwitchController.Request.BEST_EFFORT,
-                deactivate_controllers=["tool_contact_controller"],
-            ).ok
-        )