From e7e47d5dbe844020a49054f6082ea6e1cecd0055 Mon Sep 17 00:00:00 2001 From: mahp Date: Mon, 7 Aug 2023 15:43:00 +0200 Subject: [PATCH 1/4] 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. --- ur_robot_driver/test/dashboard_client.py | 28 +++++++- ur_robot_driver/test/robot_driver.py | 83 +++++++++++++++++++++- ur_robot_driver/test/urscript_interface.py | 49 ++++++++++++- 3 files changed, 152 insertions(+), 8 deletions(-) diff --git a/ur_robot_driver/test/dashboard_client.py b/ur_robot_driver/test/dashboard_client.py index 056a04d67..67afd2d87 100755 --- a/ur_robot_driver/test/dashboard_client.py +++ b/ur_robot_driver/test/dashboard_client.py @@ -35,6 +35,7 @@ import rclpy from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription +from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch_ros.substitutions import FindPackagePrefix, FindPackageShare @@ -69,7 +70,25 @@ def generate_test_description(): ) ) + declared_arguments.append( + DeclareLaunchArgument( + "robot_ip", + default_value="192.168.56.101", + description="IP address of used UR robot.", + ) + ) + + declared_arguments.append( + DeclareLaunchArgument( + "launch_ursim", + default_value="true", + description="Launches the ursim when running the test if yes", + ) + ) + ur_type = LaunchConfiguration("ur_type") + robot_ip = LaunchConfiguration("robot_ip") + launch_ursim = LaunchConfiguration("launch_ursim") dashboard_client = IncludeLaunchDescription( PythonLaunchDescriptionSource( @@ -82,9 +101,10 @@ def generate_test_description(): ) ), launch_arguments={ - "robot_ip": "192.168.56.101", + "robot_ip": robot_ip, }.items(), ) + ursim = ExecuteProcess( cmd=[ PathJoinSubstitution( @@ -101,6 +121,7 @@ def generate_test_description(): ], name="start_ursim", output="screen", + condition=IfCondition(launch_ursim), ) return LaunchDescription(declared_arguments + [ReadyToTest(), dashboard_client, ursim]) @@ -159,7 +180,10 @@ def test_switch_on(self): # Wait until the robot is booted completely end_time = time.time() + 10 mode = RobotMode.DISCONNECTED - while mode != RobotMode.POWER_OFF and time.time() < end_time: + while ( + mode not in (RobotMode.POWER_OFF, RobotMode.IDLE, RobotMode.RUNNING) + and time.time() < end_time + ): time.sleep(0.1) result = self.call_dashboard_service("get_robot_mode", GetRobotMode.Request()) self.assertTrue(result.success) diff --git a/ur_robot_driver/test/robot_driver.py b/ur_robot_driver/test/robot_driver.py index bb203414f..8f4bf0314 100755 --- a/ur_robot_driver/test/robot_driver.py +++ b/ur_robot_driver/test/robot_driver.py @@ -44,6 +44,7 @@ IncludeLaunchDescription, RegisterEventHandler, ) +from launch.conditions import IfCondition, UnlessCondition from launch.event_handlers import OnProcessExit from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PathJoinSubstitution @@ -57,6 +58,7 @@ from ur_dashboard_msgs.srv import GetRobotMode from ur_msgs.msg import IOStates from ur_msgs.srv import SetIO +from controller_manager_msgs.srv import ListControllers TIMEOUT_WAIT_SERVICE = 10 TIMEOUT_WAIT_SERVICE_INITIAL = 60 @@ -90,7 +92,25 @@ def generate_test_description(tf_prefix): ) ) + declared_arguments.append( + DeclareLaunchArgument( + "robot_ip", + default_value="192.168.56.101", + description="IP address of used UR robot.", + ) + ) + + declared_arguments.append( + DeclareLaunchArgument( + "launch_ursim", + default_value="true", + description="Launches the ursim when running the test if True", + ) + ) + ur_type = LaunchConfiguration("ur_type") + robot_ip = LaunchConfiguration("robot_ip") + launch_ursim = LaunchConfiguration("launch_ursim") robot_driver = IncludeLaunchDescription( PythonLaunchDescriptionSource( @@ -109,7 +129,9 @@ def generate_test_description(tf_prefix): "start_joint_controller": "false", "tf_prefix": tf_prefix, }.items(), + condition=IfCondition(launch_ursim), ) + ursim = ExecuteProcess( cmd=[ PathJoinSubstitution( @@ -126,22 +148,48 @@ def generate_test_description(tf_prefix): ], name="start_ursim", output="screen", + condition=IfCondition(launch_ursim), ) + wait_dashboard_server = ExecuteProcess( cmd=[ PathJoinSubstitution( [FindPackagePrefix("ur_robot_driver"), "bin", "wait_dashboard_server.sh"] - ) + ), ], name="wait_dashboard_server", output="screen", + condition=IfCondition(launch_ursim), ) + driver_starter = RegisterEventHandler( - OnProcessExit(target_action=wait_dashboard_server, on_exit=robot_driver) + OnProcessExit(target_action=wait_dashboard_server, on_exit=robot_driver), + condition=IfCondition(launch_ursim), + ) + + robot_driver_no_wait = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [FindPackageShare("ur_robot_driver"), "launch", "ur_control.launch.py"] + ) + ), + launch_arguments={ + "robot_ip": robot_ip, + "ur_type": ur_type, + "launch_rviz": "false", + "controller_spawner_timeout": str(TIMEOUT_WAIT_SERVICE_INITIAL), + "initial_joint_controller": "scaled_joint_trajectory_controller", + "headless_mode": "true", + "launch_dashboard_client": "false", + "start_joint_controller": "false", + "tf_prefix": tf_prefix, + }.items(), + condition=UnlessCondition(launch_ursim), ) return LaunchDescription( - declared_arguments + [ReadyToTest(), wait_dashboard_server, ursim, driver_starter] + declared_arguments + + [ReadyToTest(), wait_dashboard_server, ursim, driver_starter, robot_driver_no_wait] ) @@ -184,6 +232,7 @@ def init_robot(self): "/dashboard_client/get_robot_mode": GetRobotMode, "/controller_manager/switch_controller": SwitchController, "/io_and_status_controller/resend_robot_program": Trigger, + "/controller_manager/list_controllers": ListControllers, } self.service_clients.update( { @@ -282,6 +331,9 @@ def io_msg_cb(msg): def test_trajectory(self, tf_prefix): """Test robot movement.""" + # Wait for controller to be active + self.waitForController("scaled_joint_trajectory_controller") + # Construct test trajectory test_trajectory = [ (Duration(sec=6, nanosec=0), [0.0 for j in ROBOT_JOINTS]), @@ -320,6 +372,9 @@ def test_illegal_trajectory(self, tf_prefix): This is more of a validation test that the testing suite does the right thing """ + # Wait for controller to be active + self.waitForController("scaled_joint_trajectory_controller") + # Construct test trajectory, the second point wrongly starts before the first test_trajectory = [ (Duration(sec=6, nanosec=0), [0.0 for j in ROBOT_JOINTS]), @@ -347,6 +402,9 @@ def test_illegal_trajectory(self, tf_prefix): def test_trajectory_scaled(self, tf_prefix): """Test robot movement.""" + # Wait for controller to be active + self.waitForController("scaled_joint_trajectory_controller") + # Construct test trajectory test_trajectory = [ (Duration(sec=6, nanosec=0), [0.0 for j in ROBOT_JOINTS]), @@ -442,6 +500,25 @@ def get_result(self, action_name, goal_response, timeout): else: raise Exception(f"Exception while calling action: {future_res.exception()}") + def waitForController( + self, controller_name, controller_status="active", timeout=TIMEOUT_WAIT_SERVICE + ): + controller_running = False + end_time = time.time() + timeout + while controller_running is False and time.time() < end_time: + time.sleep(1) + response = self.call_service( + "/controller_manager/list_controllers", ListControllers.Request() + ) + for controller in response.controller: + if controller.name == controller_name: + controller_running = controller.state == controller_status + + if controller_running is False: + raise Exception( + f"Controller {controller_name} did not reach controller state {controller_status} within timeout of {timeout}" + ) + def waitForService(node, srv_name, srv_type, timeout=TIMEOUT_WAIT_SERVICE): client = node.create_client(srv_type, srv_name) diff --git a/ur_robot_driver/test/urscript_interface.py b/ur_robot_driver/test/urscript_interface.py index 1a303a7a1..7d29d1996 100755 --- a/ur_robot_driver/test/urscript_interface.py +++ b/ur_robot_driver/test/urscript_interface.py @@ -40,6 +40,7 @@ IncludeLaunchDescription, RegisterEventHandler, ) +from launch.conditions import IfCondition, UnlessCondition from launch.event_handlers import OnProcessExit from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PathJoinSubstitution @@ -79,7 +80,25 @@ def generate_test_description(): ) ) + declared_arguments.append( + DeclareLaunchArgument( + "robot_ip", + default_value="192.168.56.101", + description="IP address of used UR robot.", + ) + ) + + declared_arguments.append( + DeclareLaunchArgument( + "launch_ursim", + default_value="true", + description="Launches the ursim when running the test if True", + ) + ) + ur_type = LaunchConfiguration("ur_type") + robot_ip = LaunchConfiguration("robot_ip") + launch_ursim = LaunchConfiguration("launch_ursim") robot_driver = IncludeLaunchDescription( PythonLaunchDescriptionSource( @@ -97,6 +116,7 @@ def generate_test_description(): "launch_dashboard_client": "false", "start_joint_controller": "false", }.items(), + condition=IfCondition(launch_ursim), ) ursim = ExecuteProcess( @@ -115,24 +135,47 @@ def generate_test_description(): ], name="start_ursim", output="screen", + condition=IfCondition(launch_ursim), ) wait_dashboard_server = ExecuteProcess( cmd=[ PathJoinSubstitution( [FindPackagePrefix("ur_robot_driver"), "bin", "wait_dashboard_server.sh"] - ) + ), ], name="wait_dashboard_server", output="screen", + condition=IfCondition(launch_ursim), ) driver_starter = RegisterEventHandler( - OnProcessExit(target_action=wait_dashboard_server, on_exit=robot_driver) + OnProcessExit(target_action=wait_dashboard_server, on_exit=robot_driver), + condition=IfCondition(launch_ursim), + ) + + robot_driver_no_wait = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [FindPackageShare("ur_robot_driver"), "launch", "ur_control.launch.py"] + ) + ), + launch_arguments={ + "robot_ip": robot_ip, + "ur_type": ur_type, + "launch_rviz": "false", + "controller_spawner_timeout": str(TIMEOUT_WAIT_SERVICE_INITIAL), + "initial_joint_controller": "scaled_joint_trajectory_controller", + "headless_mode": "true", + "launch_dashboard_client": "false", + "start_joint_controller": "false", + }.items(), + condition=UnlessCondition(launch_ursim), ) return LaunchDescription( - declared_arguments + [ReadyToTest(), wait_dashboard_server, driver_starter, ursim] + declared_arguments + + [ReadyToTest(), wait_dashboard_server, ursim, driver_starter, robot_driver_no_wait] ) From 22d2cd41ebcc847ae17c26ec0cb75e850a0f1ad2 Mon Sep 17 00:00:00 2001 From: mahp Date: Tue, 29 Aug 2023 14:13:35 +0200 Subject: [PATCH 2/4] Removed robot driver duplication and added robot_ip to wait for dashboard --- .../scripts/wait_dashboard_server.sh | 10 +++++-- ur_robot_driver/test/robot_driver.py | 28 ++----------------- ur_robot_driver/test/urscript_interface.py | 27 ++---------------- 3 files changed, 14 insertions(+), 51 deletions(-) diff --git a/ur_robot_driver/scripts/wait_dashboard_server.sh b/ur_robot_driver/scripts/wait_dashboard_server.sh index 69e087bf6..21adf00cf 100755 --- a/ur_robot_driver/scripts/wait_dashboard_server.sh +++ b/ur_robot_driver/scripts/wait_dashboard_server.sh @@ -1,11 +1,17 @@ #!/bin/bash -netcat -z 192.168.56.101 29999 +ip="192.168.56.101" +if [[ ! -z "$1" ]] +then + ip=$1 +fi + +netcat -z $ip 29999 while [ $? -eq 1 ] do echo "Dashboard server not accepting connections..." sleep 3 - netcat -z 192.168.56.101 29999 + netcat -z $ip 29999 done echo "Dashboard server connections are possible." sleep 5 diff --git a/ur_robot_driver/test/robot_driver.py b/ur_robot_driver/test/robot_driver.py index 8f4bf0314..896cb7074 100755 --- a/ur_robot_driver/test/robot_driver.py +++ b/ur_robot_driver/test/robot_driver.py @@ -119,7 +119,7 @@ def generate_test_description(tf_prefix): ) ), launch_arguments={ - "robot_ip": "192.168.56.101", + "robot_ip": robot_ip, "ur_type": ur_type, "launch_rviz": "false", "controller_spawner_timeout": str(TIMEOUT_WAIT_SERVICE_INITIAL), @@ -129,7 +129,6 @@ def generate_test_description(tf_prefix): "start_joint_controller": "false", "tf_prefix": tf_prefix, }.items(), - condition=IfCondition(launch_ursim), ) ursim = ExecuteProcess( @@ -156,40 +155,19 @@ def generate_test_description(tf_prefix): PathJoinSubstitution( [FindPackagePrefix("ur_robot_driver"), "bin", "wait_dashboard_server.sh"] ), + robot_ip, ], name="wait_dashboard_server", output="screen", - condition=IfCondition(launch_ursim), ) driver_starter = RegisterEventHandler( OnProcessExit(target_action=wait_dashboard_server, on_exit=robot_driver), - condition=IfCondition(launch_ursim), - ) - - robot_driver_no_wait = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [FindPackageShare("ur_robot_driver"), "launch", "ur_control.launch.py"] - ) - ), - launch_arguments={ - "robot_ip": robot_ip, - "ur_type": ur_type, - "launch_rviz": "false", - "controller_spawner_timeout": str(TIMEOUT_WAIT_SERVICE_INITIAL), - "initial_joint_controller": "scaled_joint_trajectory_controller", - "headless_mode": "true", - "launch_dashboard_client": "false", - "start_joint_controller": "false", - "tf_prefix": tf_prefix, - }.items(), - condition=UnlessCondition(launch_ursim), ) return LaunchDescription( declared_arguments - + [ReadyToTest(), wait_dashboard_server, ursim, driver_starter, robot_driver_no_wait] + + [ReadyToTest(), wait_dashboard_server, ursim, driver_starter] ) diff --git a/ur_robot_driver/test/urscript_interface.py b/ur_robot_driver/test/urscript_interface.py index 7d29d1996..53f995267 100755 --- a/ur_robot_driver/test/urscript_interface.py +++ b/ur_robot_driver/test/urscript_interface.py @@ -107,7 +107,7 @@ def generate_test_description(): ) ), launch_arguments={ - "robot_ip": "192.168.56.101", + "robot_ip": robot_ip, "ur_type": ur_type, "launch_rviz": "false", "controller_spawner_timeout": str(TIMEOUT_WAIT_SERVICE_INITIAL), @@ -116,7 +116,6 @@ def generate_test_description(): "launch_dashboard_client": "false", "start_joint_controller": "false", }.items(), - condition=IfCondition(launch_ursim), ) ursim = ExecuteProcess( @@ -143,39 +142,19 @@ def generate_test_description(): PathJoinSubstitution( [FindPackagePrefix("ur_robot_driver"), "bin", "wait_dashboard_server.sh"] ), + robot_ip, ], name="wait_dashboard_server", output="screen", - condition=IfCondition(launch_ursim), ) driver_starter = RegisterEventHandler( OnProcessExit(target_action=wait_dashboard_server, on_exit=robot_driver), - condition=IfCondition(launch_ursim), - ) - - robot_driver_no_wait = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [FindPackageShare("ur_robot_driver"), "launch", "ur_control.launch.py"] - ) - ), - launch_arguments={ - "robot_ip": robot_ip, - "ur_type": ur_type, - "launch_rviz": "false", - "controller_spawner_timeout": str(TIMEOUT_WAIT_SERVICE_INITIAL), - "initial_joint_controller": "scaled_joint_trajectory_controller", - "headless_mode": "true", - "launch_dashboard_client": "false", - "start_joint_controller": "false", - }.items(), - condition=UnlessCondition(launch_ursim), ) return LaunchDescription( declared_arguments - + [ReadyToTest(), wait_dashboard_server, ursim, driver_starter, robot_driver_no_wait] + + [ReadyToTest(), wait_dashboard_server, ursim, driver_starter] ) From 2ad559caacf25c4e62d61cc0797aa6dca7142e53 Mon Sep 17 00:00:00 2001 From: mahp Date: Tue, 29 Aug 2023 15:04:37 +0200 Subject: [PATCH 3/4] Formatting. --- ur_robot_driver/test/robot_driver.py | 3 +-- ur_robot_driver/test/urscript_interface.py | 3 +-- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/ur_robot_driver/test/robot_driver.py b/ur_robot_driver/test/robot_driver.py index 896cb7074..31448119d 100755 --- a/ur_robot_driver/test/robot_driver.py +++ b/ur_robot_driver/test/robot_driver.py @@ -166,8 +166,7 @@ def generate_test_description(tf_prefix): ) return LaunchDescription( - declared_arguments - + [ReadyToTest(), wait_dashboard_server, ursim, driver_starter] + declared_arguments + [ReadyToTest(), wait_dashboard_server, ursim, driver_starter] ) diff --git a/ur_robot_driver/test/urscript_interface.py b/ur_robot_driver/test/urscript_interface.py index 53f995267..cb2f52e17 100755 --- a/ur_robot_driver/test/urscript_interface.py +++ b/ur_robot_driver/test/urscript_interface.py @@ -153,8 +153,7 @@ def generate_test_description(): ) return LaunchDescription( - declared_arguments - + [ReadyToTest(), wait_dashboard_server, ursim, driver_starter] + declared_arguments + [ReadyToTest(), wait_dashboard_server, ursim, driver_starter] ) From 3d386082cdf3ba45aba1380270533c55e07275fb Mon Sep 17 00:00:00 2001 From: mahp Date: Tue, 29 Aug 2023 15:08:50 +0200 Subject: [PATCH 4/4] More formatting --- ur_robot_driver/test/robot_driver.py | 2 +- ur_robot_driver/test/urscript_interface.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/ur_robot_driver/test/robot_driver.py b/ur_robot_driver/test/robot_driver.py index 31448119d..e6269ee45 100755 --- a/ur_robot_driver/test/robot_driver.py +++ b/ur_robot_driver/test/robot_driver.py @@ -44,7 +44,7 @@ IncludeLaunchDescription, RegisterEventHandler, ) -from launch.conditions import IfCondition, UnlessCondition +from launch.conditions import IfCondition from launch.event_handlers import OnProcessExit from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PathJoinSubstitution diff --git a/ur_robot_driver/test/urscript_interface.py b/ur_robot_driver/test/urscript_interface.py index cb2f52e17..e291808d4 100755 --- a/ur_robot_driver/test/urscript_interface.py +++ b/ur_robot_driver/test/urscript_interface.py @@ -40,7 +40,7 @@ IncludeLaunchDescription, RegisterEventHandler, ) -from launch.conditions import IfCondition, UnlessCondition +from launch.conditions import IfCondition from launch.event_handlers import OnProcessExit from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PathJoinSubstitution