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

* passthrough_trajectory: Add a test with a full quintic trajectory

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

(cherry picked from commit 65679b2)

# Conflicts:
#	ur_controllers/src/passthrough_trajectory_controller.cpp

Author: urfeex

ur_controllers/src/passthrough_trajectory_controller.cpp

@@ -579,17 +579,35 @@ bool PassthroughTrajectoryController::check_goal_tolerance()
       return false;
     }
 
+<<<<<<< HEAD
     if (!active_joint_traj_.points.back().velocities.empty()) {
       const double joint_vel = joint_velocity_state_interface_[i].get().get_value();
+=======
+    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;
+      }
+>>>>>>> 65679b2 (Fix passthrough controller to not read non-existing state_interfaces (#1314))
       const auto& expected_vel = active_joint_traj_.points.back().velocities[joint_mapping->at(joint_name)];
       if (std::abs(joint_vel - expected_vel) > joint_tol.velocity) {
         return false;
       }
     }
+<<<<<<< HEAD
     if (!active_joint_traj_.points.back().accelerations.empty()) {
       const double joint_vel = joint_acceleration_state_interface_[i].get().get_value();
       const auto& expected_vel = active_joint_traj_.points.back().accelerations[joint_mapping->at(joint_name)];
       if (std::abs(joint_vel - expected_vel) > joint_tol.acceleration) {
+=======
+    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;
+      }
+      const auto& expected_acc = active_joint_traj_.points.back().accelerations[joint_mapping->at(joint_name)];
+      if (std::abs(joint_acc.value() - expected_acc) > joint_tol.acceleration) {
+>>>>>>> 65679b2 (Fix passthrough controller to not read non-existing state_interfaces (#1314))
         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))