diff --git a/ur_robot_driver/test/dashboard_client.py b/ur_robot_driver/test/dashboard_client.py index 54beea2dd..d55d27828 100755 --- a/ur_robot_driver/test/dashboard_client.py +++ b/ur_robot_driver/test/dashboard_client.py @@ -26,84 +26,23 @@ # 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 os +import sys import time import unittest import pytest import rclpy -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription -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.node import Node -from std_srvs.srv import Trigger from ur_dashboard_msgs.msg import RobotMode -from ur_dashboard_msgs.srv import ( - GetLoadedProgram, - GetProgramState, - GetRobotMode, - IsProgramRunning, - Load, -) -TIMEOUT_WAIT_SERVICE = 10 -# 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_SERVICE_INITIAL = 120 +sys.path.append(os.path.dirname(__file__)) +from test_common import DashboardInterface, generate_dashboard_test_description # noqa: E402 @pytest.mark.launch_test def generate_test_description(): - declared_arguments = [] - - declared_arguments.append( - DeclareLaunchArgument( - "ur_type", - default_value="ur5e", - description="Type/series of used UR robot.", - choices=["ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e", "ur20"], - ) - ) - - ur_type = LaunchConfiguration("ur_type") - - dashboard_client = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [ - FindPackageShare("ur_robot_driver"), - "launch", - "ur_dashboard_client.launch.py", - ] - ) - ), - launch_arguments={ - "robot_ip": "192.168.56.101", - }.items(), - ) - ursim = ExecuteProcess( - cmd=[ - PathJoinSubstitution( - [ - FindPackagePrefix("ur_client_library"), - "lib", - "ur_client_library", - "start_ursim.sh", - ] - ), - " ", - "-m ", - ur_type, - ], - name="start_ursim", - output="screen", - ) - - return LaunchDescription(declared_arguments + [ReadyToTest(), dashboard_client, ursim]) + return generate_dashboard_test_description() class DashboardClientTest(unittest.TestCase): @@ -121,39 +60,7 @@ def tearDownClass(cls): rclpy.shutdown() def init_robot(self): - - # We wait longer for the first client, as the robot is still starting up - power_on_client = waitForService( - self.node, "/dashboard_client/power_on", Trigger, timeout=TIMEOUT_WAIT_SERVICE_INITIAL - ) - - # Connect to all other expected services - dashboard_interfaces = { - "power_off": Trigger, - "brake_release": Trigger, - "unlock_protective_stop": Trigger, - "restart_safety": Trigger, - "get_robot_mode": GetRobotMode, - "load_installation": Load, - "load_program": Load, - "close_popup": Trigger, - "get_loaded_program": GetLoadedProgram, - "program_state": GetProgramState, - "program_running": IsProgramRunning, - "play": Trigger, - "stop": Trigger, - } - self.dashboard_clients = { - srv_name: waitForService(self.node, f"/dashboard_client/{srv_name}", srv_type) - for (srv_name, srv_type) in dashboard_interfaces.items() - } - - # Add first client to dict - self.dashboard_clients["power_on"] = power_on_client - - # - # Test functions - # + self._dashboard_interface = DashboardInterface(self.node) def test_switch_on(self): """Test power on a robot.""" @@ -162,59 +69,34 @@ def test_switch_on(self): mode = RobotMode.DISCONNECTED while mode != RobotMode.POWER_OFF and time.time() < end_time: time.sleep(0.1) - result = self.call_dashboard_service("get_robot_mode", GetRobotMode.Request()) + result = self._dashboard_interface.get_robot_mode() self.assertTrue(result.success) mode = result.robot_mode.mode # Power on robot - self.assertTrue(self.call_dashboard_service("power_on", Trigger.Request()).success) + self.assertTrue(self._dashboard_interface.power_on().success) # Wait until robot mode changes end_time = time.time() + 10 mode = RobotMode.DISCONNECTED while mode not in (RobotMode.IDLE, RobotMode.RUNNING) and time.time() < end_time: time.sleep(0.1) - result = self.call_dashboard_service("get_robot_mode", GetRobotMode.Request()) + result = self._dashboard_interface.get_robot_mode() self.assertTrue(result.success) mode = result.robot_mode.mode self.assertIn(mode, (RobotMode.IDLE, RobotMode.RUNNING)) # Release robot brakes - self.assertTrue(self.call_dashboard_service("brake_release", Trigger.Request()).success) + self.assertTrue(self._dashboard_interface.brake_release().success) # Wait until robot mode is RUNNING end_time = time.time() + 10 mode = RobotMode.DISCONNECTED while mode != RobotMode.RUNNING and time.time() < end_time: time.sleep(0.1) - result = self.call_dashboard_service("get_robot_mode", GetRobotMode.Request()) + result = self._dashboard_interface.get_robot_mode() self.assertTrue(result.success) mode = result.robot_mode.mode self.assertEqual(mode, RobotMode.RUNNING) - - # - # Utility functions - # - - def call_dashboard_service(self, srv_name, request): - self.node.get_logger().info( - f"Calling dashboard service '{srv_name}' with request {request}" - ) - future = self.dashboard_clients[srv_name].call_async(request) - rclpy.spin_until_future_complete(self.node, future) - if future.result() is not None: - self.node.get_logger().info(f"Received result {future.result()}") - return future.result() - else: - raise Exception(f"Exception while calling service: {future.exception()}") - - -def waitForService(node, srv_name, srv_type, timeout=TIMEOUT_WAIT_SERVICE): - client = node.create_client(srv_type, srv_name) - if client.wait_for_service(timeout) is False: - raise Exception(f"Could not reach service '{srv_name}' within timeout of {timeout}") - - node.get_logger().info(f"Successfully connected to service '{srv_name}'") - return client diff --git a/ur_robot_driver/test/robot_driver.py b/ur_robot_driver/test/robot_driver.py index 2e00dfd6b..96c859947 100644 --- a/ur_robot_driver/test/robot_driver.py +++ b/ur_robot_driver/test/robot_driver.py @@ -26,8 +26,9 @@ # 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 sys import time import unittest @@ -37,31 +38,20 @@ from builtin_interfaces.msg import Duration from control_msgs.action import FollowJointTrajectory from controller_manager_msgs.srv import 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 rclpy.node import Node from sensor_msgs.msg import JointState -from std_srvs.srv import Trigger from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint -from ur_dashboard_msgs.msg import RobotMode -from ur_dashboard_msgs.srv import GetRobotMode from ur_msgs.msg import IOStates -from ur_msgs.srv import SetIO -TIMEOUT_WAIT_SERVICE = 10 -TIMEOUT_WAIT_SERVICE_INITIAL = 60 -TIMEOUT_WAIT_ACTION = 10 +sys.path.append(os.path.dirname(__file__)) +from test_common import ( # noqa: E402 + ActionInterface, + ControllerManagerInterface, + DashboardInterface, + IoStatusInterface, + generate_driver_test_description, +) + TIMEOUT_EXECUTE_TRAJECTORY = 30 ROBOT_JOINTS = [ @@ -80,70 +70,7 @@ [(""), ("my_ur_")], ) def generate_test_description(tf_prefix): - declared_arguments = [] - - declared_arguments.append( - DeclareLaunchArgument( - "ur_type", - default_value="ur5e", - description="Type/series of used UR robot.", - choices=["ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e", "ur20"], - ) - ) - - ur_type = LaunchConfiguration("ur_type") - - robot_driver = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [FindPackageShare("ur_robot_driver"), "launch", "ur_control.launch.py"] - ) - ), - launch_arguments={ - "robot_ip": "192.168.56.101", - "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(), - ) - ursim = ExecuteProcess( - cmd=[ - PathJoinSubstitution( - [ - FindPackagePrefix("ur_client_library"), - "lib", - "ur_client_library", - "start_ursim.sh", - ] - ), - " ", - "-m ", - ur_type, - ], - name="start_ursim", - output="screen", - ) - wait_dashboard_server = ExecuteProcess( - cmd=[ - PathJoinSubstitution( - [FindPackagePrefix("ur_robot_driver"), "bin", "wait_dashboard_server.sh"] - ) - ], - name="wait_dashboard_server", - output="screen", - ) - driver_starter = RegisterEventHandler( - OnProcessExit(target_action=wait_dashboard_server, on_exit=robot_driver) - ) - - return LaunchDescription( - declared_arguments + [ReadyToTest(), wait_dashboard_server, ursim, driver_starter] - ) + return generate_driver_test_description(tf_prefix=tf_prefix) class RobotDriverTest(unittest.TestCase): @@ -162,71 +89,32 @@ def tearDownClass(cls): rclpy.shutdown() def init_robot(self): + self._dashboard_interface = DashboardInterface(self.node) + self._controller_manager_interface = ControllerManagerInterface(self.node) + self._io_status_controller_interface = IoStatusInterface(self.node) - # Wait longer for the first service clients: - # - The robot has to start up - # - The controller_manager has to start - # - The controllers need to load and activate - service_interfaces_initial = { - "/dashboard_client/power_on": Trigger, - "/controller_manager/switch_controller": SwitchController, - "/io_and_status_controller/set_io": SetIO, - } - self.service_clients = { - srv_name: waitForService( - self.node, srv_name, srv_type, timeout=TIMEOUT_WAIT_SERVICE_INITIAL - ) - for (srv_name, srv_type) in service_interfaces_initial.items() - } - - # Connect to the rest of the required interfaces - service_interfaces = { - "/dashboard_client/brake_release": Trigger, - "/dashboard_client/stop": Trigger, - "/dashboard_client/get_robot_mode": GetRobotMode, - "/controller_manager/switch_controller": SwitchController, - "/io_and_status_controller/resend_robot_program": Trigger, - } - self.service_clients.update( - { - srv_name: waitForService(self.node, srv_name, srv_type) - for (srv_name, srv_type) in service_interfaces.items() - } + self._scaled_follow_joint_trajectory = ActionInterface( + self.node, + "/scaled_joint_trajectory_controller/follow_joint_trajectory", + FollowJointTrajectory, ) - action_interfaces = { - "/scaled_joint_trajectory_controller/follow_joint_trajectory": FollowJointTrajectory - } - self.action_clients = { - action_name: waitForAction(self.node, action_name, action_type) - for (action_name, action_type) in action_interfaces.items() - } - def setUp(self): - # Start robot - empty_req = Trigger.Request() - self.call_service("/dashboard_client/power_on", empty_req) - self.call_service("/dashboard_client/brake_release", empty_req) - time.sleep(1) - robot_mode_resp = self.call_service( - "/dashboard_client/get_robot_mode", GetRobotMode.Request() - ) - self.assertEqual(robot_mode_resp.robot_mode.mode, RobotMode.RUNNING) - self.call_service("/dashboard_client/stop", empty_req) + self._dashboard_interface.start_robot() time.sleep(1) - self.call_service("/io_and_status_controller/resend_robot_program", empty_req) + self.assertTrue(self._io_status_controller_interface.resend_robot_program().success) # # Test functions # def test_start_scaled_jtc_controller(self): - req = SwitchController.Request() - req.strictness = SwitchController.Request.BEST_EFFORT - req.start_controllers = ["scaled_joint_trajectory_controller"] - result = self.call_service("/controller_manager/switch_controller", req) - - self.assertEqual(result.ok, True) + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + activate_controllers=["scaled_joint_trajectory_controller"], + ).ok + ) def test_set_io(self): """Test to set an IO and check whether it has been set.""" @@ -247,13 +135,8 @@ def io_msg_cb(msg): # Set pin 0 to 1.0 test_pin = 0 - set_io_req = SetIO.Request() - set_io_req.fun = 1 - set_io_req.pin = test_pin - set_io_req.state = 1.0 - - self.node.get_logger().info(f"Setting pin {test_pin} to {set_io_req.state}") - self.call_service("/io_and_status_controller/set_io", set_io_req) + logging.info("Setting pin %d to 1.0", test_pin) + self._io_status_controller_interface.set_io(fun=1, pin=test_pin, state=1.0) # Wait until the pin state has changed pin_state = False @@ -263,12 +146,11 @@ def io_msg_cb(msg): if io_msg is not None: pin_state = io_msg.digital_out_states[test_pin].state - self.assertEqual(pin_state, set_io_req.state) + self.assertEqual(pin_state, 1.0) # Set pin 0 to 0.0 - set_io_req.state = 0.0 - self.node.get_logger().info(f"Setting pin {test_pin} to {set_io_req.state}") - self.call_service("/io_and_status_controller/set_io", set_io_req) + logging.info("Setting pin %d to 0.0", test_pin) + self._io_status_controller_interface.set_io(fun=1, pin=test_pin, state=0.0) # Wait until the pin state has changed back end_time = time.time() + 5 @@ -277,7 +159,7 @@ def io_msg_cb(msg): if io_msg is not None: pin_state = io_msg.digital_out_states[test_pin].state - self.assertEqual(pin_state, set_io_req.state) + self.assertEqual(pin_state, 0.0) # Clean up io subscription self.node.destroy_subscription(io_states_sub) @@ -300,21 +182,15 @@ def test_trajectory(self, tf_prefix): ) # Sending trajectory goal - self.node.get_logger().info("Sending simple goal") - goal_response = self.call_action( - "/scaled_joint_trajectory_controller/follow_joint_trajectory", - FollowJointTrajectory.Goal(trajectory=trajectory), - ) - self.assertEqual(goal_response.accepted, True) + logging.info("Sending simple goal") + goal_handle = self._scaled_follow_joint_trajectory.send_goal(trajectory=trajectory) + self.assertTrue(goal_handle.accepted) # Verify execution - result = self.get_result( - "/scaled_joint_trajectory_controller/follow_joint_trajectory", - goal_response, - TIMEOUT_EXECUTE_TRAJECTORY, + result = self._scaled_follow_joint_trajectory.get_result( + goal_handle, TIMEOUT_EXECUTE_TRAJECTORY ) self.assertEqual(result.error_code, FollowJointTrajectory.Result.SUCCESSFUL) - self.node.get_logger().info("Received result SUCCESSFUL") def test_illegal_trajectory(self, tf_prefix): """ @@ -337,15 +213,13 @@ def test_illegal_trajectory(self, tf_prefix): ) # Send illegal goal - self.node.get_logger().info("Sending illegal goal") - goal_response = self.call_action( - "/scaled_joint_trajectory_controller/follow_joint_trajectory", - FollowJointTrajectory.Goal(trajectory=trajectory), + logging.info("Sending illegal goal") + goal_handle = self._scaled_follow_joint_trajectory.send_goal( + trajectory=trajectory, ) # Verify the failure is correctly detected - self.assertEqual(goal_response.accepted, False) - self.node.get_logger().info("Goal response REJECTED") + self.assertFalse(goal_handle.accepted) def test_trajectory_scaled(self, tf_prefix): """Test robot movement.""" @@ -363,30 +237,17 @@ def test_trajectory_scaled(self, tf_prefix): ], ) - goal = FollowJointTrajectory.Goal(trajectory=trajectory) + # Execute trajectory + logging.info("Sending goal for robot to follow") + goal_handle = self._scaled_follow_joint_trajectory.send_goal(trajectory=trajectory) + self.assertTrue(goal_handle.accepted) - # TODO: uncomment when JTC starts taking into account goal_time_tolerance from goal message - # see https://github.com/ros-controls/ros2_controllers/issues/249 - # self.node.get_logger().info("Sending scaled goal without time restrictions") - self.node.get_logger().info("Sending goal for robot to follow") - goal_response = self.call_action( - "/scaled_joint_trajectory_controller/follow_joint_trajectory", goal + # Verify execution + result = self._scaled_follow_joint_trajectory.get_result( + goal_handle, + TIMEOUT_EXECUTE_TRAJECTORY, ) - - self.assertEqual(goal_response.accepted, True) - - if goal_response.accepted: - result = self.get_result( - "/scaled_joint_trajectory_controller/follow_joint_trajectory", - goal_response, - TIMEOUT_EXECUTE_TRAJECTORY, - ) - self.assertIn( - result.error_code, - (FollowJointTrajectory.Result.SUCCESSFUL,), - ) - - self.node.get_logger().info("Received result") + self.assertEqual(result.error_code, FollowJointTrajectory.Result.SUCCESSFUL) def test_trajectory_scaled_aborts_on_violation(self, tf_prefix): """Test that the robot correctly aborts the trajectory when the constraints are violated.""" @@ -417,46 +278,38 @@ def js_cb(msg): joint_state_sub = self.node.create_subscription(JointState, "/joint_states", js_cb, 1) joint_state_sub # prevent warning about unused variable - goal = FollowJointTrajectory.Goal(trajectory=trajectory) + # Send goal + logging.info("Sending goal for robot to follow") + goal_handle = self._scaled_follow_joint_trajectory.send_goal(trajectory=trajectory) + self.assertTrue(goal_handle.accepted) - self.node.get_logger().info("Sending goal for robot to follow") - goal_response = self.call_action( - "/scaled_joint_trajectory_controller/follow_joint_trajectory", goal + # Get result + result = self._scaled_follow_joint_trajectory.get_result( + goal_handle, + TIMEOUT_EXECUTE_TRAJECTORY, ) + self.assertEqual(result.error_code, FollowJointTrajectory.Result.PATH_TOLERANCE_VIOLATED) - self.assertEqual(goal_response.accepted, True) + state_when_aborted = last_joint_state - if goal_response.accepted: - result = self.get_result( - "/scaled_joint_trajectory_controller/follow_joint_trajectory", - goal_response, - TIMEOUT_EXECUTE_TRAJECTORY, - ) - self.assertIn( - result.error_code, - (FollowJointTrajectory.Result.PATH_TOLERANCE_VIOLATED,), - ) - self.node.get_logger().info("Received result") - - # self.node.get_logger().info(f"Joint state before sleep {last_joint_state.position}") - state_when_aborted = last_joint_state - - # This section is to make sure the robot stopped moving once the trajectory was aborted - time.sleep(2.0) - # Ugly workaround since we want to wait for a joint state in the same thread - while last_joint_state == state_when_aborted: - rclpy.spin_once(self.node) - state_after_sleep = last_joint_state - self.node.get_logger().info(f"before: {state_when_aborted.position.tolist()}") - self.node.get_logger().info(f"after: {state_after_sleep.position.tolist()}") - self.assertTrue( - all( - [ - abs(a - b) < 0.2 - for a, b in zip(state_after_sleep.position, state_when_aborted.position) - ] - ) + # This section is to make sure the robot stopped moving once the trajectory was aborted + time.sleep(2.0) + # Ugly workaround since we want to wait for a joint state in the same thread + while last_joint_state == state_when_aborted: + rclpy.spin_once(self.node) + state_after_sleep = last_joint_state + + logging.info("Joint states before sleep:\t %s", state_when_aborted.position.tolist()) + logging.info("Joint states after sleep:\t %s", state_after_sleep.position.tolist()) + + self.assertTrue( + all( + [ + abs(a - b) < 0.01 + for a, b in zip(state_after_sleep.position, state_when_aborted.position) + ] ) + ) # TODO: uncomment when JTC starts taking into account goal_time_tolerance from goal message # see https://github.com/ros-controls/ros2_controllers/issues/249 @@ -472,60 +325,3 @@ def js_cb(msg): # result = self.get_result("/scaled_joint_trajectory_controller/follow_joint_trajectory", goal_response, TIMEOUT_EXECUTE_TRAJECTORY) # self.assertEqual(result.error_code, FollowJointTrajectory.Result.GOAL_TOLERANCE_VIOLATED) # self.node.get_logger().info("Received result GOAL_TOLERANCE_VIOLATED") - - # - # Utility functions - # - - def call_service(self, srv_name, request): - self.node.get_logger().info(f"Calling service '{srv_name}' with request {request}") - future = self.service_clients[srv_name].call_async(request) - rclpy.spin_until_future_complete(self.node, future) - if future.result() is not None: - self.node.get_logger().info(f"Received result {future.result()}") - return future.result() - else: - raise Exception(f"Exception while calling service: {future.exception()}") - - def call_action(self, action_name, goal): - self.node.get_logger().info(f"Sending goal to action server '{action_name}'") - future = self.action_clients[action_name].send_goal_async(goal) - rclpy.spin_until_future_complete(self.node, future) - - if future.result() is not None: - return future.result() - else: - raise Exception(f"Exception while calling action: {future.exception()}") - - def get_result(self, action_name, goal_response, timeout): - self.node.get_logger().info( - f"Waiting for result for action server '{action_name}' (timeout: {timeout} seconds)" - ) - future_res = self.action_clients[action_name]._get_result_async(goal_response) - rclpy.spin_until_future_complete(self.node, future_res, timeout_sec=timeout) - - if future_res.result() is not None: - self.node.get_logger().info(f"Received result {future_res.result().result}") - return future_res.result().result - else: - raise Exception(f"Exception while calling action: {future_res.exception()}") - - -def waitForService(node, srv_name, srv_type, timeout=TIMEOUT_WAIT_SERVICE): - client = node.create_client(srv_type, srv_name) - if client.wait_for_service(timeout) is False: - raise Exception(f"Could not reach service '{srv_name}' within timeout of {timeout}") - - node.get_logger().info(f"Successfully connected to service '{srv_name}'") - return client - - -def waitForAction(node, action_name, action_type, timeout=TIMEOUT_WAIT_ACTION): - client = ActionClient(node, action_type, action_name) - if client.wait_for_server(timeout) is False: - raise Exception( - f"Could not reach action server '{action_name}' within timeout of {timeout}" - ) - - node.get_logger().info(f"Successfully connected to action '{action_name}'") - return client diff --git a/ur_robot_driver/test/test_common.py b/ur_robot_driver/test/test_common.py new file mode 100644 index 000000000..5b6033cea --- /dev/null +++ b/ur_robot_driver/test/test_common.py @@ -0,0 +1,345 @@ +# 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 +from ur_dashboard_msgs.msg import RobotMode +from ur_dashboard_msgs.srv import ( + GetLoadedProgram, + GetProgramState, + GetRobotMode, + IsProgramRunning, + Load, +) +from ur_msgs.srv import SetIO + +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()}" + ) + + +class DashboardInterface( + _ServiceInterface, + namespace="/dashboard_client", + initial_services={ + "power_on": Trigger, + }, + services={ + "power_off": Trigger, + "brake_release": Trigger, + "unlock_protective_stop": Trigger, + "restart_safety": Trigger, + "get_robot_mode": GetRobotMode, + "load_installation": Load, + "load_program": Load, + "close_popup": Trigger, + "get_loaded_program": GetLoadedProgram, + "program_state": GetProgramState, + "program_running": IsProgramRunning, + "play": Trigger, + "stop": Trigger, + }, +): + def start_robot(self): + self._check_call(self.power_on()) + self._check_call(self.brake_release()) + + time.sleep(1) + + robot_mode = self.get_robot_mode() + self._check_call(robot_mode) + if robot_mode.robot_mode.mode != RobotMode.RUNNING: + raise Exception( + f"Incorrect robot mode: Expected {RobotMode.RUNNING}, got {robot_mode.robot_mode.mode}" + ) + + self._check_call(self.stop()) + + def _check_call(self, result): + if not result.success: + raise Exception("Service call not successful") + + +class ControllerManagerInterface( + _ServiceInterface, + namespace="/controller_manager", + initial_services={"switch_controller": SwitchController}, + services={"list_controllers": ListControllers}, +): + def wait_for_controller(self, controller_name, target_state="active"): + while True: + controllers = self.list_controllers().controller + for controller in controllers: + if (controller.name == controller_name) and (controller.state == target_state): + return + + time.sleep(1) + + +class IoStatusInterface( + _ServiceInterface, + namespace="/io_and_status_controller", + initial_services={"set_io": SetIO}, + services={"resend_robot_program": Trigger}, +): + pass + + +def _declare_launch_arguments(): + declared_arguments = [] + + declared_arguments.append( + DeclareLaunchArgument( + "ur_type", + default_value="ur5e", + description="Type/series of used UR robot.", + choices=["ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e", "ur20", "ur30"], + ) + ) + + return declared_arguments + + +def _ursim_action(): + ur_type = LaunchConfiguration("ur_type") + + return ExecuteProcess( + cmd=[ + PathJoinSubstitution( + [ + FindPackagePrefix("ur_client_library"), + "lib", + "ur_client_library", + "start_ursim.sh", + ] + ), + " ", + "-m ", + ur_type, + ], + name="start_ursim", + output="screen", + ) + + +def generate_dashboard_test_description(): + dashboard_client = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + FindPackageShare("ur_robot_driver"), + "launch", + "ur_dashboard_client.launch.py", + ] + ) + ), + launch_arguments={ + "robot_ip": "192.168.56.101", + }.items(), + ) + + return LaunchDescription( + _declare_launch_arguments() + [ReadyToTest(), dashboard_client, _ursim_action()] + ) + + +def generate_driver_test_description( + tf_prefix="", controller_spawner_timeout=TIMEOUT_WAIT_SERVICE_INITIAL +): + ur_type = LaunchConfiguration("ur_type") + + robot_driver = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [FindPackageShare("ur_robot_driver"), "launch", "ur_control.launch.py"] + ) + ), + launch_arguments={ + "robot_ip": "192.168.56.101", + "ur_type": ur_type, + "launch_rviz": "false", + "controller_spawner_timeout": str(controller_spawner_timeout), + "initial_joint_controller": "scaled_joint_trajectory_controller", + "headless_mode": "true", + "launch_dashboard_client": "false", + "start_joint_controller": "false", + "tf_prefix": tf_prefix, + }.items(), + ) + wait_dashboard_server = ExecuteProcess( + cmd=[ + PathJoinSubstitution( + [FindPackagePrefix("ur_robot_driver"), "bin", "wait_dashboard_server.sh"] + ) + ], + name="wait_dashboard_server", + output="screen", + ) + driver_starter = RegisterEventHandler( + OnProcessExit(target_action=wait_dashboard_server, on_exit=robot_driver) + ) + + return LaunchDescription( + _declare_launch_arguments() + + [ReadyToTest(), wait_dashboard_server, _ursim_action(), driver_starter] + ) diff --git a/ur_robot_driver/test/urscript_interface.py b/ur_robot_driver/test/urscript_interface.py index edc1aaf0d..724ad3abe 100755 --- a/ur_robot_driver/test/urscript_interface.py +++ b/ur_robot_driver/test/urscript_interface.py @@ -26,114 +26,31 @@ # 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 pytest +import os +import sys import time import unittest +import pytest import rclpy import rclpy.node -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 std_srvs.srv import Trigger from std_msgs.msg import String as StringMsg -from ur_dashboard_msgs.msg import RobotMode -from ur_dashboard_msgs.srv import ( - GetLoadedProgram, - GetProgramState, - GetRobotMode, - IsProgramRunning, - Load, -) from ur_msgs.msg import IOStates -from controller_manager_msgs.srv import ListControllers +sys.path.append(os.path.dirname(__file__)) +from test_common import ( # noqa: E402 + ControllerManagerInterface, + DashboardInterface, + IoStatusInterface, + generate_driver_test_description, +) ROBOT_IP = "192.168.56.101" -TIMEOUT_WAIT_SERVICE = 10 -# 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_SERVICE_INITIAL = 120 @pytest.mark.launch_test def generate_test_description(): - declared_arguments = [] - - declared_arguments.append( - DeclareLaunchArgument( - "ur_type", - default_value="ur5e", - description="Type/series of used UR robot.", - choices=["ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e", "ur20"], - ) - ) - - ur_type = LaunchConfiguration("ur_type") - - robot_driver = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [FindPackageShare("ur_robot_driver"), "launch", "ur_control.launch.py"] - ) - ), - launch_arguments={ - "robot_ip": "192.168.56.101", - "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(), - ) - - ursim = ExecuteProcess( - cmd=[ - PathJoinSubstitution( - [ - FindPackagePrefix("ur_client_library"), - "lib", - "ur_client_library", - "start_ursim.sh", - ] - ), - " ", - "-m ", - ur_type, - ], - name="start_ursim", - output="screen", - ) - - wait_dashboard_server = ExecuteProcess( - cmd=[ - PathJoinSubstitution( - [FindPackagePrefix("ur_robot_driver"), "bin", "wait_dashboard_server.sh"] - ) - ], - name="wait_dashboard_server", - output="screen", - ) - - driver_starter = RegisterEventHandler( - OnProcessExit(target_action=wait_dashboard_server, on_exit=robot_driver) - ) - - return LaunchDescription( - declared_arguments + [ReadyToTest(), wait_dashboard_server, driver_starter, ursim] - ) + return generate_driver_test_description() class URScriptInterfaceTest(unittest.TestCase): @@ -151,70 +68,20 @@ def tearDownClass(cls): rclpy.shutdown() def init_robot(self): - # We wait longer for the first client, as the robot is still starting up - power_on_client = waitForService( - self.node, "/dashboard_client/power_on", Trigger, timeout=TIMEOUT_WAIT_SERVICE_INITIAL - ) - - # Connect to all other expected services - dashboard_interfaces = { - "/dashboard_client/power_off": Trigger, - "/dashboard_client/brake_release": Trigger, - "/dashboard_client/unlock_protective_stop": Trigger, - "/dashboard_client/restart_safety": Trigger, - "/dashboard_client/get_robot_mode": GetRobotMode, - "/dashboard_client/load_installation": Load, - "/dashboard_client/load_program": Load, - "/dashboard_client/close_popup": Trigger, - "/dashboard_client/get_loaded_program": GetLoadedProgram, - "/dashboard_client/program_state": GetProgramState, - "/dashboard_client/program_running": IsProgramRunning, - "/dashboard_client/play": Trigger, - "/dashboard_client/stop": Trigger, - } - self.service_clients = { - srv_name: waitForService(self.node, f"{srv_name}", srv_type) - for (srv_name, srv_type) in dashboard_interfaces.items() - } - - self.service_clients["/controller_manager/list_controllers"] = waitForService( - self.node, - "/controller_manager/list_controllers", - ListControllers, - timeout=TIMEOUT_WAIT_SERVICE_INITIAL, - ) - - # Add first client to dict - self.service_clients["/dashboard_client/power_on"] = power_on_client + self._dashboard_interface = DashboardInterface(self.node) + self._controller_manager_interface = ControllerManagerInterface(self.node) + self._io_status_controller_interface = IoStatusInterface(self.node) self.urscript_pub = self.node.create_publisher( StringMsg, "/urscript_interface/script_command", 1 ) def setUp(self): - # Start robot - empty_req = Trigger.Request() - self.call_service("/dashboard_client/power_on", empty_req) - self.call_service("/dashboard_client/brake_release", empty_req) - + self._dashboard_interface.start_robot() time.sleep(1) - robot_mode_resp = self.call_service( - "/dashboard_client/get_robot_mode", GetRobotMode.Request() - ) - self.assertEqual(robot_mode_resp.robot_mode.mode, RobotMode.RUNNING) - self.call_service("/dashboard_client/stop", empty_req) - time.sleep(1) - - io_controller_running = False + self.assertTrue(self._io_status_controller_interface.resend_robot_program().success) - while not io_controller_running: - time.sleep(1) - response = self.call_service( - "/controller_manager/list_controllers", ListControllers.Request() - ) - for controller in response.controller: - if controller.name == "io_and_status_controller": - io_controller_running = controller.state == "active" + self._controller_manager_interface.wait_for_controller("io_and_status_controller") def test_set_io(self): """Test setting an IO using a direct program call.""" @@ -272,22 +139,3 @@ def check_pin_states(self, pins, states): pin_states[i] = self.io_msg.digital_out_states[pin_id].state self.assertIsNotNone(self.io_msg, "Did not receive an IO state in requested time.") self.assertEqual(pin_states, states) - - def call_service(self, srv_name, request): - self.node.get_logger().info(f"Calling service '{srv_name}' with request {request}") - future = self.service_clients[srv_name].call_async(request) - rclpy.spin_until_future_complete(self.node, future) - if future.result() is not None: - self.node.get_logger().info(f"Received result {future.result()}") - return future.result() - else: - raise Exception(f"Exception while calling service: {future.exception()}") - - -def waitForService(node, srv_name, srv_type, timeout=TIMEOUT_WAIT_SERVICE): - client = node.create_client(srv_type, srv_name) - if client.wait_for_service(timeout) is False: - raise Exception(f"Could not reach service '{srv_name}' within timeout of {timeout}") - - node.get_logger().info(f"Successfully connected to service '{srv_name}'") - return client