Skip to content

Commit 02938bb

Browse files
urmahpurmahp
committed
Added arguments to test launch
Added arguments when launching the test: - Added robot ip - Added launch_ursim to choose whether or not to launch ursim when running the test. - If the arguments are not provided the behavior is the same as before adding the arguments. Ensure that the scaled joint trajectory controller is running, before sending the motion.
1 parent 2ca4fd1 commit 02938bb

File tree

3 files changed

+145
-10
lines changed

3 files changed

+145
-10
lines changed

‎ur_robot_driver/test/dashboard_client.py‎

Lines changed: 24 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -35,6 +35,7 @@
3535
import rclpy
3636
from launch import LaunchDescription
3737
from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription
38+
from launch.conditions import IfCondition
3839
from launch.launch_description_sources import PythonLaunchDescriptionSource
3940
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
4041
from launch_ros.substitutions import FindPackagePrefix, FindPackageShare
@@ -69,7 +70,26 @@ def generate_test_description():
6970
)
7071
)
7172

73+
declared_arguments.append(
74+
DeclareLaunchArgument(
75+
"robot_ip",
76+
default_value="192.168.56.101",
77+
description="IP address of used UR robot.",
78+
)
79+
)
80+
81+
declared_arguments.append(
82+
DeclareLaunchArgument(
83+
"launch_ursim",
84+
default_value="true",
85+
description="Launches the ursim when running the test if yes"
86+
)
87+
)
88+
7289
ur_type = LaunchConfiguration("ur_type")
90+
robot_ip = LaunchConfiguration("robot_ip")
91+
launch_ursim = LaunchConfiguration("launch_ursim")
92+
7393

7494
dashboard_client = IncludeLaunchDescription(
7595
PythonLaunchDescriptionSource(
@@ -82,9 +102,10 @@ def generate_test_description():
82102
)
83103
),
84104
launch_arguments={
85-
"robot_ip": "192.168.56.101",
105+
"robot_ip": robot_ip,
86106
}.items(),
87107
)
108+
88109
ursim = ExecuteProcess(
89110
cmd=[
90111
PathJoinSubstitution(
@@ -101,6 +122,7 @@ def generate_test_description():
101122
],
102123
name="start_ursim",
103124
output="screen",
125+
condition=IfCondition(launch_ursim)
104126
)
105127

106128
return LaunchDescription(declared_arguments + [ReadyToTest(), dashboard_client, ursim])
@@ -159,7 +181,7 @@ def test_switch_on(self):
159181
# Wait until the robot is booted completely
160182
end_time = time.time() + 10
161183
mode = RobotMode.DISCONNECTED
162-
while mode != RobotMode.POWER_OFF and time.time() < end_time:
184+
while mode not in (RobotMode.POWER_OFF, RobotMode.IDLE, RobotMode.RUNNING) and time.time() < end_time:
163185
time.sleep(0.1)
164186
result = self.call_dashboard_service("get_robot_mode", GetRobotMode.Request())
165187
self.assertTrue(result.success)

‎ur_robot_driver/test/robot_driver.py‎

Lines changed: 76 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -44,6 +44,7 @@
4444
IncludeLaunchDescription,
4545
RegisterEventHandler,
4646
)
47+
from launch.conditions import IfCondition, UnlessCondition
4748
from launch.event_handlers import OnProcessExit
4849
from launch.launch_description_sources import PythonLaunchDescriptionSource
4950
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
@@ -57,6 +58,7 @@
5758
from ur_dashboard_msgs.srv import GetRobotMode
5859
from ur_msgs.msg import IOStates
5960
from ur_msgs.srv import SetIO
61+
from controller_manager_msgs.srv import ListControllers
6062

6163
TIMEOUT_WAIT_SERVICE = 10
6264
TIMEOUT_WAIT_SERVICE_INITIAL = 60
@@ -90,7 +92,25 @@ def generate_test_description(tf_prefix):
9092
)
9193
)
9294

95+
declared_arguments.append(
96+
DeclareLaunchArgument(
97+
"robot_ip",
98+
default_value="192.168.56.101",
99+
description="IP address of used UR robot.",
100+
)
101+
)
102+
103+
declared_arguments.append(
104+
DeclareLaunchArgument(
105+
"launch_ursim",
106+
default_value="true",
107+
description="Launches the ursim when running the test if True"
108+
)
109+
)
110+
93111
ur_type = LaunchConfiguration("ur_type")
112+
robot_ip = LaunchConfiguration("robot_ip")
113+
launch_ursim = LaunchConfiguration("launch_ursim")
94114

95115
robot_driver = IncludeLaunchDescription(
96116
PythonLaunchDescriptionSource(
@@ -109,7 +129,9 @@ def generate_test_description(tf_prefix):
109129
"start_joint_controller": "false",
110130
"tf_prefix": tf_prefix,
111131
}.items(),
132+
condition=IfCondition(launch_ursim)
112133
)
134+
113135
ursim = ExecuteProcess(
114136
cmd=[
115137
PathJoinSubstitution(
@@ -126,25 +148,49 @@ def generate_test_description(tf_prefix):
126148
],
127149
name="start_ursim",
128150
output="screen",
151+
condition=IfCondition(launch_ursim)
129152
)
153+
130154
wait_dashboard_server = ExecuteProcess(
131155
cmd=[
132156
PathJoinSubstitution(
133157
[FindPackagePrefix("ur_robot_driver"), "bin", "wait_dashboard_server.sh"]
134-
)
158+
),
135159
],
136160
name="wait_dashboard_server",
137161
output="screen",
162+
condition=IfCondition(launch_ursim)
138163
)
164+
139165
driver_starter = RegisterEventHandler(
140-
OnProcessExit(target_action=wait_dashboard_server, on_exit=robot_driver)
166+
OnProcessExit(target_action=wait_dashboard_server, on_exit=robot_driver),
167+
condition=IfCondition(launch_ursim)
168+
)
169+
170+
robot_driver_no_wait = IncludeLaunchDescription(
171+
PythonLaunchDescriptionSource(
172+
PathJoinSubstitution(
173+
[FindPackageShare("ur_robot_driver"), "launch", "ur_control.launch.py"]
174+
)
175+
),
176+
launch_arguments={
177+
"robot_ip": robot_ip,
178+
"ur_type": ur_type,
179+
"launch_rviz": "false",
180+
"controller_spawner_timeout": str(TIMEOUT_WAIT_SERVICE_INITIAL),
181+
"initial_joint_controller": "scaled_joint_trajectory_controller",
182+
"headless_mode": "true",
183+
"launch_dashboard_client": "false",
184+
"start_joint_controller": "false",
185+
"tf_prefix": tf_prefix,
186+
}.items(),
187+
condition=UnlessCondition(launch_ursim)
141188
)
142189

143190
return LaunchDescription(
144-
declared_arguments + [ReadyToTest(), wait_dashboard_server, ursim, driver_starter]
191+
declared_arguments + [ReadyToTest(), wait_dashboard_server, ursim, driver_starter, robot_driver_no_wait]
145192
)
146193

147-
148194
class RobotDriverTest(unittest.TestCase):
149195
@classmethod
150196
def setUpClass(cls):
@@ -184,6 +230,7 @@ def init_robot(self):
184230
"/dashboard_client/get_robot_mode": GetRobotMode,
185231
"/controller_manager/switch_controller": SwitchController,
186232
"/io_and_status_controller/resend_robot_program": Trigger,
233+
"/controller_manager/list_controllers": ListControllers,
187234
}
188235
self.service_clients.update(
189236
{
@@ -282,6 +329,9 @@ def io_msg_cb(msg):
282329

283330
def test_trajectory(self, tf_prefix):
284331
"""Test robot movement."""
332+
# Wait for controller to be active
333+
self.waitForController("scaled_joint_trajectory_controller")
334+
285335
# Construct test trajectory
286336
test_trajectory = [
287337
(Duration(sec=6, nanosec=0), [0.0 for j in ROBOT_JOINTS]),
@@ -320,6 +370,9 @@ def test_illegal_trajectory(self, tf_prefix):
320370
321371
This is more of a validation test that the testing suite does the right thing
322372
"""
373+
# Wait for controller to be active
374+
self.waitForController("scaled_joint_trajectory_controller")
375+
323376
# Construct test trajectory, the second point wrongly starts before the first
324377
test_trajectory = [
325378
(Duration(sec=6, nanosec=0), [0.0 for j in ROBOT_JOINTS]),
@@ -347,6 +400,9 @@ def test_illegal_trajectory(self, tf_prefix):
347400

348401
def test_trajectory_scaled(self, tf_prefix):
349402
"""Test robot movement."""
403+
# Wait for controller to be active
404+
self.waitForController("scaled_joint_trajectory_controller")
405+
350406
# Construct test trajectory
351407
test_trajectory = [
352408
(Duration(sec=6, nanosec=0), [0.0 for j in ROBOT_JOINTS]),
@@ -443,6 +499,22 @@ def get_result(self, action_name, goal_response, timeout):
443499
raise Exception(f"Exception while calling action: {future_res.exception()}")
444500

445501

502+
def waitForController(self, controller_name, controller_status="active", timeout=TIMEOUT_WAIT_SERVICE):
503+
controller_running = False
504+
end_time = time.time() + timeout
505+
while controller_running == False and time.time() < end_time:
506+
time.sleep(1)
507+
response = self.call_service(
508+
"/controller_manager/list_controllers", ListControllers.Request()
509+
)
510+
for controller in response.controller:
511+
if controller.name == controller_name:
512+
controller_running = controller.state == controller_status
513+
514+
if controller_running == False:
515+
raise Exception(
516+
f"Controller {controller_name} did not reach controller state {controller_status} within timeout of {timeout}")
517+
446518
def waitForService(node, srv_name, srv_type, timeout=TIMEOUT_WAIT_SERVICE):
447519
client = node.create_client(srv_type, srv_name)
448520
if client.wait_for_service(timeout) is False:

‎ur_robot_driver/test/urscript_interface.py‎

Lines changed: 45 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -40,6 +40,7 @@
4040
IncludeLaunchDescription,
4141
RegisterEventHandler,
4242
)
43+
from launch.conditions import IfCondition, UnlessCondition
4344
from launch.event_handlers import OnProcessExit
4445
from launch.launch_description_sources import PythonLaunchDescriptionSource
4546
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
@@ -79,7 +80,25 @@ def generate_test_description():
7980
)
8081
)
8182

83+
declared_arguments.append(
84+
DeclareLaunchArgument(
85+
"robot_ip",
86+
default_value="192.168.56.101",
87+
description="IP address of used UR robot.",
88+
)
89+
)
90+
91+
declared_arguments.append(
92+
DeclareLaunchArgument(
93+
"launch_ursim",
94+
default_value="true",
95+
description="Launches the ursim when running the test if True"
96+
)
97+
)
98+
8299
ur_type = LaunchConfiguration("ur_type")
100+
robot_ip = LaunchConfiguration("robot_ip")
101+
launch_ursim = LaunchConfiguration("launch_ursim")
83102

84103
robot_driver = IncludeLaunchDescription(
85104
PythonLaunchDescriptionSource(
@@ -97,6 +116,7 @@ def generate_test_description():
97116
"launch_dashboard_client": "false",
98117
"start_joint_controller": "false",
99118
}.items(),
119+
condition=IfCondition(launch_ursim)
100120
)
101121

102122
ursim = ExecuteProcess(
@@ -115,27 +135,48 @@ def generate_test_description():
115135
],
116136
name="start_ursim",
117137
output="screen",
138+
condition=IfCondition(launch_ursim)
118139
)
119140

120141
wait_dashboard_server = ExecuteProcess(
121142
cmd=[
122143
PathJoinSubstitution(
123144
[FindPackagePrefix("ur_robot_driver"), "bin", "wait_dashboard_server.sh"]
124-
)
145+
),
125146
],
126147
name="wait_dashboard_server",
127148
output="screen",
149+
condition=IfCondition(launch_ursim)
128150
)
129151

130152
driver_starter = RegisterEventHandler(
131-
OnProcessExit(target_action=wait_dashboard_server, on_exit=robot_driver)
153+
OnProcessExit(target_action=wait_dashboard_server, on_exit=robot_driver),
154+
condition=IfCondition(launch_ursim)
155+
)
156+
157+
robot_driver_no_wait = IncludeLaunchDescription(
158+
PythonLaunchDescriptionSource(
159+
PathJoinSubstitution(
160+
[FindPackageShare("ur_robot_driver"), "launch", "ur_control.launch.py"]
161+
)
162+
),
163+
launch_arguments={
164+
"robot_ip": robot_ip,
165+
"ur_type": ur_type,
166+
"launch_rviz": "false",
167+
"controller_spawner_timeout": str(TIMEOUT_WAIT_SERVICE_INITIAL),
168+
"initial_joint_controller": "scaled_joint_trajectory_controller",
169+
"headless_mode": "true",
170+
"launch_dashboard_client": "false",
171+
"start_joint_controller": "false",
172+
}.items(),
173+
condition=UnlessCondition(launch_ursim)
132174
)
133175

134176
return LaunchDescription(
135-
declared_arguments + [ReadyToTest(), wait_dashboard_server, driver_starter, ursim]
177+
declared_arguments + [ReadyToTest(), wait_dashboard_server, ursim, driver_starter, robot_driver_no_wait]
136178
)
137179

138-
139180
class URScriptInterfaceTest(unittest.TestCase):
140181
@classmethod
141182
def setUpClass(cls):

0 commit comments

Comments
 (0)