Fix clearpath_tests so they don't crash/hang under certain conditions

clearpath_tests/clearpath_tests/canbus_test.py

@@ -29,11 +29,8 @@
 import re
 import subprocess
 
-from clearpath_generator_common.common import BaseGenerator
 from clearpath_tests.test_node import ClearpathTestNode, ClearpathTestResult
 
-import rclpy
-
 
 class CanbusTestNode(ClearpathTestNode):
     """Check the output of candump and count the number if IDs."""
@@ -68,15 +65,6 @@ def __init__(
 
         self.test_name = f'CAN {self.can_interface}'
 
-    def start(self):
-        while True:
-            result = self.read_can_log()
-
-            if result.success:
-                self.get_logger().info(result.message)
-            else:
-                self.get_logger().warning(result.message)
-
     def run_test(self):
         self.get_logger().info(f'Collecting CAN messages on {self.can_interface}...')
         result = self.read_can_log()
@@ -178,23 +166,3 @@ def get_test_result_details(self):
 
         details += '\nDevices that do not use CANopen may be incorrectly identified in the list above'  # noqa: E501
         return details
-
-
-def main():
-    setup_path = BaseGenerator.get_args()
-    rclpy.init()
-
-    ct = CanbusTestNode(setup_path=setup_path)
-
-    try:
-        ct.start()
-        rclpy.spin(ct)
-    except KeyboardInterrupt:
-        pass
-
-    ct.destroy_node()
-    rclpy.shutdown()
-
-
-if __name__ == '__main__':
-    main()

clearpath_tests/clearpath_tests/diagnostic_test.py

@@ -29,14 +29,13 @@
 import re
 
 from clearpath_config.common.types.platform import Platform
-from clearpath_generator_common.common import BaseGenerator
 from clearpath_tests.test_node import ClearpathTestNode, ClearpathTestResult
+from clearpath_tests.timer import Timeout
 
 from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus
 
 import rclpy
 from rclpy.qos import qos_profile_system_default
-from rclpy.time import Duration
 
 
 PLATFORM_ANY = '*'
@@ -188,27 +187,21 @@ def diagnostic_callback(self, diagnostic_array):
             elif status.level == DiagnosticStatus.STALE:
                 pass
 
-    def start(self):
-        self.diagnostc_sub = self.create_subscription(
-            DiagnosticArray,
-            f'/{self.namespace}/diagnostics',
-            self.diagnostic_callback,
-            qos_profile_system_default
-        )
-
     def run_test(self):
         results = []
-
         self.test_in_progress = True
-        self.start()
 
         # collect 30s worth of data
-        start_time = self.get_clock().now()
-        end_time = start_time + Duration(seconds=30.0)
-
         self.get_logger().info('Collecting 30 seconds of diagnostic data...')
-        while self.get_clock().now() < end_time:
-            rclpy.spin_once(self)
+        self.diagnostc_sub = self.create_subscription(
+            DiagnosticArray,
+            f'/{self.namespace}/diagnostics',
+            self.diagnostic_callback,
+            qos_profile_system_default
+        )
+        timeout = Timeout(self, 30)
+        while not timeout.elapsed:
+            rclpy.spin_once(self, timeout_sec=1.0)
 
         if len(self.warnings) == 0 and len(self.errors) == 0 and len(self.allowed_errors) == 0:
             results.append(ClearpathTestResult(True, 'Diagnostics', 'No errors, no warnings'))
@@ -257,23 +250,3 @@ def get_test_result_details(self):
                 details += f'* {warn.name}: {warn.message}\n'
 
         return details
-
-
-def main():
-    setup_path = BaseGenerator.get_args()
-    rclpy.init()
-
-    dt = DiagnosticTestNode(setup_path)
-
-    try:
-        dt.start()
-        rclpy.spin(dt)
-    except KeyboardInterrupt:
-        pass
-
-    dt.destroy_node()
-    rclpy.shutdown()
-
-
-if __name__ == '__main__':
-    main()

clearpath_tests/clearpath_tests/drive_test.py

@@ -28,8 +28,6 @@
 # POSSIBILITY OF SUCH DAMAGE.
 import math
 
-from clearpath_generator_common.common import BaseGenerator
-
 from clearpath_tests.mobility_test import MobilityTestNode
 from clearpath_tests.test_node import ClearpathTestResult
 
@@ -185,28 +183,3 @@ def run_test(self):
                 ))
 
         return results
-
-
-def main():
-    setup_path = BaseGenerator.get_args()
-    rclpy.init()
-
-    try:
-        dt = DriveTestNode(setup_path)
-        dt.start()
-        try:
-            while not dt.test_done:
-                rclpy.spin_once(dt)
-            dt.get_logger().info('Test complete')
-        except KeyboardInterrupt:
-            dt.get_logger().info('User aborted! Cleaning up & exiting...')
-        dt.destroy_node()
-    except TimeoutError:
-        # This error is already logged when it's raised
-        pass
-
-    rclpy.shutdown()
-
-
-if __name__ == '__main__':
-    main()

clearpath_tests/clearpath_tests/estop_test.py

@@ -29,12 +29,11 @@
 import threading
 import time
 
-from clearpath_generator_common.common import BaseGenerator
 from clearpath_tests.test_node import ClearpathTestNode, ClearpathTestResult
+from clearpath_tests.timer import Timeout
 
 from geometry_msgs.msg import TwistStamped
 import rclpy
-from rclpy.duration import Duration
 from rclpy.qos import qos_profile_sensor_data, qos_profile_system_default
 from std_msgs.msg import Bool
 
@@ -72,14 +71,6 @@ def estop_callback(self, msg: Bool):
                 state = 'clear'
             self.get_logger().info(f'{self.estop_type} state: {state}')
 
-    def start(self):
-        self.estop_sub = self.create_subscription(
-            Bool,
-            f'/{self.namespace}/platform/emergency_stop',
-            self.estop_callback,
-            qos_profile_sensor_data
-        )
-
     def run_test(self):
         if self.optional:
             user_input = self.promptYN(
@@ -90,7 +81,13 @@ def run_test(self):
                 return [ClearpathTestResult(None, self.test_name, 'Skipped; component not installed')]  # noqa: E501
 
         self.test_in_progress = True
-        self.start()
+
+        self.estop_sub = self.create_subscription(
+            Bool,
+            f'/{self.namespace}/platform/emergency_stop',
+            self.estop_callback,
+            qos_profile_sensor_data
+        )
 
         self.cmd_vel_pub = self.create_publisher(
             TwistStamped,
@@ -105,7 +102,7 @@ def run_test(self):
         ui_thread = threading.Thread(target=self.run_ui)
         ui_thread.start()
         while not self.test_done:
-            rclpy.spin_once(self)
+            rclpy.spin_once(self, timeout_sec=1.0)
         ui_thread.join()
         return self.results
 
@@ -133,14 +130,14 @@ def run_ui(self):
             return
 
         # wait until we know the state of the e-stop
-        start_time = self.get_clock().now()
-        timeout = Duration(seconds=10)
+        timeout = Timeout(self, 10.0)
         print(f'Getting {self.estop_type} status...')
         while (
-            self.estop_engaged is None and
-            (self.get_clock().now() - start_time) < timeout
+            self.estop_engaged is None
+            and not timeout.elapsed
         ):
             time.sleep(0.1)
+        timeout.abort()
 
         if self.estop_engaged is None:
             results.append(ClearpathTestResult(
@@ -240,9 +237,8 @@ def run_ui(self):
     def command_wheels(self):
         self.cmd_vel.twist.linear.x = 0.1
 
-        start_time = self.get_clock().now()
-        duration = Duration(seconds=2)
-        while (self.get_clock().now() - start_time) < duration:
+        timeout = Timeout(self, 2.0)
+        while not timeout.elapsed:
             pass
 
         self.cmd_vel.twist.linear.x = 0.0
@@ -256,33 +252,12 @@ def wait_for_estop(self, state, timeout_seconds=10):
 
         @return  True if the e-stop state is in the desired state, otherwise False
         """
-        start_time = self.get_clock().now()
-        now = self.get_clock().now()
-        timeout = Duration(seconds=timeout_seconds)
+        timeout = Timeout(self, timeout_seconds)
         while (
-            self.estop_engaged != state and
-            (now - start_time) < timeout
+            self.estop_engaged != state
+            and not timeout.elapsed
         ):
-            now = self.get_clock().now()
+            pass
+        timeout.abort()
 
         return self.estop_engaged == state
-
-
-def main():
-    setup_path = BaseGenerator.get_args()
-    rclpy.init()
-
-    st = EstopTestNode('', setup_path=setup_path)
-
-    try:
-        st.start()
-        rclpy.spin(st)
-    except KeyboardInterrupt:
-        pass
-
-    st.destroy_node()
-    rclpy.shutdown()
-
-
-if __name__ == '__main__':
-    main()

clearpath_tests/clearpath_tests/fan_test.py

@@ -26,12 +26,11 @@
 # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 # POSSIBILITY OF SUCH DAMAGE.
-from clearpath_generator_common.common import BaseGenerator
 from clearpath_platform_msgs.msg import Fans, Status
 from clearpath_tests.test_node import ClearpathTestNode, ClearpathTestResult
+from clearpath_tests.timer import Timeout
 
 import rclpy
-from rclpy.duration import Duration
 from rclpy.qos import qos_profile_sensor_data
 
 
@@ -65,10 +64,10 @@ def check_firmware_version(self):
         """
         self.mcu_status = None
 
-        start_at = self.get_clock().now()
-        timeout_duration = Duration(seconds=10)
-        while self.get_clock().now() - start_at <= timeout_duration and self.mcu_status is None:
-            rclpy.spin_once(self)
+        timeout = Timeout(self, 10.0)
+        while not timeout.elapsed and self.mcu_status is None:
+            rclpy.spin_once(self, timeout_sec=1.0)
+        timeout.abort()
 
         if self.mcu_status is None:
             return (None, False)
@@ -134,10 +133,9 @@ def run_test(self):
 
         def wait_3s():
             self.get_logger().info('Waiting for fans to spin up/down...')
-            start = self.get_clock().now()
-            sleep_time = Duration(seconds=3)
-            while self.get_clock().now() - start < sleep_time:
-                rclpy.spin_once(self)
+            timeout = Timeout(self, 3.0)
+            while not timeout.elapsed:
+                rclpy.spin_once(self, timeout_sec=1.0)
 
         for i in range(self.n_fans):
             self.fan_msg.fans[i] = 0
@@ -172,22 +170,3 @@ def wait_3s():
             results.append(ClearpathTestResult(False, 'Fans (all on)', None))
 
         return results
-
-
-def main():
-    setup_path = BaseGenerator.get_args()
-    rclpy.init()
-
-    fan_test = FanTestNode(setup_path)
-
-    try:
-        rclpy.spin(fan_test)
-    except KeyboardInterrupt:
-        pass
-
-    fan_test.destroy_node()
-    rclpy.shutdown()
-
-
-if __name__ == '__main__':
-    main()