fix_flaky_force_mode_test (#1429)

Wait for lookup before attempting it

(cherry picked from commit 0d5d77e)

# Conflicts:
#	ur_robot_driver/test/integration_test_force_mode.py

Author: urfeex

ur_robot_driver/test/integration_test_force_mode.py

@@ -120,18 +120,30 @@ def setUp(self):
         self.assertTrue(self._io_status_controller_interface.resend_robot_program().success)
         self.tf_buffer = Buffer()
         self.tf_listener = TransformListener(self.tf_buffer, self.node)
-
-    def lookup_tcp_in_base(self, tf_prefix, timepoint):
-        trans = None
-        while not trans:
-            rclpy.spin_once(self.node)
+        time.sleep(1)  # Wait whether the controller stopper resets controllers
+
+    def wait_for_lookup(self, source, target, timepoint, timeout=5.0):
+        """
+        Wait until the transform between source and target is available.
+
+        :param source: The source frame
+        :param target: The target frame
+        :param timeout: The point in time at which to make the lookup
+        :param timeout: Timeout in seconds
+        :return: transform between source and target at the given timepoint
+        :raises TimeoutError: If the transform is not available within the timeout
+        """
+        end_time = time.time() + timeout
+        while time.time() < end_time:
             try:
-                trans = self.tf_buffer.lookup_transform(
-                    tf_prefix + "base", tf_prefix + "tool0", timepoint
-                )
+                trans = self.tf_buffer.lookup_transform(source, target, timepoint)
+                return trans
             except TransformException:
-                pass
-        return trans
+                rclpy.spin_once(self.node)
+        raise TimeoutError()
+
+    def lookup_tcp_in_base(self, tf_prefix, timepoint):
+        return self.wait_for_lookup(tf_prefix + "base", tf_prefix + "tool0_controller", timepoint)
 
     # Implementation of force mode test to be reused
     # todo: If we move to pytest this could be done using parametrization
@@ -201,6 +213,9 @@ def run_force_mode(self, tf_prefix):
             ),
             header=trans_after.header,
         )
+        self.wait_for_lookup(
+            diff.header.frame_id, tf_prefix + "tool0_controller", diff.header.stamp
+        )
         diff_in_tool0_controller = self.tf_buffer.transform(
             diff,
             tf_prefix + "tool0_controller",
@@ -519,7 +534,6 @@ def test_deactivating_controller_stops_force_mode(self, tf_prefix):
                 ],
             ).ok
         )
-        self._force_mode_controller_interface = ForceModeInterface(self.node)
 
         time.sleep(0.5)
         trans_before_wait = self.lookup_tcp_in_base(tf_prefix, self.node.get_clock().now())
@@ -529,7 +543,14 @@ def test_deactivating_controller_stops_force_mode(self, tf_prefix):
         trans_after_wait = self.lookup_tcp_in_base(tf_prefix, self.node.get_clock().now())
 
         self.assertAlmostEqual(
+<<<<<<< HEAD
             trans_before_wait.transform.translation.z, trans_after_wait.transform.translation.z
+=======
+            trans_before_wait.transform.translation.z,
+            trans_after_wait.transform.translation.z,
+            delta=1e-7,
+            msg="Robot should not move after force mode is stopped",
+>>>>>>> 0d5d77e (fix_flaky_force_mode_test (#1429))
         )
 
     def test_params_out_of_range_fails(self, tf_prefix):