Move sjtc trajectory and illegal trajectory test to test_common, so it can be used by mock hardware test as well.

Author: URJala

ur_robot_driver/test/integration_test_scaled_joint_controller.py

@@ -51,6 +51,8 @@
     generate_driver_test_description,
     ROBOT_JOINTS,
     TIMEOUT_EXECUTE_TRAJECTORY,
+    sjtc_trajectory_test,
+    sjtc_illegal_trajectory_test,
 )
 
 
@@ -104,75 +106,10 @@ def test_start_scaled_jtc_controller(self):
         )
 
     def test_trajectory(self, tf_prefix):
-        """Test robot movement."""
-        self.assertTrue(
-            self._controller_manager_interface.switch_controller(
-                strictness=SwitchController.Request.BEST_EFFORT,
-                deactivate_controllers=["passthrough_trajectory_controller"],
-                activate_controllers=["scaled_joint_trajectory_controller"],
-            ).ok
-        )
-        # Construct test trajectory
-        test_trajectory = [
-            (Duration(sec=6, nanosec=0), [0.0 for j in ROBOT_JOINTS]),
-            (Duration(sec=9, nanosec=0), [-0.5 for j in ROBOT_JOINTS]),
-            (Duration(sec=12, nanosec=0), [-1.0 for j in ROBOT_JOINTS]),
-        ]
-
-        trajectory = JointTrajectory(
-            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
-            ],
-        )
-
-        # Sending trajectory goal
-        logging.info("Sending simple goal")
-        goal_handle = self._scaled_follow_joint_trajectory.send_goal(trajectory=trajectory)
-        self.assertTrue(goal_handle.accepted)
-
-        # Verify execution
-        result = self._scaled_follow_joint_trajectory.get_result(
-            goal_handle, TIMEOUT_EXECUTE_TRAJECTORY
-        )
-        self.assertEqual(result.error_code, FollowJointTrajectory.Result.SUCCESSFUL)
+        sjtc_trajectory_test(self, tf_prefix)
 
     def test_illegal_trajectory(self, tf_prefix):
-        """
-        Test trajectory server.
-
-        This is more of a validation test that the testing suite does the right thing
-        """
-        self.assertTrue(
-            self._controller_manager_interface.switch_controller(
-                strictness=SwitchController.Request.BEST_EFFORT,
-                deactivate_controllers=["passthrough_trajectory_controller"],
-                activate_controllers=["scaled_joint_trajectory_controller"],
-            ).ok
-        )
-        # Construct test trajectory, the second point wrongly starts before the first
-        test_trajectory = [
-            (Duration(sec=6, nanosec=0), [0.0 for j in ROBOT_JOINTS]),
-            (Duration(sec=3, nanosec=0), [-0.5 for j in ROBOT_JOINTS]),
-        ]
-
-        trajectory = JointTrajectory(
-            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
-            ],
-        )
-
-        # Send 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)
+        sjtc_illegal_trajectory_test(self, tf_prefix)
 
     def test_trajectory_scaled(self, tf_prefix):
         """Test robot movement."""

ur_robot_driver/test/test_common.py

@@ -58,6 +58,9 @@
     Load,
 )
 from ur_msgs.srv import SetIO, GetRobotSoftwareVersion, SetForceMode
+from builtin_interfaces.msg import Duration
+from control_msgs.action import FollowJointTrajectory
+from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
 
 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.
@@ -302,6 +305,79 @@ class ForceModeInterface(
     pass
 
 
+def sjtc_trajectory_test(tester, tf_prefix):
+    """Test robot movement."""
+    tester.assertTrue(
+        tester._controller_manager_interface.switch_controller(
+            strictness=SwitchController.Request.BEST_EFFORT,
+            deactivate_controllers=["passthrough_trajectory_controller"],
+            activate_controllers=["scaled_joint_trajectory_controller"],
+        ).ok
+    )
+    # Construct test trajectory
+    test_trajectory = [
+        (Duration(sec=6, nanosec=0), [0.0 for j in ROBOT_JOINTS]),
+        (Duration(sec=9, nanosec=0), [-0.5 for j in ROBOT_JOINTS]),
+        (Duration(sec=12, nanosec=0), [-1.0 for j in ROBOT_JOINTS]),
+    ]
+
+    trajectory = JointTrajectory(
+        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
+        ],
+    )
+
+    # Sending trajectory goal
+    logging.info("Sending simple goal")
+    goal_handle = tester._scaled_follow_joint_trajectory.send_goal(trajectory=trajectory)
+    tester.assertTrue(goal_handle.accepted)
+
+    # Verify execution
+    result = tester._scaled_follow_joint_trajectory.get_result(
+        goal_handle, TIMEOUT_EXECUTE_TRAJECTORY
+    )
+    tester.assertEqual(result.error_code, FollowJointTrajectory.Result.SUCCESSFUL)
+
+
+def sjtc_illegal_trajectory_test(tester, tf_prefix):
+    """
+    Test trajectory server.
+
+    This is more of a validation test that the testing suite does the right thing
+    """
+    tester.assertTrue(
+        tester._controller_manager_interface.switch_controller(
+            strictness=SwitchController.Request.BEST_EFFORT,
+            deactivate_controllers=["passthrough_trajectory_controller"],
+            activate_controllers=["scaled_joint_trajectory_controller"],
+        ).ok
+    )
+    # Construct test trajectory, the second point wrongly starts before the first
+    test_trajectory = [
+        (Duration(sec=6, nanosec=0), [0.0 for j in ROBOT_JOINTS]),
+        (Duration(sec=3, nanosec=0), [-0.5 for j in ROBOT_JOINTS]),
+    ]
+
+    trajectory = JointTrajectory(
+        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
+        ],
+    )
+
+    # Send illegal goal
+    logging.info("Sending illegal goal")
+    goal_handle = tester._scaled_follow_joint_trajectory.send_goal(
+        trajectory=trajectory,
+    )
+
+    # Verify the failure is correctly detected
+    tester.assertFalse(goal_handle.accepted)
+
+
 def _declare_launch_arguments():
     declared_arguments = []
 

ur_robot_driver/test/test_mock_hardware.py

@@ -31,17 +31,13 @@
 import sys
 import time
 import unittest
-import logging
 
 import launch_testing
 import pytest
 import rclpy
 from rclpy.node import Node
-from builtin_interfaces.msg import Duration
 from control_msgs.action import FollowJointTrajectory
 from controller_manager_msgs.srv import SwitchController
-from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
-
 
 sys.path.append(os.path.dirname(__file__))
 from test_common import (  # noqa: E402
@@ -50,8 +46,8 @@
     IoStatusInterface,
     ConfigurationInterface,
     generate_mock_hardware_test_description,
-    ROBOT_JOINTS,
-    TIMEOUT_EXECUTE_TRAJECTORY,
+    sjtc_trajectory_test,
+    sjtc_illegal_trajectory_test,
 )
 
 
@@ -104,66 +100,23 @@ def test_get_robot_software_version(self):
         )
 
     def test_start_scaled_jtc_controller(self):
+        # Deactivate controller, if it is not already
         self.assertTrue(
             self._controller_manager_interface.switch_controller(
                 strictness=SwitchController.Request.BEST_EFFORT,
+                deactivate_controllers=["scaled_joint_trajectory_controller"],
+            ).ok
+        )
+        # Activate controller
+        self.assertTrue(
+            self._controller_manager_interface.switch_controller(
+                strictness=SwitchController.Request.STRICT,
                 activate_controllers=["scaled_joint_trajectory_controller"],
             ).ok
         )
 
     def test_trajectory(self, tf_prefix):
-        """Test robot movement."""
-        # Construct test trajectory
-        test_trajectory = [
-            (Duration(sec=6, nanosec=0), [0.0 for j in ROBOT_JOINTS]),
-            (Duration(sec=9, nanosec=0), [-0.5 for j in ROBOT_JOINTS]),
-            (Duration(sec=12, nanosec=0), [-1.0 for j in ROBOT_JOINTS]),
-        ]
-
-        trajectory = JointTrajectory(
-            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
-            ],
-        )
-
-        # Sending trajectory goal
-        logging.info("Sending simple goal")
-        goal_handle = self._scaled_follow_joint_trajectory.send_goal(trajectory=trajectory)
-        self.assertTrue(goal_handle.accepted)
-
-        # Verify execution
-        result = self._scaled_follow_joint_trajectory.get_result(
-            goal_handle, TIMEOUT_EXECUTE_TRAJECTORY
-        )
-        self.assertEqual(result.error_code, FollowJointTrajectory.Result.SUCCESSFUL)
+        sjtc_trajectory_test(self, tf_prefix)
 
     def test_illegal_trajectory(self, tf_prefix):
-        """
-        Test trajectory server.
-
-        This is more of a validation test that the testing suite does the right thing
-        """
-        # Construct test trajectory, the second point wrongly starts before the first
-        test_trajectory = [
-            (Duration(sec=6, nanosec=0), [0.0 for j in ROBOT_JOINTS]),
-            (Duration(sec=3, nanosec=0), [-0.5 for j in ROBOT_JOINTS]),
-        ]
-
-        trajectory = JointTrajectory(
-            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
-            ],
-        )
-
-        # Send 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)
+        sjtc_illegal_trajectory_test(self, tf_prefix)