Fix passthrough controller to not read non-existing state_interfaces (UniversalRobots#1314)

* passthrough_trajectory: Add a test with a full quintic trajectory

* Check for state interfaces being present before attempting to read from it

Author: urfeex

ur_controllers/src/passthrough_trajectory_controller.cpp

@@ -597,7 +597,7 @@ bool PassthroughTrajectoryController::check_goal_tolerance()
       return false;
     }
 
-    if (!active_joint_traj_.points.back().velocities.empty()) {
+    if (!active_joint_traj_.points.back().velocities.empty() && !joint_velocity_state_interface_.empty()) {
       const auto joint_vel = joint_velocity_state_interface_[i].get().get_optional();
       if (!joint_vel.has_value()) {
         return false;
@@ -607,7 +607,7 @@ bool PassthroughTrajectoryController::check_goal_tolerance()
         return false;
       }
     }
-    if (!active_joint_traj_.points.back().accelerations.empty()) {
+    if (!active_joint_traj_.points.back().accelerations.empty() && !joint_acceleration_state_interface_.empty()) {
       const auto joint_acc = joint_acceleration_state_interface_[i].get().get_optional();
       if (!joint_acc.has_value()) {
         return false;

ur_robot_driver/test/robot_driver.py

@@ -360,7 +360,7 @@ def test_passthrough_trajectory(self, tf_prefix):
         )
         waypts = [
             [-1.58, -1.692, -1.4311, -0.0174, 1.5882, 0.0349],
-            [-3, -1.692, -1.4311, -0.0174, 1.5882, 0.0349],
+            [-2.5, -1.692, -1.4311, -0.0174, 1.5882, 0.0349],
             [-1.58, -1.692, -1.4311, -0.0174, 1.5882, 0.0349],
         ]
         time_vec = [
@@ -392,7 +392,42 @@ def test_passthrough_trajectory(self, tf_prefix):
                 goal_handle, TIMEOUT_EXECUTE_TRAJECTORY
             )
             self.assertEqual(result.error_code, FollowJointTrajectory.Result.SUCCESSFUL)
+
+        # Full quintic trajectory
+        test_trajectory = zip(time_vec, waypts)
+        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 + ROBOT_JOINTS[i] for i in range(len(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)
+
         # Test impossible goal tolerance, should fail.
+        test_trajectory = zip(time_vec, waypts)
+        trajectory = JointTrajectory(
+            points=[
+                JointTrajectoryPoint(positions=pos, time_from_start=times)
+                for (times, pos) in test_trajectory
+            ],
+            joint_names=[tf_prefix + ROBOT_JOINTS[i] for i in range(len(ROBOT_JOINTS))],
+        )
         goal_tolerance = [
             JointTolerance(position=0.000000001, name=tf_prefix + ROBOT_JOINTS[i])
             for i in range(len(ROBOT_JOINTS))