Improve stability of the tests (#352)

* Improve test stability.

---------

Signed-off-by: Jelmer de Wolde <jelmer.de.wolde@alliander.com>

Author: Jelmerdw

ros2_ws/src/rcdt_franka/src/gripper.cpp

@@ -59,6 +59,14 @@ void Gripper::open(
   auto goal = Move::Goal();
   goal.width = 0.08;
   goal.speed = 0.03;
+
+  auto server_ready =
+      client_move->wait_for_action_server(std::chrono::seconds(timeout));
+  if (!server_ready) {
+    RCLCPP_ERROR(this->get_logger(), "Action server not available. Timeout");
+    return;
+  }
+
   auto future_goal_handle = client_move->async_send_goal(goal);
   if (future_goal_handle.wait_for(std::chrono::seconds(timeout)) ==
       std::future_status::timeout) {
@@ -93,6 +101,14 @@ void Gripper::close(
   goal.width = 0.0;
   goal.force = 100.0;
   goal.speed = 0.03;
+
+  auto server_ready =
+      client_move->wait_for_action_server(std::chrono::seconds(timeout));
+  if (!server_ready) {
+    RCLCPP_ERROR(this->get_logger(), "Action server not available. Timeout");
+    return;
+  }
+
   auto future_goal_handle = client_grasp->async_send_goal(goal);
   if (future_goal_handle.wait_for(std::chrono::seconds(timeout)) ==
       std::future_status::timeout) {

ros2_ws/src/rcdt_test/rcdt_test/combined/end_to_end/test_nav2.py

@@ -109,7 +109,7 @@ def test_goal_pose(
             timeout (int): The timeout in seconds.
 
         Raises:
-            AssertionError: When a timeout occurs.
+            TimeoutError: When a timeout occurs.
         """
         # 1) Obtain current pose in map frame:
         tf_buffer = Buffer()
@@ -124,7 +124,7 @@ def test_goal_pose(
                     "map", f"{namespace_vehicle}/base_link", Time()
                 )
             if time.time() - start_time > timeout:
-                raise AssertionError("Timeout while waiting for current pose.")
+                raise TimeoutError()
 
         # 2) Publish goal pose 1 meter in front of current position:
         goal_pose = PoseStamped()
@@ -151,8 +151,8 @@ def test_goal_pose(
                 current_pose.transform.translation.x - goal_pose.pose.position.x
             )
             if time.time() - start_time > timeout:
-                raise AssertionError(
-                    f"Timeout. Distance is {distance} while tolerance is {navigation_distance_tolerance}."
+                raise TimeoutError(
+                    f"Distance is {distance} while tolerance is {navigation_distance_tolerance}."
                 )
 
         # 4) Stop navigation, since the goal can be reached before the navigation is finished due to tolerance:
@@ -172,7 +172,7 @@ def test_gps_navigation(
             timeout (int): The timeout in seconds.
 
         Raises:
-            AssertionError: When a timeout occurs.
+            TimeoutError: When a timeout occurs.
         """
         # 1) Obtain current GPS location:
         current_nav_sat = NavSatFix()
@@ -189,7 +189,7 @@ def callback(msg: NavSatFix) -> None:
         while current_nav_sat == NavSatFix():
             rclpy.spin_once(test_node, timeout_sec=0)
             if time.time() - start_time > timeout:
-                raise AssertionError("Timeout while waiting for current GPS location.")
+                raise TimeoutError("Timeout while waiting for current GPS location.")
 
         # 2) Publish goal GPS location 1e-5 degrees north of current location:
         goal_nav_sat = copy.deepcopy(current_nav_sat)
@@ -211,8 +211,8 @@ def callback(msg: NavSatFix) -> None:
             rclpy.spin_once(test_node, timeout_sec=0)
             distance = abs(current_nav_sat.latitude - goal_nav_sat.latitude)
             if time.time() - start_time > timeout:
-                raise AssertionError(
-                    f"Timeout. Distance is {distance} while tolerance is {navigation_degree_tolerance}."
+                raise TimeoutError(
+                    f"Distance is {distance} while tolerance is {navigation_degree_tolerance}."
                 )
 
         # 4) Stop navigation, since the goal can be reached before the navigation is finished due to tolerance:

ros2_ws/src/rcdt_test/rcdt_test/franka/integration/test_franka_moveit.py

@@ -75,12 +75,9 @@ def test_move_to_drop_configuration(
         joint_movement_tolerance (float): The tolerance for joint movement.
         timeout (int): The timeout in seconds before stopping the test.
     """
-    assert (
-        call_move_to_configuration_service(
-            test_node, namespace, "drop", timeout=timeout
-        )
-        is True
-    )
+    assert call_move_to_configuration_service(
+        test_node, namespace, "drop", timeout=timeout
+    ), "Failed to call move_to_configuration service."
     drop_values = [-1.57079632679, -0.65, 0, -2.4, 0, 1.75, 0.78539816339]
     for i in range(7):
         joint_value = get_joint_position(

ros2_ws/src/rcdt_utilities/rcdt_utilities/test_utils.py

@@ -74,9 +74,14 @@ def wait_for_subscriber(pub: Publisher, timeout: int) -> None:
         pub (Publisher): The publisher to wait for.
         timeout (int): The maximum time to wait for a subscriber in seconds.
 
+    Raises:
+        TimeoutError: If no subscriber is found within the timeout period.
+
     """
-    start_time = time.monotonic()
-    while pub.get_subscription_count() == 0 and time.monotonic() - start_time < timeout:
+    start_time = time.time()
+    while pub.get_subscription_count() == 0:
+        if time.time() > (start_time + timeout):
+            raise TimeoutError()
         time.sleep(0.1)
 
 
@@ -93,7 +98,7 @@ def get_joint_position(namespace: str, joint: str, timeout: int) -> float:
     """
     topic_list = [(f"{namespace}/joint_states", JointState)]
     wait_for_topics = WaitForTopics(topic_list, timeout=timeout)
-    assert wait_for_topics.wait()
+    assert wait_for_topics.wait(), "Did not receive JointState."
     msg: JointState = wait_for_topics.received_messages(f"{namespace}/joint_states")[0]
     idx = msg.name.index(joint)
     position = msg.position[idx]
@@ -277,7 +282,7 @@ def assert_joy_topic_switch(
         state_topic (str): Topic to listen for state updates from joy_topic_manager.
 
     Raises:
-        AssertionError: When a timeout occurs.
+        TimeoutError: When a timeout occurs.
     """
     logger.info("Starting to assert joy topic switch")
     qos = QoSProfile(
@@ -316,9 +321,7 @@ def callback_function(msg: String) -> None:
     while result.get("state") != expected_topic:
         rclpy.spin_once(node, timeout_sec=0)
         if time.time() > (start_time + timeout):
-            raise AssertionError(
-                f"Timeout. Did not receive {expected_topic} on {state_topic}."
-            )
+            raise TimeoutError(f"Did not receive {expected_topic} on {state_topic}.")
 
     assert result.get("state") == expected_topic, (
         f"Expected state '{expected_topic}', but got '{result.get('state')}'"