Skip to content

Commit 5579fc4

Browse files
URJalaURJala
committed
Split passthrough controller tests to its own file.
And into separate test cases. Moved timout for trajectory execution to common file
1 parent 74142b6 commit 5579fc4

File tree

3 files changed

+270
-144
lines changed

3 files changed

+270
-144
lines changed

‎ur_robot_driver/test/integration_test_passthrough_controller.py

Lines changed: 267 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,267 @@
1+
#!/usr/bin/env python
2+
# Copyright 2019, Universal Robots A/S
3+
#
4+
# Redistribution and use in source and binary forms, with or without
5+
# modification, are permitted provided that the following conditions are met:
6+
#
7+
# * Redistributions of source code must retain the above copyright
8+
# notice, this list of conditions and the following disclaimer.
9+
#
10+
# * Redistributions in binary form must reproduce the above copyright
11+
# notice, this list of conditions and the following disclaimer in the
12+
# documentation and/or other materials provided with the distribution.
13+
#
14+
# * Neither the name of the {copyright_holder} nor the names of its
15+
# contributors may be used to endorse or promote products derived from
16+
# this software without specific prior written permission.
17+
#
18+
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
19+
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
20+
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
21+
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
22+
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
23+
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
24+
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
25+
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
26+
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
27+
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
28+
# POSSIBILITY OF SUCH DAMAGE.
29+
30+
import os
31+
import sys
32+
import time
33+
import unittest
34+
35+
from math import pi
36+
import launch_testing
37+
import pytest
38+
import rclpy
39+
from builtin_interfaces.msg import Duration
40+
from control_msgs.action import FollowJointTrajectory
41+
from control_msgs.msg import JointTolerance
42+
from controller_manager_msgs.srv import SwitchController
43+
from rclpy.node import Node
44+
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
45+
46+
sys.path.append(os.path.dirname(__file__))
47+
from test_common import ( # noqa: E402
48+
ActionInterface,
49+
ControllerManagerInterface,
50+
DashboardInterface,
51+
IoStatusInterface,
52+
ConfigurationInterface,
53+
generate_driver_test_description,
54+
ROBOT_JOINTS,
55+
TIMEOUT_EXECUTE_TRAJECTORY,
56+
)
57+
58+
59+
# Mock hardware does not work with passthrough controller, so dont test with it
60+
@pytest.mark.launch_test
61+
@launch_testing.parametrize("tf_prefix", [(""), ("my_ur_")])
62+
def generate_test_description(tf_prefix):
63+
return generate_driver_test_description(tf_prefix=tf_prefix)
64+
65+
66+
HOME = {
67+
"elbow_joint": 0.0,
68+
"shoulder_lift_joint": -1.5708,
69+
"shoulder_pan_joint": 0.0,
70+
"wrist_1_joint": -1.5708,
71+
"wrist_2_joint": 0.0,
72+
"wrist_3_joint": 0.0,
73+
}
74+
waypts = [[HOME[joint] + i * pi / 4 for joint in ROBOT_JOINTS] for i in [0, -1, 1]]
75+
time_vec = [
76+
Duration(sec=4, nanosec=0),
77+
Duration(sec=8, nanosec=0),
78+
Duration(sec=12, nanosec=0),
79+
]
80+
TEST_TRAJECTORY = zip(time_vec, waypts)
81+
82+
83+
class RobotDriverTest(unittest.TestCase):
84+
@classmethod
85+
def setUpClass(cls, use_mock_hardware):
86+
# Initialize the ROS context
87+
rclpy.init()
88+
cls.node = Node("robot_driver_test")
89+
time.sleep(1)
90+
cls.init_robot(cls)
91+
92+
@classmethod
93+
def tearDownClass(cls):
94+
# Shutdown the ROS context
95+
cls.node.destroy_node()
96+
rclpy.shutdown()
97+
98+
def init_robot(self):
99+
100+
self._dashboard_interface = DashboardInterface(self.node)
101+
102+
self._controller_manager_interface = ControllerManagerInterface(self.node)
103+
self._io_status_controller_interface = IoStatusInterface(self.node)
104+
self._configuration_controller_interface = ConfigurationInterface(self.node)
105+
106+
self._scaled_follow_joint_trajectory = ActionInterface(
107+
self.node,
108+
"/scaled_joint_trajectory_controller/follow_joint_trajectory",
109+
FollowJointTrajectory,
110+
)
111+
self._passthrough_forward_joint_trajectory = ActionInterface(
112+
self.node,
113+
"/passthrough_trajectory_controller/follow_joint_trajectory",
114+
FollowJointTrajectory,
115+
)
116+
117+
def setUp(self):
118+
if self._dashboard_interface:
119+
self._dashboard_interface.start_robot()
120+
time.sleep(1)
121+
self.assertTrue(self._io_status_controller_interface.resend_robot_program().success)
122+
123+
#
124+
# Test functions
125+
#
126+
127+
def test_start_passthrough_controller(self):
128+
self.assertTrue(
129+
self._controller_manager_interface.switch_controller(
130+
strictness=SwitchController.Request.BEST_EFFORT,
131+
activate_controllers=["passthrough_trajectory_controller"],
132+
deactivate_controllers=["scaled_joint_trajectory_controller"],
133+
).ok
134+
)
135+
self.assertTrue(
136+
self._controller_manager_interface.switch_controller(
137+
strictness=SwitchController.Request.BEST_EFFORT,
138+
deactivate_controllers=["passthrough_trajectory_controller"],
139+
activate_controllers=["scaled_joint_trajectory_controller"],
140+
).ok
141+
)
142+
143+
def test_passthrough_trajectory(self, tf_prefix):
144+
self.assertTrue(
145+
self._controller_manager_interface.switch_controller(
146+
strictness=SwitchController.Request.BEST_EFFORT,
147+
activate_controllers=["passthrough_trajectory_controller"],
148+
deactivate_controllers=["scaled_joint_trajectory_controller"],
149+
).ok
150+
)
151+
152+
goal_tolerance = [
153+
JointTolerance(position=0.01, name=tf_prefix + joint) for joint in ROBOT_JOINTS
154+
]
155+
goal_time_tolerance = Duration(sec=1, nanosec=0)
156+
trajectory = JointTrajectory(
157+
points=[
158+
JointTrajectoryPoint(positions=pos, time_from_start=times)
159+
for (times, pos) in TEST_TRAJECTORY
160+
],
161+
joint_names=[tf_prefix + joint for joint in ROBOT_JOINTS],
162+
)
163+
goal_handle = self._passthrough_forward_joint_trajectory.send_goal(
164+
trajectory=trajectory,
165+
goal_time_tolerance=goal_time_tolerance,
166+
goal_tolerance=goal_tolerance,
167+
)
168+
self.assertTrue(goal_handle.accepted)
169+
if goal_handle.accepted:
170+
result = self._passthrough_forward_joint_trajectory.get_result(
171+
goal_handle, TIMEOUT_EXECUTE_TRAJECTORY
172+
)
173+
self.assertEqual(result.error_code, FollowJointTrajectory.Result.SUCCESSFUL)
174+
175+
def test_quintic_trajectory(self, tf_prefix):
176+
# Full quintic trajectory
177+
178+
trajectory = JointTrajectory(
179+
points=[
180+
JointTrajectoryPoint(
181+
positions=pos,
182+
time_from_start=times,
183+
velocities=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
184+
accelerations=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
185+
)
186+
for (times, pos) in TEST_TRAJECTORY
187+
],
188+
joint_names=[tf_prefix + joint for joint in ROBOT_JOINTS],
189+
)
190+
goal_time_tolerance = Duration(sec=1, nanosec=0)
191+
goal_tolerance = [
192+
JointTolerance(position=0.01, name=tf_prefix + joint) for joint in ROBOT_JOINTS
193+
]
194+
goal_handle = self._passthrough_forward_joint_trajectory.send_goal(
195+
trajectory=trajectory,
196+
goal_time_tolerance=goal_time_tolerance,
197+
goal_tolerance=goal_tolerance,
198+
)
199+
200+
self.assertTrue(goal_handle.accepted)
201+
if goal_handle.accepted:
202+
result = self._passthrough_forward_joint_trajectory.get_result(
203+
goal_handle, TIMEOUT_EXECUTE_TRAJECTORY
204+
)
205+
self.assertEqual(result.error_code, FollowJointTrajectory.Result.SUCCESSFUL)
206+
207+
def test_impossible_goal_tolerance_fails(self, tf_prefix):
208+
# Test impossible goal tolerance, should fail.
209+
210+
trajectory = JointTrajectory(
211+
points=[
212+
JointTrajectoryPoint(positions=pos, time_from_start=times)
213+
for (times, pos) in TEST_TRAJECTORY
214+
],
215+
joint_names=[tf_prefix + joint for joint in ROBOT_JOINTS],
216+
)
217+
goal_tolerance = [
218+
JointTolerance(position=0.000000001, name=tf_prefix + joint) for joint in ROBOT_JOINTS
219+
]
220+
goal_time_tolerance = Duration(sec=1, nanosec=0)
221+
goal_handle = self._passthrough_forward_joint_trajectory.send_goal(
222+
trajectory=trajectory,
223+
goal_time_tolerance=goal_time_tolerance,
224+
goal_tolerance=goal_tolerance,
225+
)
226+
self.assertTrue(goal_handle.accepted)
227+
if goal_handle.accepted:
228+
result = self._passthrough_forward_joint_trajectory.get_result(
229+
goal_handle, TIMEOUT_EXECUTE_TRAJECTORY
230+
)
231+
self.assertEqual(
232+
result.error_code, FollowJointTrajectory.Result.GOAL_TOLERANCE_VIOLATED
233+
)
234+
235+
def test_impossible_goal_time_tolerance_fails(self, tf_prefix):
236+
# Test impossible goal time
237+
goal_tolerance = [
238+
JointTolerance(position=0.01, name=tf_prefix + joint) for joint in ROBOT_JOINTS
239+
]
240+
goal_time_tolerance = Duration(sec=0, nanosec=10)
241+
trajectory = JointTrajectory(
242+
points=[
243+
JointTrajectoryPoint(positions=pos, time_from_start=times)
244+
for (times, pos) in TEST_TRAJECTORY
245+
],
246+
joint_names=[tf_prefix + joint for joint in ROBOT_JOINTS],
247+
)
248+
goal_handle = self._passthrough_forward_joint_trajectory.send_goal(
249+
trajectory=trajectory,
250+
goal_time_tolerance=goal_time_tolerance,
251+
goal_tolerance=goal_tolerance,
252+
)
253+
self.assertTrue(goal_handle.accepted)
254+
if goal_handle.accepted:
255+
result = self._passthrough_forward_joint_trajectory.get_result(
256+
goal_handle, TIMEOUT_EXECUTE_TRAJECTORY
257+
)
258+
self.assertEqual(
259+
result.error_code, FollowJointTrajectory.Result.GOAL_TOLERANCE_VIOLATED
260+
)
261+
self.assertTrue(
262+
self._controller_manager_interface.switch_controller(
263+
strictness=SwitchController.Request.BEST_EFFORT,
264+
deactivate_controllers=["passthrough_trajectory_controller"],
265+
activate_controllers=["scaled_joint_trajectory_controller"],
266+
).ok
267+
)

0 commit comments

Comments
 (0)