Skip to content

Commit

Permalink
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Add tests
Browse files Browse the repository at this point in the history
Currently, there is one test executing a joint trajectory on the spawned robot.
fmauch committed Nov 19, 2023
1 parent b70ca70 commit 0f92b46
Showing 4 changed files with 339 additions and 5 deletions.
8 changes: 8 additions & 0 deletions ur_simulation_gazebo/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -3,6 +3,14 @@ project(ur_simulation_gazebo)

find_package(ament_cmake REQUIRED)

if(BUILD_TESTING)
find_package(launch_testing_ament_cmake)
add_launch_test(test/test_gazebo.py
TIMEOUT
180
)
endif()

install(DIRECTORY config launch
DESTINATION share/${PROJECT_NAME}
)
16 changes: 11 additions & 5 deletions ur_simulation_gazebo/launch/ur_sim_control.launch.py
Original file line number Diff line number Diff line change
@@ -44,7 +44,6 @@


def launch_setup(context, *args, **kwargs):

# Initialize Arguments
ur_type = LaunchConfiguration("ur_type")
safety_limits = LaunchConfiguration("safety_limits")
@@ -59,6 +58,7 @@ def launch_setup(context, *args, **kwargs):
start_joint_controller = LaunchConfiguration("start_joint_controller")
initial_joint_controller = LaunchConfiguration("initial_joint_controller")
launch_rviz = LaunchConfiguration("launch_rviz")
gazebo_gui = LaunchConfiguration("gazebo_gui")

initial_joint_controllers = PathJoinSubstitution(
[FindPackageShare(runtime_config_package), "config", controllers_file]
@@ -72,9 +72,7 @@ def launch_setup(context, *args, **kwargs):
[
PathJoinSubstitution([FindExecutable(name="xacro")]),
" ",
PathJoinSubstitution(
[FindPackageShare(description_package), "urdf", description_file]
),
PathJoinSubstitution([FindPackageShare(description_package), "urdf", description_file]),
" ",
"safety_limits:=",
safety_limits,
@@ -91,7 +89,7 @@ def launch_setup(context, *args, **kwargs):
"ur_type:=",
ur_type,
" ",
"prefix:=",
"tf_prefix:=",
prefix,
" ",
"sim_gazebo:=true",
@@ -151,6 +149,9 @@ def launch_setup(context, *args, **kwargs):
PythonLaunchDescriptionSource(
[FindPackageShare("gazebo_ros"), "/launch", "/gazebo.launch.py"]
),
launch_arguments={
"gui": gazebo_gui,
}.items(),
)

# Spawn robot
@@ -264,5 +265,10 @@ def generate_launch_description():
declared_arguments.append(
DeclareLaunchArgument("launch_rviz", default_value="true", description="Launch RViz?")
)
declared_arguments.append(
DeclareLaunchArgument(
"gazebo_gui", default_value="true", description="Start gazebo with GUI?"
)
)

return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)])
173 changes: 173 additions & 0 deletions ur_simulation_gazebo/test/test_common.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,173 @@
# 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 time

import rclpy
from controller_manager_msgs.srv import ListControllers, SwitchController
from launch import LaunchDescription
from launch.actions import (
DeclareLaunchArgument,
ExecuteProcess,
IncludeLaunchDescription,
RegisterEventHandler,
)
from launch.event_handlers import OnProcessExit
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch_ros.substitutions import FindPackagePrefix, FindPackageShare
from launch_testing.actions import ReadyToTest
from rclpy.action import ActionClient
from std_srvs.srv import Trigger

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)

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()}"
)




147 changes: 147 additions & 0 deletions ur_simulation_gazebo/test/test_gazebo.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,147 @@
#!/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 (
DeclareLaunchArgument,
ExecuteProcess,
IncludeLaunchDescription,
RegisterEventHandler,
)
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.substitutions import FindPackagePrefix, 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,
)


TIMEOUT_EXECUTE_TRAJECTORY = 30

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: Add ur20
@pytest.mark.launch_test
@launch_testing.parametrize(
"ur_type",
["ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e"],
)
def generate_test_description(ur_type):
gazebo = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
PathJoinSubstitution(
[FindPackageShare("ur_simulation_gazebo"), "launch", "ur_sim_control.launch.py"]
)
),
launch_arguments={
"ur_type": ur_type,
"launch_rviz": "false",
"start_joint_controller": "true",
"launch_rviz": "false",
"gazebo_gui": "false",
}.items(),
)
return LaunchDescription([ReadyToTest(), gazebo])


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,
)

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)

0 comments on commit 0f92b46

Please sign in to comment.