156 writing tests for panther in simulation (#169)

End-to-end tests: Verifies Panther’s movement behavior in simulation when controlled via joystick inputs.
Integration test to see if the panther starts correctly and if the controllers work.
Common test logic previously written for the Franka robot has been refactored and moved into shared utility functions
Added a shared conftest.

---------

Signed-off-by: Max Waterhout <[email protected]>
Co-authored-by: Max Waterhout <[email protected]>

Author: MaxWaterhout

conftest.py

@@ -0,0 +1,41 @@
+# SPDX-FileCopyrightText: Alliander N. V.
+#
+# SPDX-License-Identifier: Apache-2.0
+
+"""
+Global pytest fixtures for ROS 2 integration testing.
+"""
+
+from typing import Iterator
+
+import pytest
+import rclpy
+from rclpy.node import Node
+
+
+@pytest.fixture(scope="module")
+def test_node() -> Iterator[Node]:
+    """Fixture to create a singleton node for testing."""
+    rclpy.init()
+    node = Node("test_node")
+    yield node
+    node.destroy_node()
+    rclpy.shutdown()
+
+
+@pytest.fixture(scope="module")
+def timeout(pytestconfig: pytest.Config) -> int:
+    """Fixture to get the timeout value from pytest config."""
+    return int(pytestconfig.getini("timeout"))
+
+
+@pytest.fixture(scope="session")
+def finger_joint_fault_tolerance() -> float:
+    """Tolerance of testing finger joint movements."""
+    return 0.025
+
+
+@pytest.fixture(scope="session")
+def joint_movement_tolerance() -> float:
+    """Tolerance of testing joint movements."""
+    return 0.01

docs/content/panther.md

@@ -180,7 +180,7 @@ Now you should be able to place a *2D Goal Pose* in Rviz, somewhere on the map.
 Don't forget to reset the E-STOP when the Panther should drive:
 
 ```bash
-ros2 service call /hardware/e_stop_reset std_srvs/srv/Trigger {}
+ros2 service call /panther/hardware/e_stop_reset std_srvs/srv/Trigger {}
 ```
 
 :::

ros2_ws/src/rcdt_franka/launch/franka.launch.py

@@ -4,7 +4,7 @@
 
 from launch import LaunchContext, LaunchDescription
 from launch.actions import OpaqueFunction
-from launch_ros.actions import Node, SetParameter
+from launch_ros.actions import SetParameter
 from rcdt_utilities.launch_utils import SKIP, LaunchArgument, get_file_path
 from rcdt_utilities.register import Register, RegisteredLaunchDescription
 
@@ -76,7 +76,9 @@ def launch_setup(context: LaunchContext) -> None:
         get_file_path("rcdt_franka", ["launch"], "gripper_services.launch.py")
     )
 
-    manipulate_pose = Node(package="rcdt_utilities", executable="manipulate_pose.py")
+    utilities = RegisteredLaunchDescription(
+        get_file_path("rcdt_utilities", ["launch"], "utils.launch.py")
+    )
 
     return [
         SetParameter(name="use_sim_time", value=use_sim),
@@ -85,7 +87,7 @@ def launch_setup(context: LaunchContext) -> None:
         Register.group(gripper_services, context),
         Register.group(moveit, context),
         Register.group(joystick, context),
-        Register.on_start(manipulate_pose, context),
+        Register.group(utilities, context),
         Register.group(rviz, context) if use_rviz else SKIP,
         Register.group(realsense, context) if use_realsense else SKIP,
     ]

ros2_ws/src/rcdt_franka/test/conftest.py

@@ -2,23 +2,10 @@
 #
 # SPDX-License-Identifier: Apache-2.0
 
-from typing import Iterator
 
 import pytest
-import rclpy
 from rcdt_utilities.launch_utils import get_file_path
 from rcdt_utilities.register import RegisteredLaunchDescription
-from rclpy.node import Node
-
-
-@pytest.fixture(scope="session")
-def test_node() -> Iterator[Node]:
-    """Fixture to create a singleton node for testing."""
-    rclpy.init()
-    node = Node("test_node")
-    yield node
-    node.destroy_node()
-    rclpy.shutdown()
 
 
 @pytest.fixture(scope="module")
@@ -57,14 +44,3 @@ def gripper_services_launch() -> RegisteredLaunchDescription:
     return RegisteredLaunchDescription(
         get_file_path("rcdt_franka", ["launch"], "gripper_services.launch.py")
     )
-
-
-@pytest.fixture(scope="session")
-def finger_joint_fault_tolerance() -> float:
-    return 0.025
-
-
-@pytest.fixture(scope="session")
-def joint_movement_tolerance() -> float:
-    """Tolerance of testing joint movements."""
-    return 0.01

ros2_ws/src/rcdt_franka/test/end_to_end/base_franka.py

@@ -3,90 +3,35 @@
 # SPDX-License-Identifier: Apache-2.0
 
 
-import time
-
 import pytest
-import rclpy
-from geometry_msgs.msg import PoseStamped
-from rcdt_utilities.launch_utils import assert_for_message, assert_for_node
-from rcdt_utilities.test_utils import wait_for_register
+from rcdt_utilities.geometry import Pose
+from rcdt_utilities.launch_utils import assert_for_message
+from rcdt_utilities.test_utils import (
+    assert_joy_topic_switch,
+    assert_movements_with_joy,
+    wait_for_register,
+    wait_until_reached_joint,
+)
 from rclpy.node import Node
-from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSProfile
 from sensor_msgs.msg import JointState, Joy
-from std_msgs.msg import String
-from utils import EndToEndUtils, call_express_pose_in_other_frame
 
 
-class FrankaFullTests(EndToEndUtils):
-    def test_wait_for_register(self, pytestconfig: pytest.Config) -> None:
-        wait_for_register(pytestconfig)
+class FrankaFullTests:
+    def test_wait_for_register(self, timeout: int) -> None:
+        wait_for_register(timeout=timeout)
 
-    def test_joint_states_published(self) -> None:
+    def test_joint_states_published(self, timeout: int) -> None:
         """Test that joint states are published. This is a basic test to check that the
         launch file is working and that the robot is publishing joint states."""
-        assert_for_message(JointState, "franka/joint_states", 60)
-
-    def test_ready_to_start(self, test_node: Node) -> None:
-        """This test will ensure the tests are ready to start by waiting for the move_group and moveit_manager node.
-        Also waits until the gripper_action_controller is active."""
-        assert (
-            assert_for_node(
-                fully_qualified_node_name="franka/move_group",
-                test_node=test_node,
-                timeout=30,
-            )
-            is True
-        )
-        assert (
-            assert_for_node(
-                fully_qualified_node_name="franka/moveit_manager",
-                test_node=test_node,
-                timeout=30,
-            )
-            is True
-        )
-        assert (
-            self.wait_until_active(
-                node=test_node, controller_name="gripper_action_controller"
-            )
-            is True
-        )
+        assert_for_message(JointState, "franka/joint_states", timeout=timeout)
 
-    def test_switch_joy_to_franka_topic(self, test_node: Node) -> None:
+    def test_switch_joy_to_franka_topic(self, test_node: Node, timeout: int) -> None:
         """Test to see if the switch to Franka mode is correct."""
-        latched_qos = QoSProfile(
-            depth=1,
-            durability=QoSDurabilityPolicy.TRANSIENT_LOCAL,
-            history=QoSHistoryPolicy.KEEP_LAST,
-        )
-        result = {}
-
-        def listener_callback(msg: String) -> None:
-            result["state"] = msg.data
-
-        # Subscribe to the correct topic
-        test_node.create_subscription(
-            String,
-            "/joy_topic_manager/state",  # this matches ~/state in JoyTopicManager
-            listener_callback,
-            qos_profile=latched_qos,
-        )
-
-        pub = test_node.create_publisher(Joy, "/joy", 10)
-        msg = Joy()
-        msg.buttons = [1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
-        pub.publish(msg)
-        timeout_sec = 10.0
-        start_time = time.time()
-        while (
-            result.get("state") != "/franka/joy"
-            and time.time() - start_time < timeout_sec
-        ):
-            rclpy.spin_once(test_node, timeout_sec=0.1)
-            time.sleep(0.1)
-
-        assert result.get("state") == "/franka/joy", (
-            f"Unexpected state: {result.get('state')}"
+        assert_joy_topic_switch(
+            node=test_node,
+            expected_topic="/franka/joy",
+            button_config=[1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
+            timeout=timeout,
         )
 
     @pytest.mark.parametrize(
@@ -102,6 +47,7 @@ def test_joy_gripper_node(
         expected_value: float,
         test_node: Node,
         finger_joint_fault_tolerance: float,
+        timeout: int,
     ) -> None:
         """Test that the joy node is running and the gripper is moving.
         The first call is an initialization call.
@@ -114,13 +60,13 @@ def test_joy_gripper_node(
         msg = Joy()
         msg.buttons = buttons
         pub.publish(msg)
-        rclpy.spin_once(test_node, timeout_sec=0.1)
 
-        reached_goal, joint_value = self.wait_until_reached_joint(
+        reached_goal, joint_value = wait_until_reached_joint(
             namespace="franka",
             joint="fr3_finger_joint1",
             expected_value=expected_value,
             tolerance=finger_joint_fault_tolerance,
+            timeout_sec=timeout,
         )
         assert reached_goal is True, (
             f"The joint did not reach the joint. Currently {joint_value}, expected {expected_value}"
@@ -139,34 +85,22 @@ def test_move_arm_with_joy(
         axes: list[float],
         direction: str,
         test_node: Node,
+        timeout: int,
         movement_threshold: float = 0.01,
     ) -> None:
         """Tests the linear movements of the hand while controlling with the joystick.
         Assert if it moves above a certain movement_threshold."""
-        pose = PoseStamped()
-        pose.header.frame_id = "franka/fr3_hand"
-        first_pose = call_express_pose_in_other_frame(
-            node=test_node, pose=pose, target_frame="franka/fr3_link0"
-        ).pose.pose
-
-        pub = test_node.create_publisher(Joy, "/joy", 10)
-
-        msg = Joy()
-        msg.axes = axes
-        pub.publish(msg)
-        rclpy.spin_once(test_node, timeout_sec=0.1)
-        time.sleep(1)  # small delay for the robot to move
-
-        pose = PoseStamped()
-        pose.header.frame_id = "franka/fr3_hand"
-        moved_pose = call_express_pose_in_other_frame(
-            node=test_node, pose=pose, target_frame="franka/fr3_link0"
-        ).pose.pose
-
-        delta = getattr(moved_pose.position, direction) - getattr(
-            first_pose.position, direction
-        )
 
-        assert abs(delta) > movement_threshold, (
-            f"{direction} position did not change after input. Δ = {delta:.4f}"
+        def compare_fn(p1: Pose, p2: Pose) -> float:
+            return getattr(p2.position, direction) - getattr(p1.position, direction)
+
+        assert_movements_with_joy(
+            node=test_node,
+            joy_axes=axes,
+            compare_fn=compare_fn,
+            threshold=movement_threshold,
+            description=f"{direction} position",
+            frame_base="franka/fr3_hand",
+            frame_target="franka/fr3_link0",
+            timeout=timeout,
         )

ros2_ws/src/rcdt_franka/test/end_to_end/test_franka.py

@@ -7,24 +7,21 @@
 import pytest
 from end_to_end.base_franka import FrankaFullTests
 from launch import LaunchDescription
-from launch.actions import IncludeLaunchDescription
 from rcdt_utilities.launch_utils import get_file_path
+from rcdt_utilities.register import Register, RegisteredLaunchDescription
 
 
 @launch_pytest.fixture(scope="class")
 def franka_launch() -> LaunchDescription:
-    return LaunchDescription(
-        [
-            IncludeLaunchDescription(
-                get_file_path("rcdt_franka", ["launch"], "franka.launch.py"),
-                launch_arguments={
-                    "rviz": "False",
-                    "world": "empty_camera.sdf",
-                    "realsense": "False",
-                }.items(),
-            )
-        ]
+    franka = RegisteredLaunchDescription(
+        get_file_path("rcdt_franka", ["launch"], "franka.launch.py"),
+        launch_arguments={
+            "rviz": "False",
+            "world": "empty_camera.sdf",
+            "realsense": "False",
+        },
     )
+    return Register.connect_context([franka])
 
 
 @pytest.mark.launch(fixture=franka_launch)

ros2_ws/src/rcdt_franka/test/integration/test_franka_core.py

@@ -11,7 +11,7 @@
 from rcdt_utilities.test_utils import get_joint_position, wait_for_register
 from rclpy.node import Node
 from sensor_msgs.msg import JointState
-from utils import call_move_gripper_service, follow_joint_trajectory_goal
+from utils import follow_joint_trajectory_goal
 
 
 @launch_pytest.fixture(scope="module")
@@ -23,42 +23,31 @@ def franka_core_launch(
 
 
 @pytest.mark.launch(fixture=franka_core_launch)
-def test_wait_for_register(pytestconfig: pytest.Config) -> None:
-    wait_for_register(pytestconfig)
+def test_wait_for_register(timeout: int) -> None:
+    wait_for_register(timeout=timeout)
 
 
 @pytest.mark.launch(fixture=franka_core_launch)
-def test_joint_states_published() -> None:
-    assert_for_message(JointState, "franka/joint_states", 60)
-
-
-@pytest.mark.launch(fixture=franka_core_launch)
-def test_can_move_fingers(test_node: Node, finger_joint_fault_tolerance: float) -> None:
-    assert (
-        call_move_gripper_service(
-            test_node,
-            width=0.04,
-            action_name="/franka/gripper_action_controller/gripper_cmd",
-        )
-        is True
-    )
-    pos = get_joint_position(namespace="franka", joint="fr3_finger_joint1")
-    assert pos == pytest.approx(0.04, abs=finger_joint_fault_tolerance), (
-        f"Got gripper position of {pos}"
-    )
+def test_joint_states_published(timeout: int) -> None:
+    assert_for_message(JointState, "franka/joint_states", timeout=timeout)
 
 
 @pytest.mark.launch(fixture=franka_core_launch)
 def test_follow_joint_trajectory_goal(
-    test_node: Node, joint_movement_tolerance: float
+    test_node: Node, joint_movement_tolerance: float, timeout: int
 ) -> None:
     follow_joint_trajectory_goal(
         test_node,
         positions=[0.0, -0.5, 0.5, 0.0, 0.0, 0.0, 0.0],
         controller="franka/fr3_arm_controller",
+        timeout=timeout,
+    )
+    pos_joint2 = get_joint_position(
+        namespace="franka", joint="fr3_joint2", timeout=timeout
+    )
+    pos_joint3 = get_joint_position(
+        namespace="franka", joint="fr3_joint3", timeout=timeout
     )
-    pos_joint2 = get_joint_position(namespace="franka", joint="fr3_joint2")
-    pos_joint3 = get_joint_position(namespace="franka", joint="fr3_joint3")
 
     assert pos_joint2 == pytest.approx(-0.5, abs=joint_movement_tolerance), (
         f"Got joint position {pos_joint2}"