Improve stability of tests.

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

Author: Jelmerdw

.github/workflows/reusable-workspace.yml

@@ -60,7 +60,7 @@ jobs:
             --entrypoint /bin/bash "${{ inputs.container-image }}" -i -c "
               source /home/rcdt/.bashrc &&
               source install/setup.bash &&
-              uv run xvfb-run pytest -s -rsxf src/
+              uv run xvfb-run pytest -s -v -rsxf src/
             "
 
       - name: Show disk usage after job

conftest.py

@@ -4,6 +4,7 @@
 
 """Global pytest fixtures for ROS 2 integration testing."""
 
+import time
 from typing import Iterator
 
 import pytest
@@ -12,6 +13,7 @@
 from _pytest.config.argparsing import Parser
 from rcdt_utilities.launch_utils import reset
 from rclpy.node import Node
+from ros2node.api import NodeName, get_node_names
 
 
 def pytest_addoption(parser: Parser) -> None:
@@ -23,6 +25,32 @@ def pytest_addoption(parser: Parser) -> None:
     parser.addoption("--simulation", action="store", default="True")
 
 
+@pytest.fixture(scope="module", autouse=True)
+def wait_for_nodes_to_stop(test_node: Node) -> None:
+    """Fixture to wait for all nodes to stop before starting with the tests.
+
+    Args:
+        test_node (Node): The test node used to check for active nodes.
+    """
+    test_node_name = test_node.get_fully_qualified_name()
+    node_names: list[NodeName] = [
+        NodeName(name="dummy", full_name="dummy", namespace="dummy")
+    ]
+    while len(node_names) > 0:
+        node_names: list[NodeName] = get_node_names(node=test_node)
+        node_names = [
+            node_name
+            for node_name in node_names
+            if node_name.full_name != test_node_name
+        ]
+        msg = "\nWaiting for the following nodes to stop:\n"
+        for node_name in node_names:
+            msg += f"{node_name.full_name}\n"
+        if len(node_names) > 0:
+            test_node.get_logger().info(msg)
+        time.sleep(1)
+
+
 @pytest.fixture(scope="module", autouse=True)
 def reset_platform() -> None:
     """Fixture to automatically reset the platform configuration from a previous test module."""
@@ -47,15 +75,15 @@ def test_node() -> Iterator[Node]:
 
 @pytest.fixture(scope="module")
 def timeout(pytestconfig: Config) -> int:
-    """Fixture to get the timeout value from pytest config.
+    """Fixture to get the timeout value from pytest config and return half of it.
 
     Args:
         pytestconfig (Config): The pytest configuration object.
 
     Returns:
         int: The timeout value in seconds.
     """
-    return int(pytestconfig.getini("timeout"))
+    return int(int(pytestconfig.getini("timeout")) / 2)
 
 
 @pytest.fixture(scope="session")
@@ -91,7 +119,7 @@ def navigation_distance_tolerance() -> float:
     Returns:
         float: The tolerance value for navigation.
     """
-    return 0.2
+    return 0.25
 
 
 @pytest.fixture(scope="session")
@@ -103,4 +131,4 @@ def navigation_degree_tolerance() -> float:
     Returns:
         float: The tolerance value for navigation.
     """
-    return 2e-6
+    return 2.5e-6  # ~0.25 meters

ros2_ws/src/rcdt_franka/include/gripper.hpp

@@ -33,6 +33,8 @@ class Gripper : public rclcpp::Node {
   rclcpp_action::Client<Move>::SharedPtr client_move;
   /** The action client to grasp with the gripper. */
   rclcpp_action::Client<Grasp>::SharedPtr client_grasp;
+  /** Timeout for future objects in seconds. */
+  int timeout = 5;
 
   /**
    * @brief Handle goal requests

ros2_ws/src/rcdt_franka/src/gripper.cpp

@@ -60,14 +60,14 @@ void Gripper::open(
   goal.width = 0.08;
   goal.speed = 0.03;
   auto future_goal_handle = client_move->async_send_goal(goal);
-  if (future_goal_handle.wait_for(std::chrono::seconds(5)) ==
+  if (future_goal_handle.wait_for(std::chrono::seconds(timeout)) ==
       std::future_status::timeout) {
     RCLCPP_ERROR(this->get_logger(), "Failed to obtain goal_handle. Timeout.");
     return;
   }
 
   auto future_result = client_move->async_get_result(future_goal_handle.get());
-  if (future_result.wait_for(std::chrono::seconds(5)) ==
+  if (future_result.wait_for(std::chrono::seconds(timeout)) ==
       std::future_status::timeout) {
     RCLCPP_ERROR(this->get_logger(), "Failed to obtain result. Timeout.");
     return;
@@ -94,14 +94,14 @@ void Gripper::close(
   goal.force = 100.0;
   goal.speed = 0.03;
   auto future_goal_handle = client_grasp->async_send_goal(goal);
-  if (future_goal_handle.wait_for(std::chrono::seconds(5)) ==
+  if (future_goal_handle.wait_for(std::chrono::seconds(timeout)) ==
       std::future_status::timeout) {
     RCLCPP_ERROR(this->get_logger(), "Failed to obtain goal_handle. Timeout.");
     return;
   }
 
   auto future_result = client_grasp->async_get_result(future_goal_handle.get());
-  if (future_result.wait_for(std::chrono::seconds(5)) ==
+  if (future_result.wait_for(std::chrono::seconds(timeout)) ==
       std::future_status::timeout) {
     RCLCPP_ERROR(this->get_logger(), "Failed to obtain result. Timeout.");
     return;

ros2_ws/src/rcdt_panther/src_py/nav2_manager.py

@@ -37,6 +37,7 @@ def cb_goal_pose(self, msg: PoseStamped) -> None:
         Args:
             msg (PoseStamped): The received PoseStamped message.
         """
+        self.get_logger().info("Received new goal pose for navigation.")
         self.basic_navigator.goToPose(msg)
 
     def cb_waypoints(self, msg: Path) -> None:
@@ -45,6 +46,7 @@ def cb_waypoints(self, msg: Path) -> None:
         Args:
             msg (Path): The received Path message.
         """
+        self.get_logger().info("Received new waypoints for navigation.")
         self.basic_navigator.followWaypoints(msg.poses)
 
     def cb_gps_waypoints(self, msg: GeoPath) -> None:
@@ -53,6 +55,7 @@ def cb_gps_waypoints(self, msg: GeoPath) -> None:
         Args:
             msg (GeoPath): The received GeoPath message.
         """
+        self.get_logger().info("Received new GPS waypoints for navigation.")
         geo_poses = []
         for geo_pose_stamped in msg.poses:
             geo_pose_stamped: GeoPoseStamped

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

@@ -5,6 +5,8 @@
 import contextlib
 import copy
 import itertools
+import sys
+import time
 from functools import partial
 from typing import Callable
 
@@ -27,57 +29,59 @@
     assert_for_message,
     call_trigger_service,
     wait_for_register,
+    wait_for_subscriber,
 )
 from rclpy.node import Node
 from rclpy.time import Time
 from sensor_msgs.msg import JointState, NavSatFix
-from std_srvs.srv import Trigger
 from tf2_ros import TransformException  # ty: ignore[unresolved-import]
 from tf2_ros.buffer import Buffer
 from tf2_ros.transform_listener import TransformListener
 
 launch_fixtures = []
 
 
-def get_tests(namespace_vehicle: str, namespace_gps: str, timeout: int) -> dict:  # noqa: PLR0915
+def get_tests(namespace_vehicle: str, namespace_gps: str) -> dict:  # noqa: PLR0915
     """Get the Nav2 tests.
 
     Args:
         namespace_vehicle (str): The namespace of the vehicle.
         namespace_gps (str): The namespace of the GPS.
-        timeout (int): The timeout in seconds.
 
     Returns:
         dict: A dictionary containing the tests.
     """
 
-    def test_wait_for_register(_self: object) -> None:
+    def test_wait_for_register(_self: object, timeout: int) -> None:
         """Test that the panther core is registered in the RCDT.
 
         Args:
             _self (object): The test class instance.
+            timeout (int): The timeout in seconds.
         """
         wait_for_register(timeout=timeout)
 
-    def test_joint_states_published(_self: object) -> None:
+    def test_joint_states_published(_self: object, timeout: int) -> None:
         """Test that the joint states are published.
 
         Args:
             _self (object): The test class instance.
+            timeout (int): The timeout in seconds.
         """
         assert_for_message(
             JointState, f"/{namespace_vehicle}/joint_states", timeout=timeout
         )
 
     def test_e_stop_request_reset(
-        _self: object, request: SubRequest, test_node: Node
+        _self: object, request: SubRequest, test_node: Node, timeout: int
     ) -> None:
         """Test that the E-Stop request service can be called to unlock the Panther.
 
         Args:
             _self (object): The test class instance.
             request (SubRequest): The pytest request object, used to access command line options
             test_node (Node): The ROS 2 node to use for the test.
+            timeout (int): The timeout in seconds.
         """
         if request.config.getoption("simulation"):
             pytest.skip("E-Stop is not available.")  # ty: ignore[call-non-callable]
@@ -91,26 +95,36 @@ def test_e_stop_request_reset(
         )
 
     def test_goal_pose(
-        _self: object, test_node: Node, navigation_distance_tolerance: float
+        _self: object,
+        test_node: Node,
+        navigation_distance_tolerance: float,
+        timeout: int,
     ) -> None:
         """Test that navigation to a goal pose is successful.
 
         Args:
             _self (object): The test class instance.
             test_node (Node): The ROS 2 node to use for the test.
             navigation_distance_tolerance (float): The tolerance for navigation.
+            timeout (int): The timeout in seconds.
+
+        Raises:
+            AssertionError: When a timeout occurs.
         """
         # 1) Obtain current pose in map frame:
         tf_buffer = Buffer()
         TransformListener(tf_buffer, test_node)
         current_pose = TransformStamped()
 
+        start_time = time.time()
         while current_pose == TransformStamped():
             rclpy.spin_once(test_node, timeout_sec=0)
             with contextlib.suppress(TransformException):
                 current_pose = tf_buffer.lookup_transform(
                     "map", f"{namespace_vehicle}/base_link", Time()
                 )
+            if time.time() - start_time > timeout:
+                raise AssertionError("Timeout while waiting for current pose.")
 
         # 2) Publish goal pose 1 meter in front of current position:
         goal_pose = PoseStamped()
@@ -120,39 +134,45 @@ def test_goal_pose(
         goal_pose.pose.position.z = current_pose.transform.translation.z
 
         publisher = test_node.create_publisher(PoseStamped, "/goal_pose", 10)
+        wait_for_subscriber(publisher, timeout)
         publisher.publish(goal_pose)
+        test_node.get_logger().info("Published goal pose for navigation.")
 
         # 3) Wait until goal is reached within tolerance:
-        while (
-            abs(current_pose.transform.translation.x - goal_pose.pose.position.x)
-            > navigation_distance_tolerance
-        ):
+        start_time = time.time()
+        distance: float = sys.float_info.max
+        while distance > navigation_distance_tolerance:
             rclpy.spin_once(test_node, timeout_sec=0)
             with contextlib.suppress(TransformException):
                 current_pose = tf_buffer.lookup_transform(
                     "map", f"{namespace_vehicle}/base_link", Time()
                 )
+            distance = abs(
+                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}."
+                )
 
-        # 4) Stop the navigation, since the test can finish before the goal is reached due to the tolerance:
-        stop_navigation_client = test_node.create_client(
-            Trigger, f"/{namespace_vehicle}/nav2_manager/stop"
-        )
-        future = stop_navigation_client.call_async(Trigger.Request())
-        rclpy.spin_until_future_complete(test_node, future, timeout_sec=timeout)
-        service_response: Trigger.Response = future.result()
-        assert False if not service_response else service_response.success, (
-            "Failed to stop navigation."
+        # 4) Stop navigation, since the goal can be reached before the navigation is finished due to tolerance:
+        assert call_trigger_service(
+            test_node, f"/{namespace_vehicle}/nav2_manager/stop", timeout
         )
 
     def test_gps_navigation(
-        _self: object, test_node: Node, navigation_degree_tolerance: float
+        _self: object, test_node: Node, navigation_degree_tolerance: float, timeout: int
     ) -> None:
         """Test that GPS navigation to a goal pose is successful.
 
         Args:
             _self (object): The test class instance.
             test_node (Node): The ROS 2 node to use for the test.
             navigation_degree_tolerance (float): The tolerance for navigation.
+            timeout (int): The timeout in seconds.
+
+        Raises:
+            AssertionError: When a timeout occurs.
         """
         # 1) Obtain current GPS location:
         current_nav_sat = NavSatFix()
@@ -165,14 +185,18 @@ def callback(msg: NavSatFix) -> None:
             NavSatFix, f"/{namespace_gps}/gps/fix", callback, 10
         )
 
+        start_time = time.time()
         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.")
 
         # 2) Publish goal GPS location 1e-5 degrees north of current location:
         goal_nav_sat = copy.deepcopy(current_nav_sat)
         goal_nav_sat.latitude += 1e-5
 
         publisher = test_node.create_publisher(GeoPath, "/gps_waypoints", 10)
+        wait_for_subscriber(publisher, timeout)
         goal_msg = GeoPath()
         goal_pose = GeoPoseStamped()
         goal_pose.pose.position.latitude = goal_nav_sat.latitude
@@ -181,21 +205,19 @@ def callback(msg: NavSatFix) -> None:
         publisher.publish(goal_msg)
 
         # 3) Wait until goal is reached within tolerance:
-        while (
-            abs(goal_nav_sat.latitude - current_nav_sat.latitude)
-            > navigation_degree_tolerance
-        ):
+        start_time = time.time()
+        distance: float = sys.float_info.max
+        while distance > navigation_degree_tolerance:
             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}."
+                )
 
-        # 4) Stop the navigation, since the test can finish before the goal is reached due to the tolerance:
-        stop_navigation_client = test_node.create_client(
-            Trigger, f"/{namespace_vehicle}/nav2_manager/stop"
-        )
-        future = stop_navigation_client.call_async(Trigger.Request())
-        rclpy.spin_until_future_complete(test_node, future, timeout_sec=timeout)
-        service_response: Trigger.Response = future.result()
-        assert False if not service_response else service_response.success, (
-            "Failed to stop navigation."
+        # 4) Stop navigation, since the goal can be reached before the navigation is finished due to tolerance:
+        assert call_trigger_service(
+            test_node, f"/{namespace_vehicle}/nav2_manager/stop", timeout
         )
 
     # Collect all test methods defined above
@@ -280,7 +302,7 @@ def launch_fixture(
     test_class = type(
         f"TestNav2_{vehicle_platform}_{lidar_platform}_{gps_platform}",
         (object,),
-        get_tests(vehicle_platform, gps_platform, timeout=150),
+        get_tests(vehicle_platform, gps_platform),
     )
     pytest.mark.launch(fixture=launch_fixture_wrapper)(test_class)
     globals()[test_class.__name__] = test_class