Integration tests (#17)

* Add tests

* Add actual test files

* Only test one model

* Make integration test optional

Since the simulation doesn't shutdown cleanly, we do not want to leave behind a dangling simulation after running a colcon build.

* Remove repos-not-released for jazzy

.github/workflows/jazzy-binary-main.yml

@@ -19,4 +19,3 @@ jobs:
       ros_distro: jazzy
       ros_repo: main
       ref_for_scheduled_build: ros2
-      upstream_workspace: ur_simulation_gz-not-released.jazzy.repos

.github/workflows/jazzy-binary-testing.yml

@@ -19,4 +19,3 @@ jobs:
       ros_distro: jazzy
       ros_repo: testing
       ref_for_scheduled_build: ros2
-      upstream_workspace: ur_simulation_gz-not-released.jazzy.repos

.github/workflows/reusable_ici.yml

@@ -76,6 +76,7 @@ jobs:
           UPSTREAM_WORKSPACE: ${{ inputs.upstream_workspace }}
           ROS_DISTRO: ${{ inputs.ros_distro }}
           ROS_REPO: ${{ inputs.ros_repo }}
+          CMAKE_ARGS: -DUR_SIM_INTEGRATION_TESTS=ON
       - name: prepare target_ws for cache
         if: ${{ always() && ! matrix.env.CCOV }}
         run: |

ur_simulation_gz/CMakeLists.txt

@@ -3,13 +3,27 @@ project(ur_simulation_gz)
 
 find_package(ament_cmake REQUIRED)
 
+# Default to off as starting gzsim doesn't shut down correctly at the moment
+option(
+  UR_SIM_INTEGRATION_TESTS
+  "Run ur_simulation_gz integration tests"
+  OFF
+)
+
 install(DIRECTORY config launch urdf
   DESTINATION share/${PROJECT_NAME}
 )
 
 if(BUILD_TESTING)
+  find_package(launch_testing_ament_cmake)
   find_package(ament_cmake_pytest REQUIRED)
 
+  if(${UR_SIM_INTEGRATION_TESTS})
+    add_launch_test(test/test_gz.py
+      TIMEOUT
+        180
+    )
+  endif()
   ament_add_pytest_test(description test/test_description.py)
 endif()
 

ur_simulation_gz/package.xml

@@ -29,6 +29,10 @@
   <exec_depend>xacro</exec_depend>
 
   <test_depend>ament_cmake_pytest</test_depend>
+  <test_depend>launch_testing_ament_cmake</test_depend>
+  <test_depend>launch_testing_ros</test_depend>
+  <test_depend>liburdfdom-tools</test_depend>
+  <test_depend>xacro</test_depend>
 
   <export>
     <build_type>ament_cmake</build_type>

ur_simulation_gz/test/test_common.py

@@ -0,0 +1,155 @@
+# Copyright 2023, FZI Forschungszentrum Informatik
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+#    * Redistributions of source code must retain the above copyright
+#      notice, this list of conditions and the following disclaimer.
+#
+#    * Redistributions in binary form must reproduce the above copyright
+#      notice, this list of conditions and the following disclaimer in the
+#      documentation and/or other materials provided with the distribution.
+#
+#    * Neither the name of the {copyright_holder} nor the names of its
+#      contributors may be used to endorse or promote products derived from
+#      this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+import logging
+
+import rclpy
+from rclpy.action import ActionClient
+
+TIMEOUT_WAIT_SERVICE = 10
+TIMEOUT_WAIT_SERVICE_INITIAL = 120  # If we download the docker image simultaneously to the tests, it can take quite some time until the dashboard server is reachable and usable.
+TIMEOUT_WAIT_ACTION = 10
+
+
+def _wait_for_service(node, srv_name, srv_type, timeout):
+    client = node.create_client(srv_type, srv_name)
+
+    logging.info("Waiting for service '%s' with timeout %fs...", srv_name, timeout)
+    if client.wait_for_service(timeout) is False:
+        raise Exception(f"Could not reach service '{srv_name}' within timeout of {timeout}")
+    logging.info("  Successfully connected to service '%s'", srv_name)
+
+    return client
+
+
+def _wait_for_action(node, action_name, action_type, timeout):
+    client = ActionClient(node, action_type, action_name)
+
+    logging.info("Waiting for action server '%s' with timeout %fs...", action_name, timeout)
+    if client.wait_for_server(timeout) is False:
+        raise Exception(
+            f"Could not reach action server '{action_name}' within timeout of {timeout}"
+        )
+
+    logging.info("  Successfully connected to action server '%s'", action_name)
+    return client
+
+
+def _call_service(node, client, request):
+    logging.info("Calling service client '%s' with request '%s'", client.srv_name, request)
+    future = client.call_async(request)
+
+    rclpy.spin_until_future_complete(node, future)
+
+    if future.result() is not None:
+        logging.info("  Received result: %s", future.result())
+        return future.result()
+
+    raise Exception(f"Error while calling service '{client.srv_name}': {future.exception()}")
+
+
+class _ServiceInterface:
+    def __init__(
+        self, node, initial_timeout=TIMEOUT_WAIT_SERVICE_INITIAL, timeout=TIMEOUT_WAIT_SERVICE
+    ):
+        self.__node = node
+
+        self.__service_clients = {
+            srv_name: (
+                _wait_for_service(self.__node, srv_name, srv_type, initial_timeout),
+                srv_type,
+            )
+            for srv_name, srv_type in self.__initial_services.items()
+        }
+        self.__service_clients.update(
+            {
+                srv_name: (_wait_for_service(self.__node, srv_name, srv_type, timeout), srv_type)
+                for srv_name, srv_type in self.__services.items()
+            }
+        )
+
+    def __init_subclass__(mcs, namespace="", initial_services={}, services={}, **kwargs):
+        super().__init_subclass__(**kwargs)
+
+        mcs.__initial_services = {
+            namespace + "/" + srv_name: srv_type for srv_name, srv_type in initial_services.items()
+        }
+        mcs.__services = {
+            namespace + "/" + srv_name: srv_type for srv_name, srv_type in services.items()
+        }
+
+        for srv_name, srv_type in list(initial_services.items()) + list(services.items()):
+            full_srv_name = namespace + "/" + srv_name
+
+            setattr(
+                mcs,
+                srv_name,
+                lambda s, full_srv_name=full_srv_name, *args, **kwargs: _call_service(
+                    s.__node,
+                    s.__service_clients[full_srv_name][0],
+                    s.__service_clients[full_srv_name][1].Request(*args, **kwargs),
+                ),
+            )
+
+
+class ActionInterface:
+    def __init__(self, node, action_name, action_type, timeout=TIMEOUT_WAIT_ACTION):
+        self.__node = node
+
+        self.__action_name = action_name
+        self.__action_type = action_type
+        self.__action_client = _wait_for_action(node, action_name, action_type, timeout)
+
+    def send_goal(self, *args, **kwargs):
+        goal = self.__action_type.Goal(*args, **kwargs)
+
+        logging.info("Sending goal to action server '%s': %s", self.__action_name, goal)
+        future = self.__action_client.send_goal_async(goal)
+
+        rclpy.spin_until_future_complete(self.__node, future)
+
+        # TODO: Replace this timeout with a proper check whether the robot is initialized
+        if future.result() is not None:
+            logging.info("  Received result: %s", future.result())
+            return future.result()
+        pass
+
+    def get_result(self, goal_handle, timeout):
+        future_res = goal_handle.get_result_async()
+
+        logging.info(
+            "Waiting for action result from '%s' with timeout %fs", self.__action_name, timeout
+        )
+        rclpy.spin_until_future_complete(self.__node, future_res, timeout_sec=timeout)
+
+        if future_res.result() is not None:
+            logging.info("  Received result: %s", future_res.result().result)
+            return future_res.result().result
+        else:
+            raise Exception(
+                f"Exception while calling action '{self.__action_name}': {future_res.exception()}"
+            )

ur_simulation_gz/test/test_gz.py

@@ -0,0 +1,146 @@
+#!/usr/bin/env python
+# Copyright 2023, FZI Forschungszentrum Informatik
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+#    * Redistributions of source code must retain the above copyright
+#      notice, this list of conditions and the following disclaimer.
+#
+#    * Redistributions in binary form must reproduce the above copyright
+#      notice, this list of conditions and the following disclaimer in the
+#      documentation and/or other materials provided with the distribution.
+#
+#    * Neither the name of the {copyright_holder} nor the names of its
+#      contributors may be used to endorse or promote products derived from
+#      this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+import logging
+import os
+import pytest
+import sys
+import time
+import unittest
+
+
+import rclpy
+from rclpy.node import Node
+from launch import LaunchDescription
+from launch.actions import (
+    IncludeLaunchDescription,
+)
+from launch.substitutions import PathJoinSubstitution
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.substitutions import FindPackageShare
+from launch_testing.actions import ReadyToTest
+
+from builtin_interfaces.msg import Duration
+from control_msgs.action import FollowJointTrajectory
+from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
+
+sys.path.append(os.path.dirname(__file__))
+from test_common import (  # noqa: E402
+    ActionInterface,
+)
+
+
+# Increased timeout since sim_time could be slower... TODO: Make this more properly
+TIMEOUT_EXECUTE_TRAJECTORY = 60
+
+ROBOT_JOINTS = [
+    "elbow_joint",
+    "shoulder_lift_joint",
+    "shoulder_pan_joint",
+    "wrist_1_joint",
+    "wrist_2_joint",
+    "wrist_3_joint",
+]
+
+
+# TODO: Add tf_prefix parametrization
+# TODO: Currently, launching multiple simulations from this makes the old simulation not stop. This
+# might change, once the gz launch system migration is done using gzserver and such....
+# @launch_testing.parametrize(
+# "ur_type",
+# ["ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e", "ur20", "ur30"],
+# )
+@pytest.mark.launch_test
+def generate_test_description():
+    simulator = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            PathJoinSubstitution(
+                [FindPackageShare("ur_simulation_gz"), "launch", "ur_sim_control.launch.py"]
+            )
+        ),
+        launch_arguments={
+            "ur_type": "ur5e",
+            "launch_rviz": "false",
+            "gazebo_gui": "false",
+            "start_joint_controller": "true",
+        }.items(),
+    )
+    return LaunchDescription([ReadyToTest(), simulator])
+
+
+class GazeboTest(unittest.TestCase):
+    @classmethod
+    def setUpClass(cls):
+        # Initialize the ROS context
+        rclpy.init()
+        cls.node = Node("ur_gazebo_test")
+        time.sleep(1)
+        cls.init_robot(cls)
+
+    @classmethod
+    def tearDownClass(cls):
+        # Shutdown the ROS context
+        cls.node.destroy_node()
+        rclpy.shutdown()
+
+    def init_robot(self):
+        self._follow_joint_trajectory = ActionInterface(
+            self.node,
+            "/joint_trajectory_controller/follow_joint_trajectory",
+            FollowJointTrajectory,
+        )
+        # TODO: Replace this timeout with a proper check whether the robot is initialized
+        time.sleep(5)
+
+    def test_trajectory(self):
+        """Test robot movement."""
+        # Construct test trajectory
+        test_trajectory = [
+            (Duration(sec=6, nanosec=0), [0.0 for j in ROBOT_JOINTS]),
+            (Duration(sec=9, nanosec=0), [-0.5 for j in ROBOT_JOINTS]),
+            (Duration(sec=12, nanosec=0), [-1.0 for j in ROBOT_JOINTS]),
+        ]
+
+        trajectory = JointTrajectory(
+            # joint_names=[tf_prefix + joint for joint in ROBOT_JOINTS],
+            joint_names=ROBOT_JOINTS,
+            points=[
+                JointTrajectoryPoint(positions=test_pos, time_from_start=test_time)
+                for (test_time, test_pos) in test_trajectory
+            ],
+        )
+
+        # Sending trajectory goal
+        logging.info("Sending simple goal")
+        goal_handle = self._follow_joint_trajectory.send_goal(trajectory=trajectory)
+        self.assertTrue(goal_handle.accepted)
+
+        # Verify execution
+        result = self._follow_joint_trajectory.get_result(goal_handle, TIMEOUT_EXECUTE_TRAJECTORY)
+        self.assertEqual(result.error_code, FollowJointTrajectory.Result.SUCCESSFUL)