Reduce flakiness of trajectory controller tests (#1443)

* Set minimal velocity tolerance in passthrough trajectory test

When running a quintic spline with the passthrough trajectory test there
can be a minimal velocity deviation at the end of the trajectory due to
how the robot interpolates motions. With a velocity tolerance of 0.0 there
is a chance that we hit this tolerance.

* Explicitly switch on controllers in test where they are needed

(cherry picked from commit 1cd3ac5)

Author: urfeex

ur_robot_driver/test/robot_driver.py

@@ -187,6 +187,13 @@ def io_msg_cb(msg):
 
     def test_trajectory(self, tf_prefix):
         """Test robot movement."""
+        self.assertTrue(
+            self._controller_manager_interface.switch_controller(
+                strictness=SwitchController.Request.BEST_EFFORT,
+                deactivate_controllers=["passthrough_trajectory_controller"],
+                activate_controllers=["scaled_joint_trajectory_controller"],
+            ).ok
+        )
         # Construct test trajectory
         test_trajectory = [
             (Duration(sec=6, nanosec=0), [0.0 for j in ROBOT_JOINTS]),
@@ -219,6 +226,13 @@ def test_illegal_trajectory(self, tf_prefix):
 
         This is more of a validation test that the testing suite does the right thing
         """
+        self.assertTrue(
+            self._controller_manager_interface.switch_controller(
+                strictness=SwitchController.Request.BEST_EFFORT,
+                deactivate_controllers=["passthrough_trajectory_controller"],
+                activate_controllers=["scaled_joint_trajectory_controller"],
+            ).ok
+        )
         # Construct test trajectory, the second point wrongly starts before the first
         test_trajectory = [
             (Duration(sec=6, nanosec=0), [0.0 for j in ROBOT_JOINTS]),
@@ -244,6 +258,13 @@ def test_illegal_trajectory(self, tf_prefix):
 
     def test_trajectory_scaled(self, tf_prefix):
         """Test robot movement."""
+        self.assertTrue(
+            self._controller_manager_interface.switch_controller(
+                strictness=SwitchController.Request.BEST_EFFORT,
+                deactivate_controllers=["passthrough_trajectory_controller"],
+                activate_controllers=["scaled_joint_trajectory_controller"],
+            ).ok
+        )
         # Construct test trajectory
         test_trajectory = [
             (Duration(sec=6, nanosec=0), [0.0 for j in ROBOT_JOINTS]),
@@ -369,7 +390,7 @@ def test_passthrough_trajectory(self, tf_prefix):
             Duration(sec=12, nanosec=0),
         ]
         goal_tolerance = [
-            JointTolerance(position=0.01, name=tf_prefix + ROBOT_JOINTS[i])
+            JointTolerance(position=0.01, velocity=5e-5, name=tf_prefix + ROBOT_JOINTS[i])
             for i in range(len(ROBOT_JOINTS))
         ]
         goal_time_tolerance = Duration(sec=1, nanosec=0)