Adapt pytest test_arm to support UR arm.

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

Author: Jelmerdw

alliander_tests/src/alliander_tests/tests/test_arm.py

@@ -2,8 +2,11 @@
 #
 # SPDX-License-Identifier: Apache-2.0
 
+import numpy as np
 import pytest
+import xmltodict
 from alliander_utilities.config_objects import Arm
+from control_msgs.msg import JointTrajectoryControllerState
 from rclpy.node import Node
 from sensor_msgs.msg import JointState
 
@@ -13,18 +16,17 @@
     call_trigger_action,
     check_joint_positions,
     follow_joint_trajectory_goal,
+    get_message,
+    get_parameter,
     wait_until_reached_joint,
 )
 
-arm = Arm("franka", gripper=True, moveit=True)
-PLATFORMS = [arm]
-
 
 class _TestArm:
     """Base class for arm tests.
 
     Attributes:
-         platforms (dict): A dictionary of the platforms to launch.
+        platforms (dict): A dictionary of the platforms to launch.
     """
 
     platforms: dict
@@ -56,6 +58,8 @@ def test_gripper_action(
             finger_joint_fault_tolerance (float): The tolerance for the finger joint position.
             timeout (int): The timeout in seconds before stopping the test.
         """
+        if self.platforms["arm"].name == "ur":
+            pytest.skip("Gripper is not yet implemented for UR arm.")
         assert (
             call_trigger_action(
                 test_node,
@@ -85,18 +89,27 @@ def test_follow_joint_trajectory_goal(
             joint_movement_tolerance (float): The tolerance for joint movement.
             timeout (int): The timeout in seconds to wait for the joint trajectory goal to be followed.
         """
-        expected_positions = [0.15, -0.39, 0.1, -2.06, 0.0, 1.68, 1.01]
+        # Get joint names and current position and define a goal position:
+        controller_state = get_message(
+            JointTrajectoryControllerState,
+            f"/{self.platforms['arm'].namespace}/joint_trajectory_controller/controller_state",
+            timeout=timeout,
+        )
+        current_positions = list(controller_state.reference.positions)
+        goal_positions = [position + np.deg2rad(10) for position in current_positions]
+
+        # Call the follow_joint_trajectory action and check if the joints reached the expected positions:
         follow_joint_trajectory_goal(
             test_node,
-            positions=expected_positions,
-            controller=f"{self.platforms['arm'].namespace}/fr3_arm_controller",
+            controller_state.joint_names,
+            goal_positions,
+            controller=f"{self.platforms['arm'].namespace}/joint_trajectory_controller",
             timeout=timeout,
         )
-        joint_names = [f"fr3_joint{i + 1}" for i in range(7)]
         check_joint_positions(
             self.platforms["arm"].namespace,
-            joint_names,
-            expected_positions,
+            controller_state.joint_names,
+            goal_positions,
             joint_movement_tolerance,
             timeout,
         )
@@ -111,21 +124,43 @@ 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.
         """
+        # Get the robot_description_semantic and convert to a dictionary:
+        robot_description_semantic_str = get_parameter(
+            test_node,
+            f"/{self.platforms['arm'].namespace}/move_group",
+            "robot_description_semantic",
+            timeout=timeout,
+        ).string_value
+        robot_description_semantic = xmltodict.parse(robot_description_semantic_str)
+
+        # Extract the configurations from the robot_description_semantic:
+        configurations = {}
+        group_states = robot_description_semantic["robot"]["group_state"]
+        for group_state in group_states:
+            configurations[group_state["@name"]] = {"names": [], "positions": []}
+            for joint in group_state["joint"]:
+                configurations[group_state["@name"]]["names"].append(joint["@name"])
+                configurations[group_state["@name"]]["positions"].append(
+                    joint["@value"]
+                )
+
+        # Call the move_to_configuration service and check if the joints reached the expected positions:
+        configuration = "drop"
+        names = configurations[configuration]["names"]
+        positions = [float(pos) for pos in configurations[configuration]["positions"]]
         assert call_move_to_configuration_service(
-            test_node, self.platforms["arm"].namespace, "drop", timeout=timeout
+            test_node, self.platforms["arm"].namespace, configuration, timeout=timeout
         ), "Failed to call move_to_configuration service."
-        joint_names = [f"fr3_joint{i + 1}" for i in range(7)]
-        expected_positions = [-1.57079632679, -0.65, 0, -2.4, 0, 1.75, 0.78539816339]
         check_joint_positions(
             self.platforms["arm"].namespace,
-            joint_names,
-            expected_positions,
+            names,
+            positions,
             joint_movement_tolerance,
             timeout,
         )
 
 
-for arm in ["franka"]:
+for arm in ["franka", "ur"]:
     arm_platform = Arm(arm, (0, 0, 0.5), gripper=True, moveit=True)
     test_class = type(
         f"Test{arm.capitalize()}",

alliander_tests/src/alliander_tests/utils.py

@@ -13,6 +13,8 @@
 from builtin_interfaces.msg import Duration
 from control_msgs.action import FollowJointTrajectory
 from launch_testing_ros.wait_for_topics import WaitForTopics
+from rcl_interfaces.msg import ParameterValue
+from rcl_interfaces.srv import GetParameters
 from rclpy.action import ActionClient
 from rclpy.action.client import ClientGoalHandle
 from rclpy.client import Client
@@ -163,6 +165,31 @@ def call_trigger_service(node: Node, service_name: str, timeout: int) -> bool:
     return future.result() is not None
 
 
+def get_parameter(
+    node: Node, parameter_node: str, parameter_name: str, timeout: int
+) -> ParameterValue:
+    """Get a parameter using a service call.
+
+    Args:
+        node (Node): The rclpy node used to get the parameter.
+        parameter_node (str): The name of the node that has the parameter.
+        parameter_name (str): The name of the parameter to get.
+        timeout (int): Timeout in seconds to wait for the parameter.
+
+    Returns:
+        ParameterValue: The value of the parameter
+    """
+    client = create_ready_service_client(
+        node, GetParameters, f"{parameter_node}/get_parameters", timeout
+    )
+    request = GetParameters.Request()
+    request.names = [parameter_name]
+
+    future = client.call_async(request)
+    rclpy.spin_until_future_complete(node, future=future, timeout_sec=timeout)
+    return future.result().values[0]
+
+
 def call_trigger_action(node: Node, action_name: str, timeout: int) -> bool:
     """Call a trigger action and return True if the action was called successfully.
 
@@ -223,18 +250,33 @@ def create_ready_action_client(
     return client
 
 
-def assert_for_message(message_type: type, topic: str, timeout: int) -> None:
-    """Assert that a message of a specific type is received on a given topic within a timeout period.
+def get_message(message_type: type, topic: str, timeout: int) -> Any:
+    """Try to receive a message of a specific type on a given topic within a timeout period.
 
     Args:
         message_type (type): The type of the message to wait for.
         topic (str): The topic to listen to.
         timeout (int): The maximum time in seconds to wait for the message.
+
+    Returns:
+        Any: The received message, or None if no message was received within the timeout.
     """
     wait_for_topics = WaitForTopics([(topic, message_type)], timeout)
     received = wait_for_topics.wait()
     wait_for_topics.shutdown()
-    assert received, (
+    return wait_for_topics.received_messages(topic)[-1] if received else None
+
+
+def assert_for_message(message_type: type, topic: str, timeout: int) -> None:
+    """Assert that a message of a specific type is received on a given topic within a timeout period.
+
+    Args:
+        message_type (type): The type of the message to wait for.
+        topic (str): The topic to listen to.
+        timeout (int): The maximum time in seconds to wait for the message.
+    """
+    message = get_message(message_type, topic, timeout)
+    assert message is not None, (
         f"No message received of type {message_type.__name__} on topic {topic} within {timeout} seconds."
     )
 
@@ -303,20 +345,22 @@ def call_move_to_configuration_service(
 
 def follow_joint_trajectory_goal(
     node: Node,
+    names: list[str],
     positions: list[float],
     controller: str,
     timeout: int,
-    time_from_start: int = 3,
 ) -> None:
     """Test sending a joint trajectory goal to the arm controller.
 
     Args:
         node (Node): The ROS 2 node to use for the action client.
+        names (list[str]): The names of the joints to control.
         positions (list[float]): The joint positions to move to.
         controller (str): The name of the controller to use.
         timeout (int): The timeout in seconds for the action client.
-        time_from_start (int, optional): The time from start in seconds. Defaults to 3.
     """
+    time_from_start = 3
+
     action_client = create_ready_action_client(
         node,
         FollowJointTrajectory,
@@ -325,15 +369,7 @@ def follow_joint_trajectory_goal(
     )
 
     goal_msg = FollowJointTrajectory.Goal()
-    goal_msg.trajectory.joint_names = [
-        "fr3_joint1",
-        "fr3_joint2",
-        "fr3_joint3",
-        "fr3_joint4",
-        "fr3_joint5",
-        "fr3_joint6",
-        "fr3_joint7",
-    ]
+    goal_msg.trajectory.joint_names = names
 
     point = JointTrajectoryPoint()
     point.positions = positions