From c84c492db887d93f01d0d978293d2d4ba13be2c0 Mon Sep 17 00:00:00 2001
From: Stefan Scherzinger <scherzin@fzi.de>
Date: Tue, 9 Jan 2024 17:13:24 +0100
Subject: [PATCH] Update integration tests for handling *NaN* inputs

---
 .../integration_tests/integration_tests.py    | 72 +++++++++----------
 1 file changed, 35 insertions(+), 37 deletions(-)

diff --git a/cartesian_controller_tests/integration_tests/integration_tests.py b/cartesian_controller_tests/integration_tests/integration_tests.py
index 8b9dbcae..67277d0b 100755
--- a/cartesian_controller_tests/integration_tests/integration_tests.py
+++ b/cartesian_controller_tests/integration_tests/integration_tests.py
@@ -155,46 +155,44 @@ def test_inputs_with_nans(self):
         The controllers' callbacks store them even when in inactive state after startup.
         We then check if every controller can be activated normally.
         """
-        # Target pose
-        target_pose = PoseStamped()
-        target_pose.header.frame_id = "base_link"
-        target_pose.pose.position.x = float("nan")
-        target_pose.pose.position.y = float("nan")
-        target_pose.pose.position.z = float("nan")
-        target_pose.pose.orientation.x = float("nan")
-        target_pose.pose.orientation.y = float("nan")
-        target_pose.pose.orientation.z = float("nan")
-        target_pose.pose.orientation.w = float("nan")
-        self.target_pose_pub.publish(target_pose)
-
-        # Force-torque sensor
-        ft_sensor_wrench = WrenchStamped()
-        ft_sensor_wrench.wrench.force.x = float("nan")
-        ft_sensor_wrench.wrench.force.y = float("nan")
-        ft_sensor_wrench.wrench.force.z = float("nan")
-        ft_sensor_wrench.wrench.torque.x = float("nan")
-        ft_sensor_wrench.wrench.torque.y = float("nan")
-        ft_sensor_wrench.wrench.torque.z = float("nan")
-        self.ft_sensor_wrench_pub.publish(ft_sensor_wrench)
-
-        # Target wrench
-        target_wrench = WrenchStamped()
-        target_wrench.wrench.force.x = float("nan")
-        target_wrench.wrench.force.y = float("nan")
-        target_wrench.wrench.force.z = float("nan")
-        target_wrench.wrench.torque.x = float("nan")
-        target_wrench.wrench.torque.y = float("nan")
-        target_wrench.wrench.torque.z = float("nan")
-        self.target_wrench_pub.publish(target_wrench)
-
-        time.sleep(1)  # give the controllers some time to process
+        def publish_nan_targets():
+            # Target pose
+            target_pose = PoseStamped()
+            target_pose.header.frame_id = "base_link"
+            target_pose.pose.position.x = float('nan')
+            target_pose.pose.position.y = float('nan')
+            target_pose.pose.position.z = float('nan')
+            target_pose.pose.orientation.x = float('nan')
+            target_pose.pose.orientation.y = float('nan')
+            target_pose.pose.orientation.z = float('nan')
+            target_pose.pose.orientation.w = float('nan')
+            self.target_pose_pub.publish(target_pose)
+
+            # Force-torque sensor
+            ft_sensor_wrench = WrenchStamped()
+            ft_sensor_wrench.wrench.force.x = float('nan')
+            ft_sensor_wrench.wrench.force.y = float('nan')
+            ft_sensor_wrench.wrench.force.z = float('nan')
+            ft_sensor_wrench.wrench.torque.x = float('nan')
+            ft_sensor_wrench.wrench.torque.y = float('nan')
+            ft_sensor_wrench.wrench.torque.z = float('nan')
+            self.ft_sensor_wrench_pub.publish(ft_sensor_wrench)
+
+            # Target wrench
+            target_wrench = WrenchStamped()
+            target_wrench.wrench.force.x = float('nan')
+            target_wrench.wrench.force.y = float('nan')
+            target_wrench.wrench.force.z = float('nan')
+            target_wrench.wrench.torque.x = float('nan')
+            target_wrench.wrench.torque.y = float('nan')
+            target_wrench.wrench.torque.z = float('nan')
+            self.target_wrench_pub.publish(target_wrench)
 
         for name in self.our_controllers:
             self.start_controller(name)
-            time.sleep(3)  # process some update() cycles
-            self.assertTrue(
-                self.check_state(name, "active"), f"{name} survives inputs with NaNs"
-            )
+            publish_nan_targets()
+            time.sleep(3) # process some update() cycles
+            self.assertTrue(self.check_state(name, 'active'), f"{name} survives inputs with NaNs")
             self.stop_controller(name)
 
     def check_state(self, controller, state):