Re #45 Sync arm and gripper trajectory playback

In RSDK 1.1.1, the amount of time required to move to the initial
position for trajectory playback was not taken into account when
calculating when to start gripper playback. By subtracting this
amount of time from gripper playback start, we end up with
synchronized (or nearly) gripper movements with respect to the
robot's arm movements.

Author: IanTheEngineer

scripts/joint_trajectory_file_playback.py

@@ -113,6 +113,7 @@ def __init__(self):
         # Timing offset to prevent gripper playback before trajectory has started
         self._slow_move_offset = 0.0
         self._trajectory_start_offset = rospy.Duration(0.0)
+        self._trajectory_actual_offset = rospy.Duration(0.0)
 
         #param namespace
         self._param_ns = '/rsdk_joint_trajectory_action_server/'
@@ -121,7 +122,7 @@ def __init__(self):
         self._gripper_rate = 20.0  # Hz
 
     def _execute_gripper_commands(self):
-        start_time = rospy.get_time()
+        start_time = rospy.get_time() - self._trajectory_actual_offset.to_sec()
         r_cmd = self._r_grip.trajectory.points
         l_cmd = self._l_grip.trajectory.points
         pnt_times = [pnt.time_from_start.to_sec() for pnt in r_cmd]
@@ -261,6 +262,7 @@ def _feedback(self, data):
         if (not self._get_trajectory_flag() and
               data.actual.time_from_start >= self._trajectory_start_offset):
             self._set_trajectory_flag(value=True)
+            self._trajectory_actual_offset = data.actual.time_from_start
 
     def _set_trajectory_flag(self, value=False):
         with self._lock: