From c39f4979ed60b97419d6668ea6e003f5e4bb9652 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Fri, 21 Jul 2023 14:19:01 +0200 Subject: [PATCH] Run robot driver test also with tf_prefix (#729) (#752) * Run robot driver test also with tf_prefix * Use tf_prefix substitution in controllers config file * Set default value of tf_prefix in launchfile to empty instead of '""' --------- Co-authored-by: Robert Wilbrandt (cherry picked from commit 79bfddc7ac4cd3a69594da26ce6ae8b8024eae73) Co-authored-by: Felix Exner (fexner) --- ur_robot_driver/CMakeLists.txt | 2 +- ur_robot_driver/config/ur_controllers.yaml | 82 ++++++++++----------- ur_robot_driver/launch/ur_control.launch.py | 15 +++- ur_robot_driver/test/robot_driver.py | 24 +++--- 4 files changed, 69 insertions(+), 54 deletions(-) diff --git a/ur_robot_driver/CMakeLists.txt b/ur_robot_driver/CMakeLists.txt index 6069c5ac..1b05dc34 100644 --- a/ur_robot_driver/CMakeLists.txt +++ b/ur_robot_driver/CMakeLists.txt @@ -185,7 +185,7 @@ if(BUILD_TESTING) ) add_launch_test(test/robot_driver.py TIMEOUT - 500 + 800 ) add_launch_test(test/urscript_interface.py TIMEOUT diff --git a/ur_robot_driver/config/ur_controllers.yaml b/ur_robot_driver/config/ur_controllers.yaml index 01eb555d..a512dc1c 100644 --- a/ur_robot_driver/config/ur_controllers.yaml +++ b/ur_robot_driver/config/ur_controllers.yaml @@ -28,15 +28,15 @@ controller_manager: speed_scaling_state_broadcaster: ros__parameters: state_publish_rate: 100.0 - tf_prefix: "" + tf_prefix: "$(var tf_prefix)" io_and_status_controller: ros__parameters: - tf_prefix: "" + tf_prefix: "$(var tf_prefix)" force_torque_sensor_broadcaster: ros__parameters: - sensor_name: tcp_fts_sensor + sensor_name: $(var tf_prefix)tcp_fts_sensor state_interface_names: - force.x - force.y @@ -44,19 +44,19 @@ force_torque_sensor_broadcaster: - torque.x - torque.y - torque.z - frame_id: tool0 + frame_id: $(var tf_prefix)tool0 topic_name: ft_data joint_trajectory_controller: ros__parameters: joints: - - shoulder_pan_joint - - shoulder_lift_joint - - elbow_joint - - wrist_1_joint - - wrist_2_joint - - wrist_3_joint + - $(var tf_prefix)shoulder_pan_joint + - $(var tf_prefix)shoulder_lift_joint + - $(var tf_prefix)elbow_joint + - $(var tf_prefix)wrist_1_joint + - $(var tf_prefix)wrist_2_joint + - $(var tf_prefix)wrist_3_joint command_interfaces: - position state_interfaces: @@ -68,23 +68,23 @@ joint_trajectory_controller: constraints: stopped_velocity_tolerance: 0.2 goal_time: 0.0 - shoulder_pan_joint: { trajectory: 0.2, goal: 0.1 } - shoulder_lift_joint: { trajectory: 0.2, goal: 0.1 } - elbow_joint: { trajectory: 0.2, goal: 0.1 } - wrist_1_joint: { trajectory: 0.2, goal: 0.1 } - wrist_2_joint: { trajectory: 0.2, goal: 0.1 } - wrist_3_joint: { trajectory: 0.2, goal: 0.1 } + $(var tf_prefix)shoulder_pan_joint: { trajectory: 0.2, goal: 0.1 } + $(var tf_prefix)shoulder_lift_joint: { trajectory: 0.2, goal: 0.1 } + $(var tf_prefix)elbow_joint: { trajectory: 0.2, goal: 0.1 } + $(var tf_prefix)wrist_1_joint: { trajectory: 0.2, goal: 0.1 } + $(var tf_prefix)wrist_2_joint: { trajectory: 0.2, goal: 0.1 } + $(var tf_prefix)wrist_3_joint: { trajectory: 0.2, goal: 0.1 } scaled_joint_trajectory_controller: ros__parameters: joints: - - shoulder_pan_joint - - shoulder_lift_joint - - elbow_joint - - wrist_1_joint - - wrist_2_joint - - wrist_3_joint + - $(var tf_prefix)shoulder_pan_joint + - $(var tf_prefix)shoulder_lift_joint + - $(var tf_prefix)elbow_joint + - $(var tf_prefix)wrist_1_joint + - $(var tf_prefix)wrist_2_joint + - $(var tf_prefix)wrist_3_joint command_interfaces: - position state_interfaces: @@ -96,31 +96,31 @@ scaled_joint_trajectory_controller: constraints: stopped_velocity_tolerance: 0.2 goal_time: 0.0 - shoulder_pan_joint: { trajectory: 0.2, goal: 0.1 } - shoulder_lift_joint: { trajectory: 0.2, goal: 0.1 } - elbow_joint: { trajectory: 0.2, goal: 0.1 } - wrist_1_joint: { trajectory: 0.2, goal: 0.1 } - wrist_2_joint: { trajectory: 0.2, goal: 0.1 } - wrist_3_joint: { trajectory: 0.2, goal: 0.1 } - speed_scaling_interface_name: speed_scaling/speed_scaling_factor + $(var tf_prefix)shoulder_pan_joint: { trajectory: 0.2, goal: 0.1 } + $(var tf_prefix)shoulder_lift_joint: { trajectory: 0.2, goal: 0.1 } + $(var tf_prefix)elbow_joint: { trajectory: 0.2, goal: 0.1 } + $(var tf_prefix)wrist_1_joint: { trajectory: 0.2, goal: 0.1 } + $(var tf_prefix)wrist_2_joint: { trajectory: 0.2, goal: 0.1 } + $(var tf_prefix)wrist_3_joint: { trajectory: 0.2, goal: 0.1 } + speed_scaling_interface_name: $(var tf_prefix)speed_scaling/speed_scaling_factor forward_velocity_controller: ros__parameters: joints: - - shoulder_pan_joint - - shoulder_lift_joint - - elbow_joint - - wrist_1_joint - - wrist_2_joint - - wrist_3_joint + - $(var tf_prefix)shoulder_pan_joint + - $(var tf_prefix)shoulder_lift_joint + - $(var tf_prefix)elbow_joint + - $(var tf_prefix)wrist_1_joint + - $(var tf_prefix)wrist_2_joint + - $(var tf_prefix)wrist_3_joint interface_name: velocity forward_position_controller: ros__parameters: joints: - - shoulder_pan_joint - - shoulder_lift_joint - - elbow_joint - - wrist_1_joint - - wrist_2_joint - - wrist_3_joint + - $(var tf_prefix)shoulder_pan_joint + - $(var tf_prefix)shoulder_lift_joint + - $(var tf_prefix)elbow_joint + - $(var tf_prefix)wrist_1_joint + - $(var tf_prefix)wrist_2_joint + - $(var tf_prefix)wrist_3_joint diff --git a/ur_robot_driver/launch/ur_control.launch.py b/ur_robot_driver/launch/ur_control.launch.py index c7bbaa8f..98bb7dac 100644 --- a/ur_robot_driver/launch/ur_control.launch.py +++ b/ur_robot_driver/launch/ur_control.launch.py @@ -30,6 +30,7 @@ # Author: Denis Stogl from launch_ros.actions import Node +from launch_ros.parameter_descriptions import ParameterFile from launch_ros.substitutions import FindPackageShare from launch import LaunchDescription @@ -204,7 +205,11 @@ def launch_setup(context, *args, **kwargs): control_node = Node( package="controller_manager", executable="ros2_control_node", - parameters=[robot_description, update_rate_config_file, initial_joint_controllers], + parameters=[ + robot_description, + update_rate_config_file, + ParameterFile(initial_joint_controllers, allow_substs=True), + ], output="screen", condition=IfCondition(use_fake_hardware), ) @@ -212,7 +217,11 @@ def launch_setup(context, *args, **kwargs): ur_control_node = Node( package="ur_robot_driver", executable="ur_ros2_control_node", - parameters=[robot_description, update_rate_config_file, initial_joint_controllers], + parameters=[ + robot_description, + update_rate_config_file, + ParameterFile(initial_joint_controllers, allow_substs=True), + ], output="screen", condition=UnlessCondition(use_fake_hardware), ) @@ -427,7 +436,7 @@ def generate_launch_description(): declared_arguments.append( DeclareLaunchArgument( "tf_prefix", - default_value='""', + default_value="", description="tf_prefix of the joint names, useful for \ multi-robot setup. If changed, also joint names in the controllers' configuration \ have to be updated.", diff --git a/ur_robot_driver/test/robot_driver.py b/ur_robot_driver/test/robot_driver.py index 4dfc9140..8e81a42c 100755 --- a/ur_robot_driver/test/robot_driver.py +++ b/ur_robot_driver/test/robot_driver.py @@ -31,6 +31,7 @@ import time import unittest +import launch_testing import pytest import rclpy from builtin_interfaces.msg import Duration @@ -52,10 +53,10 @@ from rclpy.node import Node from std_srvs.srv import Trigger from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint -from ur_msgs.msg import IOStates -from ur_msgs.srv import SetIO 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 @@ -73,7 +74,11 @@ @pytest.mark.launch_test -def generate_test_description(): +@launch_testing.parametrize( + "tf_prefix", + [(""), ("my_ur_")], +) +def generate_test_description(tf_prefix): declared_arguments = [] declared_arguments.append( @@ -102,6 +107,7 @@ def generate_test_description(): "headless_mode": "true", "launch_dashboard_client": "false", "start_joint_controller": "false", + "tf_prefix": tf_prefix, }.items(), ) ursim = ExecuteProcess( @@ -269,7 +275,7 @@ def io_msg_cb(msg): # Clean up io subscription self.node.destroy_subscription(io_states_sub) - def test_trajectory(self): + def test_trajectory(self, tf_prefix): """Test robot movement.""" # Construct test trajectory test_trajectory = [ @@ -279,7 +285,7 @@ def test_trajectory(self): ] trajectory = JointTrajectory( - joint_names=ROBOT_JOINTS, + joint_names=[tf_prefix + joint for joint in ROBOT_JOINTS], points=[ JointTrajectoryPoint(positions=test_pos, time_from_start=test_time) for (test_time, test_pos) in test_trajectory @@ -303,7 +309,7 @@ def test_trajectory(self): self.assertEqual(result.error_code, FollowJointTrajectory.Result.SUCCESSFUL) self.node.get_logger().info("Received result SUCCESSFUL") - def test_illegal_trajectory(self): + def test_illegal_trajectory(self, tf_prefix): """ Test trajectory server. @@ -316,7 +322,7 @@ def test_illegal_trajectory(self): ] trajectory = JointTrajectory( - joint_names=ROBOT_JOINTS, + joint_names=[tf_prefix + joint for joint in ROBOT_JOINTS], points=[ JointTrajectoryPoint(positions=test_pos, time_from_start=test_time) for (test_time, test_pos) in test_trajectory @@ -334,7 +340,7 @@ def test_illegal_trajectory(self): self.assertEqual(goal_response.accepted, False) self.node.get_logger().info("Goal response REJECTED") - def test_trajectory_scaled(self): + def test_trajectory_scaled(self, tf_prefix): """Test robot movement.""" # Construct test trajectory test_trajectory = [ @@ -343,7 +349,7 @@ def test_trajectory_scaled(self): ] trajectory = JointTrajectory( - joint_names=ROBOT_JOINTS, + joint_names=[tf_prefix + joint for joint in ROBOT_JOINTS], points=[ JointTrajectoryPoint(positions=test_pos, time_from_start=test_time) for (test_time, test_pos) in test_trajectory