From fba56d2427af08c668fa2d03ec56db48cd630687 Mon Sep 17 00:00:00 2001 From: Robert Wilbrandt Date: Tue, 17 Oct 2023 14:44:55 +0200 Subject: [PATCH] Move launch and interfacing boilerplate to common file --- ur_robot_driver/test/dashboard_client.py | 85 +----- ur_robot_driver/test/robot_driver.py | 285 ++++++--------------- ur_robot_driver/test/test_common.py | 184 ++++++++++++- ur_robot_driver/test/urscript_interface.py | 99 ++----- 4 files changed, 293 insertions(+), 360 deletions(-) diff --git a/ur_robot_driver/test/dashboard_client.py b/ur_robot_driver/test/dashboard_client.py index ff16a417f..d55d27828 100755 --- a/ur_robot_driver/test/dashboard_client.py +++ b/ur_robot_driver/test/dashboard_client.py @@ -26,8 +26,6 @@ # 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 @@ -36,23 +34,10 @@ import pytest import rclpy 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, -) sys.path.append(os.path.dirname(__file__)) -from test_common import generate_dashboard_test_description # noqa: E402 - -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 +from test_common import DashboardInterface, generate_dashboard_test_description # noqa: E402 @pytest.mark.launch_test @@ -75,38 +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.""" @@ -115,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 50b2ab6cf..f3c026b45 100755 --- a/ur_robot_driver/test/robot_driver.py +++ b/ur_robot_driver/test/robot_driver.py @@ -26,8 +26,6 @@ # 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 @@ -39,22 +37,21 @@ from builtin_interfaces.msg import Duration from control_msgs.action import FollowJointTrajectory from controller_manager_msgs.srv import SwitchController -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 sys.path.append(os.path.dirname(__file__)) -from test_common import generate_driver_test_description # noqa: 402 +from test_common import ( # noqa: 402 + ControllerManagerInterface, + ActionInterface, + DashboardInterface, + IoStatusInterface, + generate_driver_test_description, +) -TIMEOUT_WAIT_SERVICE = 10 -TIMEOUT_WAIT_SERVICE_INITIAL = 60 -TIMEOUT_WAIT_ACTION = 10 TIMEOUT_EXECUTE_TRAJECTORY = 30 ROBOT_JOINTS = [ @@ -73,9 +70,7 @@ [(""), ("my_ur_")], ) def generate_test_description(tf_prefix): - return generate_driver_test_description( - tf_prefix=tf_prefix, controller_spawner_timeout=TIMEOUT_WAIT_SERVICE_INITIAL - ) + return generate_driver_test_description(tf_prefix=tf_prefix) class RobotDriverTest(unittest.TestCase): @@ -94,58 +89,30 @@ def tearDownClass(cls): rclpy.shutdown() def init_robot(self): - # 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._dashboard_interface = DashboardInterface(self.node) + self._controller_manager_interface = ControllerManagerInterface(self.node) + self._io_status_controller_interface = IoStatusInterface(self.node) - 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() - } + self._scaled_follow_joint_trajectory = ActionInterface( + self.node, + "/scaled_joint_trajectory_controller/follow_joint_trajectory", + FollowJointTrajectory, + ) 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.assertTrue(self._dashboard_interface.power_on().success) + self.assertTrue(self._dashboard_interface.brake_release().success) + 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) + + robot_mode = self._dashboard_interface.get_robot_mode() + self.assertTrue(robot_mode.success) + self.assertEqual(robot_mode.robot_mode.mode, RobotMode.RUNNING) + + self.assertTrue(self._dashboard_interface.stop().success) 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 @@ -155,9 +122,13 @@ def test_start_scaled_jtc_controller(self): req = SwitchController.Request() req.strictness = SwitchController.Request.BEST_EFFORT req.activate_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.""" @@ -178,13 +149,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) + self.node.get_logger().info(f"Setting pin {test_pin} to 1.0") + 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 @@ -194,12 +160,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) + self.node.get_logger().info(f"Setting pin {test_pin} to 0.0") + 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 @@ -208,7 +173,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) @@ -232,17 +197,13 @@ 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) + + 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") @@ -269,13 +230,12 @@ 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), + goal_handle = self._scaled_follow_joint_trajectory.send_goal( + trajectory=trajectory, ) # Verify the failure is correctly detected - self.assertEqual(goal_response.accepted, False) + self.assertFalse(goal_handle.accepted) self.node.get_logger().info("Goal response REJECTED") def test_trajectory_scaled(self, tf_prefix): @@ -294,30 +254,19 @@ def test_trajectory_scaled(self, tf_prefix): ], ) - goal = FollowJointTrajectory.Goal(trajectory=trajectory) - - # 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") + # Execute trajectory self.node.get_logger().info("Sending goal for robot to follow") - goal_response = self.call_action( - "/scaled_joint_trajectory_controller/follow_joint_trajectory", goal - ) + goal_handle = self._scaled_follow_joint_trajectory.send_goal(trajectory=trajectory) + self.assertTrue(goal_handle.accepted) - 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,), - ) + # Verify execution + 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") + self.node.get_logger().info("Received result") def test_trajectory_scaled_aborts_on_violation(self, tf_prefix): """Test that the robot correctly aborts the trajectory when the constraints are violated.""" @@ -348,46 +297,37 @@ 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 self.node.get_logger().info("Sending goal for robot to follow") - goal_response = self.call_action( - "/scaled_joint_trajectory_controller/follow_joint_trajectory", goal - ) - - self.assertEqual(goal_response.accepted, True) + goal_handle = self._scaled_follow_joint_trajectory.send_goal(trajectory=trajectory) + self.assertTrue(goal_handle.accepted) - 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.01 - for a, b in zip(state_after_sleep.position, state_when_aborted.position) - ] - ) + # 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.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.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 @@ -403,60 +343,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 index be16a3261..4c89e4386 100644 --- a/ur_robot_driver/test/test_common.py +++ b/ur_robot_driver/test/test_common.py @@ -25,6 +25,10 @@ # 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 rclpy +from controller_manager_msgs.srv import ListControllers, SwitchController from launch import LaunchDescription from launch.actions import ( DeclareLaunchArgument, @@ -37,6 +41,182 @@ 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.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, + }, +): + pass + + +class ControllerManagerInterface( + _ServiceInterface, + namespace="/controller_manager", + initial_services={"switch_controller": SwitchController}, + services={"list_controllers": ListControllers}, +): + pass + + +class IoStatusInterface( + _ServiceInterface, + namespace="/io_and_status_controller", + initial_services={"set_io": SetIO}, + services={"resend_robot_program": Trigger}, +): + pass def _declare_launch_arguments(): @@ -97,7 +277,9 @@ def generate_dashboard_test_description(): ) -def generate_driver_test_description(tf_prefix="", controller_spawner_timeout=60): +def generate_driver_test_description( + tf_prefix="", controller_spawner_timeout=TIMEOUT_WAIT_SERVICE_INITIAL +): ur_type = LaunchConfiguration("ur_type") robot_driver = IncludeLaunchDescription( diff --git a/ur_robot_driver/test/urscript_interface.py b/ur_robot_driver/test/urscript_interface.py index 66eb4b397..911b6f851 100755 --- a/ur_robot_driver/test/urscript_interface.py +++ b/ur_robot_driver/test/urscript_interface.py @@ -26,7 +26,6 @@ # 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 @@ -35,27 +34,19 @@ import pytest import rclpy import rclpy.node -from controller_manager_msgs.srv import ListControllers from std_msgs.msg import String as StringMsg -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.msg import IOStates sys.path.append(os.path.dirname(__file__)) -from test_common import generate_driver_test_description # noqa: E402 +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 @@ -78,41 +69,9 @@ 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 @@ -120,25 +79,24 @@ def init_robot(self): 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.assertTrue(self._dashboard_interface.power_on().success) + self.assertTrue(self._dashboard_interface.brake_release().success) 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) + + robot_mode = self._dashboard_interface.get_robot_mode() + self.assertTrue(robot_mode.success) + self.assertEqual(robot_mode.robot_mode.mode, RobotMode.RUNNING) + + self.assertTrue(self._dashboard_interface.stop().success) time.sleep(1) + self.assertTrue(self._io_status_controller_interface.resend_robot_program().success) io_controller_running = False while not io_controller_running: time.sleep(1) - response = self.call_service( - "/controller_manager/list_controllers", ListControllers.Request() - ) + response = self._controller_manager_interface.list_controllers() for controller in response.controller: if controller.name == "io_and_status_controller": io_controller_running = controller.state == "active" @@ -199,22 +157,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