Run robot driver test also with tf_prefix (#729)

* 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 <[email protected]> (cherry picked from commit 79bfddc)
UniversalRobots · Aug 9, 2023 · 1a0f34a · 1a0f34a
1 parent 698914b
commit 1a0f34a
Show file tree

Hide file tree

Showing 4 changed files with 69 additions and 54 deletions.
diff --git 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
@@ -28,35 +28,35 @@ 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
       - force.z
       - 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
@@ -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
@@ -205,15 +206,23 @@ 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),
     )
 
     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),
     )
@@ -428,7 +437,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
@@ -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(
@@ -270,7 +276,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 = [
@@ -280,7 +286,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
@@ -304,7 +310,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.
 
@@ -317,7 +323,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
@@ -335,7 +341,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 = [
@@ -344,7 +350,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