Implement new timeout in rotation test

Author: civerachb-cpr

clearpath_tests/clearpath_tests/rotation_test.py

@@ -28,13 +28,13 @@
 # POSSIBILITY OF SUCH DAMAGE.
 
 from clearpath_config.common.types.platform import Platform
-from clearpath_generator_common.common import BaseGenerator
 from clearpath_tests.mobility_test import MobilityTestNode
 from clearpath_tests.test_node import ClearpathTestResult
 from clearpath_tests.tf import ConfigurableTransformListener
+from clearpath_tests.timer import Timeout
+
 from geometry_msgs.msg import Vector3Stamped
 import rclpy
-from rclpy.duration import Duration
 from rclpy.qos import qos_profile_sensor_data
 from sensor_msgs.msg import Imu
 from tf2_geometry_msgs import do_transform_vector3
@@ -140,14 +140,14 @@ def run_test(self):
         self.start()
 
         # wait until we get the first IMU message or 10s passes
-        start_time = self.get_clock().now()
-        timeout_duration = Duration(seconds=10)
+        timeout = Timeout(self, 10.0)
         while (
             self.latest_imu is None
-            and self.get_clock().now() - start_time <= timeout_duration
+            and not timeout.elapsed
             and not self.test_error
         ):
-            rclpy.spin_once(self)
+            rclpy.spin_once(self, timeout_sec=1.0)
+        timeout.abort()
 
         if self.test_error:
             self.get_logger().warning(f'Test aborted due to an error: {self.test_error_msg}')
@@ -162,13 +162,14 @@ def run_test(self):
 
         # start turning, but wait 1s for us to get up to speed  before recording data
         self.cmd_vel.twist.angular.z = self.max_speed
-        startup_wait = Duration(seconds=1.0)
-        start_time = self.get_clock().now()
+        timeout = Timeout(self, 1.0)
         while (
             not self.test_error
-            and self.get_clock().now() - start_time <= startup_wait
+            and not timeout.elapsed
         ):
-            rclpy.spin_once(self)
+            rclpy.spin_once(self, timeout_sec=1.0)
+        timeout.abort()
+
         if self.test_error:
             self.record_data = False
             self.cmd_vel.twist.angular.z = 0.0
@@ -177,14 +178,15 @@ def run_test(self):
 
         # record data for 10s
         self.record_data = True
-        test_wait = Duration(seconds=10)
-        start_time = self.get_clock().now()
+        timeout = Timeout(self, 10.0)
         while (
             not self.test_error
-            and self.get_clock().now() - start_time <= test_wait
+            and not timeout.elapsed
         ):
-            rclpy.spin_once(self)
+            rclpy.spin_once(self, timeout_sec=1.0)
+        timeout.abort()
         self.record_data = False
+
         if self.test_error:
             self.cmd_vel.twist.angular.z = 0.0
             self.get_logger().warning(f'Test aborted due to an error: {self.test_error_msg}')
@@ -193,10 +195,10 @@ def run_test(self):
         # stop driving
         # wait 1s to ensure we publish the command
         self.cmd_vel.twist.angular.z = 0.0
-        test_wait = Duration(seconds=1)
-        start_time = self.get_clock().now()
-        while self.get_clock().now() - start_time <= test_wait:
-            rclpy.spin_once(self)
+        timeout = Timeout(self, 1.0)
+        while not timeout.elapsed:
+            rclpy.spin_once(self, timeout_sec=1.0)
+        timeout.abort()
 
         # process the results
         results = self.test_results