From 40a9bfa6fe406c1f9420bcbf263f0887bb89a6c3 Mon Sep 17 00:00:00 2001 From: Robert Wilbrandt Date: Wed, 11 Oct 2023 17:09:58 +0200 Subject: [PATCH 1/6] Remove duplication of launch description in tests --- ur_robot_driver/test/dashboard_client.py | 58 +-------- ur_robot_driver/test/robot_driver.py | 82 ++---------- ur_robot_driver/test/test_common.py | 137 +++++++++++++++++++++ ur_robot_driver/test/urscript_interface.py | 89 ++----------- 4 files changed, 158 insertions(+), 208 deletions(-) create mode 100644 ur_robot_driver/test/test_common.py diff --git a/ur_robot_driver/test/dashboard_client.py b/ur_robot_driver/test/dashboard_client.py index cea1b5881..ff16a417f 100755 --- a/ur_robot_driver/test/dashboard_client.py +++ b/ur_robot_driver/test/dashboard_client.py @@ -28,17 +28,13 @@ # 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 @@ -50,6 +46,9 @@ 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. @@ -58,52 +57,7 @@ @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): diff --git a/ur_robot_driver/test/robot_driver.py b/ur_robot_driver/test/robot_driver.py index effc93e32..50b2ab6cf 100755 --- a/ur_robot_driver/test/robot_driver.py +++ b/ur_robot_driver/test/robot_driver.py @@ -28,6 +28,8 @@ # POSSIBILITY OF SUCH DAMAGE. +import os +import sys import time import unittest @@ -37,18 +39,6 @@ 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 @@ -59,6 +49,9 @@ 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 + TIMEOUT_WAIT_SERVICE = 10 TIMEOUT_WAIT_SERVICE_INITIAL = 60 TIMEOUT_WAIT_ACTION = 10 @@ -80,69 +73,8 @@ [(""), ("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, controller_spawner_timeout=TIMEOUT_WAIT_SERVICE_INITIAL ) diff --git a/ur_robot_driver/test/test_common.py b/ur_robot_driver/test/test_common.py new file mode 100644 index 000000000..be16a3261 --- /dev/null +++ b/ur_robot_driver/test/test_common.py @@ -0,0 +1,137 @@ +# 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. +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 + + +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"], + ) + ) + + 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=60): + 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..66eb4b397 100755 --- a/ur_robot_driver/test/urscript_interface.py +++ b/ur_robot_driver/test/urscript_interface.py @@ -27,26 +27,17 @@ # 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 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, @@ -56,8 +47,9 @@ 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 generate_driver_test_description # noqa: E402 ROBOT_IP = "192.168.56.101" TIMEOUT_WAIT_SERVICE = 10 @@ -68,72 +60,7 @@ @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): From fba56d2427af08c668fa2d03ec56db48cd630687 Mon Sep 17 00:00:00 2001 From: Robert Wilbrandt Date: Tue, 17 Oct 2023 14:44:55 +0200 Subject: [PATCH 2/6] 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 From cafb07bd7209c2aa1e078e3b57850caf16fab634 Mon Sep 17 00:00:00 2001 From: Robert Wilbrandt Date: Tue, 17 Oct 2023 15:00:39 +0200 Subject: [PATCH 3/6] Move all test logs to python logging This makes it easier to differentiate between ROS and test framework messages --- ur_robot_driver/test/robot_driver.py | 27 ++++++++++++--------------- 1 file changed, 12 insertions(+), 15 deletions(-) diff --git a/ur_robot_driver/test/robot_driver.py b/ur_robot_driver/test/robot_driver.py index f3c026b45..758c0d096 100755 --- a/ur_robot_driver/test/robot_driver.py +++ b/ur_robot_driver/test/robot_driver.py @@ -26,6 +26,7 @@ # 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 @@ -44,7 +45,7 @@ from ur_msgs.msg import IOStates sys.path.append(os.path.dirname(__file__)) -from test_common import ( # noqa: 402 +from test_common import ( # noqa: E402 ControllerManagerInterface, ActionInterface, DashboardInterface, @@ -149,7 +150,7 @@ def io_msg_cb(msg): # Set pin 0 to 1.0 test_pin = 0 - self.node.get_logger().info(f"Setting pin {test_pin} to 1.0") + 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 @@ -163,7 +164,7 @@ def io_msg_cb(msg): self.assertEqual(pin_state, 1.0) # Set pin 0 to 0.0 - self.node.get_logger().info(f"Setting pin {test_pin} to 0.0") + 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 @@ -196,8 +197,7 @@ def test_trajectory(self, tf_prefix): ) # Sending trajectory goal - self.node.get_logger().info("Sending simple goal") - + logging.info("Sending simple goal") goal_handle = self._scaled_follow_joint_trajectory.send_goal(trajectory=trajectory) self.assertTrue(goal_handle.accepted) @@ -206,7 +206,6 @@ def test_trajectory(self, tf_prefix): 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): """ @@ -229,14 +228,13 @@ def test_illegal_trajectory(self, tf_prefix): ) # Send illegal goal - self.node.get_logger().info("Sending illegal goal") + logging.info("Sending illegal goal") goal_handle = self._scaled_follow_joint_trajectory.send_goal( trajectory=trajectory, ) # Verify the failure is correctly detected self.assertFalse(goal_handle.accepted) - self.node.get_logger().info("Goal response REJECTED") def test_trajectory_scaled(self, tf_prefix): """Test robot movement.""" @@ -255,7 +253,7 @@ def test_trajectory_scaled(self, tf_prefix): ) # Execute trajectory - self.node.get_logger().info("Sending goal for robot to follow") + logging.info("Sending goal for robot to follow") goal_handle = self._scaled_follow_joint_trajectory.send_goal(trajectory=trajectory) self.assertTrue(goal_handle.accepted) @@ -266,8 +264,6 @@ def test_trajectory_scaled(self, tf_prefix): ) self.assertEqual(result.error_code, FollowJointTrajectory.Result.SUCCESSFUL) - 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.""" # Construct test trajectory @@ -298,7 +294,7 @@ def js_cb(msg): joint_state_sub # prevent warning about unused variable # Send goal - self.node.get_logger().info("Sending goal for robot to follow") + logging.info("Sending goal for robot to follow") goal_handle = self._scaled_follow_joint_trajectory.send_goal(trajectory=trajectory) self.assertTrue(goal_handle.accepted) @@ -309,7 +305,6 @@ def js_cb(msg): ) 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 @@ -318,8 +313,10 @@ def js_cb(msg): 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()}") + + 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( [ From 23d74e4e3fe6ce2a3ad716ec7863bee1f65e7b5b Mon Sep 17 00:00:00 2001 From: Robert Wilbrandt Date: Tue, 17 Oct 2023 15:16:14 +0200 Subject: [PATCH 4/6] Move waiting for a controller to test_common --- ur_robot_driver/test/test_common.py | 10 +++++++++- ur_robot_driver/test/urscript_interface.py | 11 ++--------- 2 files changed, 11 insertions(+), 10 deletions(-) diff --git a/ur_robot_driver/test/test_common.py b/ur_robot_driver/test/test_common.py index 4c89e4386..0f5eaf7ee 100644 --- a/ur_robot_driver/test/test_common.py +++ b/ur_robot_driver/test/test_common.py @@ -26,6 +26,7 @@ # 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 @@ -207,7 +208,14 @@ class ControllerManagerInterface( initial_services={"switch_controller": SwitchController}, services={"list_controllers": ListControllers}, ): - pass + 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( diff --git a/ur_robot_driver/test/urscript_interface.py b/ur_robot_driver/test/urscript_interface.py index 911b6f851..c1393b4f3 100755 --- a/ur_robot_driver/test/urscript_interface.py +++ b/ur_robot_driver/test/urscript_interface.py @@ -40,10 +40,10 @@ sys.path.append(os.path.dirname(__file__)) from test_common import ( # noqa: E402 - ControllerManagerInterface, DashboardInterface, IoStatusInterface, generate_driver_test_description, + ControllerManagerInterface, ) ROBOT_IP = "192.168.56.101" @@ -92,14 +92,7 @@ def setUp(self): 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._controller_manager_interface.list_controllers() - 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.""" From 4e723864c9194f3df404f1d011cf7efbe76e851b Mon Sep 17 00:00:00 2001 From: Robert Wilbrandt Date: Thu, 19 Oct 2023 10:53:16 +0200 Subject: [PATCH 5/6] Move robot starting to dashboard interface --- ur_robot_driver/test/robot_driver.py | 15 ++------------- ur_robot_driver/test/test_common.py | 20 +++++++++++++++++++- ur_robot_driver/test/urscript_interface.py | 15 ++------------- 3 files changed, 23 insertions(+), 27 deletions(-) diff --git a/ur_robot_driver/test/robot_driver.py b/ur_robot_driver/test/robot_driver.py index 758c0d096..b5e04b629 100755 --- a/ur_robot_driver/test/robot_driver.py +++ b/ur_robot_driver/test/robot_driver.py @@ -41,13 +41,12 @@ from rclpy.node import Node from sensor_msgs.msg import JointState from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint -from ur_dashboard_msgs.msg import RobotMode from ur_msgs.msg import IOStates sys.path.append(os.path.dirname(__file__)) from test_common import ( # noqa: E402 - ControllerManagerInterface, ActionInterface, + ControllerManagerInterface, DashboardInterface, IoStatusInterface, generate_driver_test_description, @@ -101,17 +100,7 @@ def init_robot(self): ) def setUp(self): - # Start robot - self.assertTrue(self._dashboard_interface.power_on().success) - self.assertTrue(self._dashboard_interface.brake_release().success) - - time.sleep(1) - - 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) + self._dashboard_interface.start_robot() time.sleep(1) self.assertTrue(self._io_status_controller_interface.resend_robot_program().success) diff --git a/ur_robot_driver/test/test_common.py b/ur_robot_driver/test/test_common.py index 0f5eaf7ee..b8d7c08b1 100644 --- a/ur_robot_driver/test/test_common.py +++ b/ur_robot_driver/test/test_common.py @@ -44,6 +44,7 @@ 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, @@ -199,7 +200,24 @@ class DashboardInterface( "stop": Trigger, }, ): - pass + 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( diff --git a/ur_robot_driver/test/urscript_interface.py b/ur_robot_driver/test/urscript_interface.py index c1393b4f3..724ad3abe 100755 --- a/ur_robot_driver/test/urscript_interface.py +++ b/ur_robot_driver/test/urscript_interface.py @@ -35,15 +35,14 @@ import rclpy import rclpy.node from std_msgs.msg import String as StringMsg -from ur_dashboard_msgs.msg import RobotMode from ur_msgs.msg import IOStates sys.path.append(os.path.dirname(__file__)) from test_common import ( # noqa: E402 + ControllerManagerInterface, DashboardInterface, IoStatusInterface, generate_driver_test_description, - ControllerManagerInterface, ) ROBOT_IP = "192.168.56.101" @@ -78,17 +77,7 @@ def init_robot(self): ) def setUp(self): - # Start robot - self.assertTrue(self._dashboard_interface.power_on().success) - self.assertTrue(self._dashboard_interface.brake_release().success) - - time.sleep(1) - - 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) + self._dashboard_interface.start_robot() time.sleep(1) self.assertTrue(self._io_status_controller_interface.resend_robot_program().success) From 7cb1b69307ca917ab4b7d4e1091c6dfb0bcc613e Mon Sep 17 00:00:00 2001 From: Robert Wilbrandt Date: Thu, 26 Oct 2023 10:18:35 +0200 Subject: [PATCH 6/6] Remove unused request definition --- ur_robot_driver/test/robot_driver.py | 4 ---- 1 file changed, 4 deletions(-) diff --git a/ur_robot_driver/test/robot_driver.py b/ur_robot_driver/test/robot_driver.py index b5e04b629..96c859947 100755 --- a/ur_robot_driver/test/robot_driver.py +++ b/ur_robot_driver/test/robot_driver.py @@ -109,10 +109,6 @@ def setUp(self): # def test_start_scaled_jtc_controller(self): - req = SwitchController.Request() - req.strictness = SwitchController.Request.BEST_EFFORT - req.activate_controllers = ["scaled_joint_trajectory_controller"] - self.assertTrue( self._controller_manager_interface.switch_controller( strictness=SwitchController.Request.BEST_EFFORT,