finish splitting passthrough controller tests

Author: URJala

ur_robot_driver/test/integration_test_passthrough_controller.py

@@ -77,7 +77,7 @@ def generate_test_description(tf_prefix):
     Duration(sec=8, nanosec=0),
     Duration(sec=12, nanosec=0),
 ]
-TEST_TRAJECTORY = zip(time_vec, waypts)
+TEST_TRAJECTORY = [(time_vec[i], waypts[i]) for i in range(len(waypts))]
 
 
 class RobotDriverTest(unittest.TestCase):
@@ -165,96 +165,116 @@ def test_passthrough_trajectory(self, tf_prefix):
             )
             self.assertEqual(result.error_code, FollowJointTrajectory.Result.SUCCESSFUL)
 
-    # def test_quintic_trajectory(self, tf_prefix):
-    #     # Full quintic trajectory
-
-    #     trajectory = JointTrajectory(
-    #         points=[
-    #             JointTrajectoryPoint(
-    #                 positions=pos,
-    #                 time_from_start=times,
-    #                 velocities=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
-    #                 accelerations=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
-    #             )
-    #             for (times, pos) in TEST_TRAJECTORY
-    #         ],
-    #         joint_names=[tf_prefix + joint for joint in ROBOT_JOINTS],
-    #     )
-    #     goal_time_tolerance = Duration(sec=1, nanosec=0)
-    #     goal_tolerance = [
-    #         JointTolerance(position=0.01, name=tf_prefix + joint) for joint in ROBOT_JOINTS
-    #     ]
-    #     goal_handle = self._passthrough_forward_joint_trajectory.send_goal(
-    #         trajectory=trajectory,
-    #         goal_time_tolerance=goal_time_tolerance,
-    #         goal_tolerance=goal_tolerance,
-    #     )
+    def test_quintic_trajectory(self, tf_prefix):
+        # Full quintic trajectory
+        self.assertTrue(
+            self._controller_manager_interface.switch_controller(
+                strictness=SwitchController.Request.BEST_EFFORT,
+                activate_controllers=["passthrough_trajectory_controller"],
+                deactivate_controllers=["scaled_joint_trajectory_controller"],
+            ).ok
+        )
+        trajectory = JointTrajectory(
+            points=[
+                JointTrajectoryPoint(
+                    positions=pos,
+                    time_from_start=times,
+                    velocities=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+                    accelerations=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+                )
+                for (times, pos) in TEST_TRAJECTORY
+            ],
+            joint_names=[tf_prefix + joint for joint in ROBOT_JOINTS],
+        )
+        goal_time_tolerance = Duration(sec=1, nanosec=0)
+        goal_tolerance = [
+            JointTolerance(position=0.01, name=tf_prefix + joint) for joint in ROBOT_JOINTS
+        ]
+        goal_handle = self._passthrough_forward_joint_trajectory.send_goal(
+            trajectory=trajectory,
+            goal_time_tolerance=goal_time_tolerance,
+            goal_tolerance=goal_tolerance,
+        )
 
-    #     self.assertTrue(goal_handle.accepted)
-    #     if goal_handle.accepted:
-    #         result = self._passthrough_forward_joint_trajectory.get_result(
-    #             goal_handle, TIMEOUT_EXECUTE_TRAJECTORY
-    #         )
-    #         self.assertEqual(result.error_code, FollowJointTrajectory.Result.SUCCESSFUL)
+        self.assertTrue(goal_handle.accepted)
+        if goal_handle.accepted:
+            result = self._passthrough_forward_joint_trajectory.get_result(
+                goal_handle, TIMEOUT_EXECUTE_TRAJECTORY
+            )
+            self.assertEqual(result.error_code, FollowJointTrajectory.Result.SUCCESSFUL)
 
-    # def test_impossible_goal_tolerance_fails(self, tf_prefix):
-    #     # Test impossible goal tolerance, should fail.
+    def test_impossible_goal_tolerance_fails(self, tf_prefix):
+        # Test impossible goal tolerance, should fail.
+        self.assertTrue(
+            self._controller_manager_interface.switch_controller(
+                strictness=SwitchController.Request.BEST_EFFORT,
+                activate_controllers=["passthrough_trajectory_controller"],
+                deactivate_controllers=["scaled_joint_trajectory_controller"],
+            ).ok
+        )
+        trajectory = JointTrajectory(
+            points=[
+                JointTrajectoryPoint(positions=pos, time_from_start=times)
+                for (times, pos) in TEST_TRAJECTORY
+            ],
+            joint_names=[tf_prefix + joint for joint in ROBOT_JOINTS],
+        )
+        goal_tolerance = [
+            JointTolerance(position=0.000000001, name=tf_prefix + joint) for joint in ROBOT_JOINTS
+        ]
+        goal_time_tolerance = Duration(sec=1, nanosec=0)
+        goal_handle = self._passthrough_forward_joint_trajectory.send_goal(
+            trajectory=trajectory,
+            goal_time_tolerance=goal_time_tolerance,
+            goal_tolerance=goal_tolerance,
+        )
+        self.assertTrue(goal_handle.accepted)
+        if goal_handle.accepted:
+            result = self._passthrough_forward_joint_trajectory.get_result(
+                goal_handle, TIMEOUT_EXECUTE_TRAJECTORY
+            )
+            self.assertEqual(
+                result.error_code, FollowJointTrajectory.Result.GOAL_TOLERANCE_VIOLATED
+            )
 
-    #     trajectory = JointTrajectory(
-    #         points=[
-    #             JointTrajectoryPoint(positions=pos, time_from_start=times)
-    #             for (times, pos) in TEST_TRAJECTORY
-    #         ],
-    #         joint_names=[tf_prefix + joint for joint in ROBOT_JOINTS],
-    #     )
-    #     goal_tolerance = [
-    #         JointTolerance(position=0.000000001, name=tf_prefix + joint) for joint in ROBOT_JOINTS
-    #     ]
-    #     goal_time_tolerance = Duration(sec=1, nanosec=0)
-    #     goal_handle = self._passthrough_forward_joint_trajectory.send_goal(
-    #         trajectory=trajectory,
-    #         goal_time_tolerance=goal_time_tolerance,
-    #         goal_tolerance=goal_tolerance,
-    #     )
-    #     self.assertTrue(goal_handle.accepted)
-    #     if goal_handle.accepted:
-    #         result = self._passthrough_forward_joint_trajectory.get_result(
-    #             goal_handle, TIMEOUT_EXECUTE_TRAJECTORY
-    #         )
-    #         self.assertEqual(
-    #             result.error_code, FollowJointTrajectory.Result.GOAL_TOLERANCE_VIOLATED
-    #         )
+    def test_impossible_goal_time_tolerance_fails(self, tf_prefix):
+        # Test impossible goal time
+        self.assertTrue(
+            self._controller_manager_interface.switch_controller(
+                strictness=SwitchController.Request.BEST_EFFORT,
+                activate_controllers=["passthrough_trajectory_controller"],
+                deactivate_controllers=["scaled_joint_trajectory_controller"],
+            ).ok
+        )
 
-    # def test_impossible_goal_time_tolerance_fails(self, tf_prefix):
-    #     # Test impossible goal time
-    #     goal_tolerance = [
-    #         JointTolerance(position=0.01, name=tf_prefix + joint) for joint in ROBOT_JOINTS
-    #     ]
-    #     goal_time_tolerance = Duration(sec=0, nanosec=10)
-    #     trajectory = JointTrajectory(
-    #         points=[
-    #             JointTrajectoryPoint(positions=pos, time_from_start=times)
-    #             for (times, pos) in TEST_TRAJECTORY
-    #         ],
-    #         joint_names=[tf_prefix + joint for joint in ROBOT_JOINTS],
-    #     )
-    #     goal_handle = self._passthrough_forward_joint_trajectory.send_goal(
-    #         trajectory=trajectory,
-    #         goal_time_tolerance=goal_time_tolerance,
-    #         goal_tolerance=goal_tolerance,
-    #     )
-    #     self.assertTrue(goal_handle.accepted)
-    #     if goal_handle.accepted:
-    #         result = self._passthrough_forward_joint_trajectory.get_result(
-    #             goal_handle, TIMEOUT_EXECUTE_TRAJECTORY
-    #         )
-    #         self.assertEqual(
-    #             result.error_code, FollowJointTrajectory.Result.GOAL_TOLERANCE_VIOLATED
-    #         )
-    #     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
-    #     )
+        goal_tolerance = [
+            JointTolerance(position=0.01, name=tf_prefix + joint) for joint in ROBOT_JOINTS
+        ]
+        goal_time_tolerance = Duration(sec=0, nanosec=10)
+        trajectory = JointTrajectory(
+            points=[
+                JointTrajectoryPoint(positions=pos, time_from_start=times)
+                for (times, pos) in TEST_TRAJECTORY
+            ],
+            joint_names=[tf_prefix + joint for joint in ROBOT_JOINTS],
+        )
+        goal_handle = self._passthrough_forward_joint_trajectory.send_goal(
+            trajectory=trajectory,
+            goal_time_tolerance=goal_time_tolerance,
+            goal_tolerance=goal_tolerance,
+        )
+        self.assertTrue(goal_handle.accepted)
+        if goal_handle.accepted:
+            result = self._passthrough_forward_joint_trajectory.get_result(
+                goal_handle, TIMEOUT_EXECUTE_TRAJECTORY
+            )
+            self.assertEqual(
+                result.error_code, FollowJointTrajectory.Result.GOAL_TOLERANCE_VIOLATED
+            )
+        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
+        )