From f56b4dad96b84069bfb5f8218f65ed891526d3b1 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Mon, 15 Apr 2024 13:05:16 +0200 Subject: [PATCH 1/5] Add tests --- ur_simulation_gz/CMakeLists.txt | 5 +++++ ur_simulation_gz/package.xml | 4 ++++ 2 files changed, 9 insertions(+) diff --git a/ur_simulation_gz/CMakeLists.txt b/ur_simulation_gz/CMakeLists.txt index 48d5f6c..b547e1c 100644 --- a/ur_simulation_gz/CMakeLists.txt +++ b/ur_simulation_gz/CMakeLists.txt @@ -8,8 +8,13 @@ install(DIRECTORY config launch urdf ) if(BUILD_TESTING) + find_package(launch_testing_ament_cmake) find_package(ament_cmake_pytest REQUIRED) + add_launch_test(test/test_gz.py + TIMEOUT + 180 + ) ament_add_pytest_test(description test/test_description.py) endif() diff --git a/ur_simulation_gz/package.xml b/ur_simulation_gz/package.xml index caeff62..2d91092 100644 --- a/ur_simulation_gz/package.xml +++ b/ur_simulation_gz/package.xml @@ -29,6 +29,10 @@ xacro ament_cmake_pytest + launch_testing_ament_cmake + launch_testing_ros + liburdfdom-tools + xacro ament_cmake From a0939cb6070c811085b610fc142ece18afe76760 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 16 May 2024 10:08:01 +0200 Subject: [PATCH 2/5] Add actual test files --- ur_simulation_gz/test/test_common.py | 155 +++++++++++++++++++++++++++ ur_simulation_gz/test/test_gz.py | 146 +++++++++++++++++++++++++ 2 files changed, 301 insertions(+) create mode 100644 ur_simulation_gz/test/test_common.py create mode 100644 ur_simulation_gz/test/test_gz.py diff --git a/ur_simulation_gz/test/test_common.py b/ur_simulation_gz/test/test_common.py new file mode 100644 index 0000000..550d0c8 --- /dev/null +++ b/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()}" + ) diff --git a/ur_simulation_gz/test/test_gz.py b/ur_simulation_gz/test/test_gz.py new file mode 100644 index 0000000..099bc61 --- /dev/null +++ b/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 +import launch_testing + +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 +@pytest.mark.launch_test +@launch_testing.parametrize( + "ur_type", + ["ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e", "ur20", "ur30"], +) +def generate_test_description(ur_type): + simulator = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [FindPackageShare("ur_simulation_gz"), "launch", "ur_sim_control.launch.py"] + ) + ), + launch_arguments={ + "ur_type": ur_type, + "launch_rviz": "true", + "gazebo_gui": "false", + "start_joint_controller": "true", + # "gz_gui": "false", + }.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, ur_type): + """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) From c7478fcd03c74704fd78fd7675b3ff22f61f532a Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Mon, 24 Jun 2024 14:16:03 +0200 Subject: [PATCH 3/5] Only test one model --- ur_simulation_gz/test/test_gz.py | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/ur_simulation_gz/test/test_gz.py b/ur_simulation_gz/test/test_gz.py index 099bc61..d85b0ad 100644 --- a/ur_simulation_gz/test/test_gz.py +++ b/ur_simulation_gz/test/test_gz.py @@ -45,7 +45,6 @@ from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.substitutions import FindPackageShare from launch_testing.actions import ReadyToTest -import launch_testing from builtin_interfaces.msg import Duration from control_msgs.action import FollowJointTrajectory @@ -71,12 +70,14 @@ # 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 -@launch_testing.parametrize( - "ur_type", - ["ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e", "ur20", "ur30"], -) -def generate_test_description(ur_type): +def generate_test_description(): simulator = IncludeLaunchDescription( PythonLaunchDescriptionSource( PathJoinSubstitution( @@ -84,11 +85,10 @@ def generate_test_description(ur_type): ) ), launch_arguments={ - "ur_type": ur_type, - "launch_rviz": "true", + "ur_type": "ur5e", + "launch_rviz": "false", "gazebo_gui": "false", "start_joint_controller": "true", - # "gz_gui": "false", }.items(), ) return LaunchDescription([ReadyToTest(), simulator]) @@ -118,7 +118,7 @@ def init_robot(self): # TODO: Replace this timeout with a proper check whether the robot is initialized time.sleep(5) - def test_trajectory(self, ur_type): + def test_trajectory(self): """Test robot movement.""" # Construct test trajectory test_trajectory = [ From d20adab336d41ea6fdbc7d741685a252a504ab5d Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Mon, 1 Jul 2024 14:35:29 +0200 Subject: [PATCH 4/5] 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. --- .github/workflows/reusable_ici.yml | 1 + ur_simulation_gz/CMakeLists.txt | 17 +++++++++++++---- 2 files changed, 14 insertions(+), 4 deletions(-) diff --git a/.github/workflows/reusable_ici.yml b/.github/workflows/reusable_ici.yml index f056f41..611e072 100644 --- a/.github/workflows/reusable_ici.yml +++ b/.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: | diff --git a/ur_simulation_gz/CMakeLists.txt b/ur_simulation_gz/CMakeLists.txt index b547e1c..01354ef 100644 --- a/ur_simulation_gz/CMakeLists.txt +++ b/ur_simulation_gz/CMakeLists.txt @@ -3,6 +3,13 @@ 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} ) @@ -11,10 +18,12 @@ if(BUILD_TESTING) find_package(launch_testing_ament_cmake) find_package(ament_cmake_pytest REQUIRED) - add_launch_test(test/test_gz.py - TIMEOUT - 180 - ) + 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() From 44f9dc4bcf5b0437d997355236a2ad6f4f00d218 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Mon, 1 Jul 2024 15:15:05 +0200 Subject: [PATCH 5/5] Remove repos-not-released for jazzy --- .github/workflows/jazzy-binary-main.yml | 1 - .github/workflows/jazzy-binary-testing.yml | 1 - ur_simulation_gz-not-released.jazzy.repos | 5 ----- 3 files changed, 7 deletions(-) delete mode 100644 ur_simulation_gz-not-released.jazzy.repos diff --git a/.github/workflows/jazzy-binary-main.yml b/.github/workflows/jazzy-binary-main.yml index d69e700..b9b9b6e 100644 --- a/.github/workflows/jazzy-binary-main.yml +++ b/.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 diff --git a/.github/workflows/jazzy-binary-testing.yml b/.github/workflows/jazzy-binary-testing.yml index 44ddcc6..276f784 100644 --- a/.github/workflows/jazzy-binary-testing.yml +++ b/.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 diff --git a/ur_simulation_gz-not-released.jazzy.repos b/ur_simulation_gz-not-released.jazzy.repos deleted file mode 100644 index 9ec6deb..0000000 --- a/ur_simulation_gz-not-released.jazzy.repos +++ /dev/null @@ -1,5 +0,0 @@ -repositories: - gz_ros2_control: - type: git - url: https://github.com/ros-controls/gz_ros2_control.git - version: master