Supports interruting in-progress trajectories

ur5_driver/driver.py

@@ -214,9 +214,10 @@ class UR5TrajectoryFollower(object):
                 
         with self.following_lock:
             if self.goal_handle:
-                rospy.logerr("Already have a goal in progress!  Rejecting.  (TODO)")
-                goal_handle.set_rejected()
-                return
+                # Cancels the existing goal
+                self.goal_handle.set_canceled()
+                self.first_waypoint_id += len(self.goal_handle.get_goal().trajectory.points)
+                self.goal_handle = None
 
             self.goal_handle = goal_handle
             self.tracking_i = 0
@@ -251,6 +252,8 @@ class UR5TrajectoryFollower(object):
             log("Waypoint finished: %i" % waypoint_id)
             if not self.goal_handle:
                 return
+            if waypoint_id < self.first_waypoint_id:
+                return
             
             index = waypoint_id - self.first_waypoint_id
             if index != self.tracking_i:

ur5_driver/test_move.py

 #!/usr/bin/env python
+import time
 import roslib; roslib.load_manifest('ur5_driver')
 import rospy
 import actionlib
@@ -67,6 +68,24 @@ def move_repeated():
         client.cancel_goal()
         raise
 
+def move_interrupt():
+    g = FollowJointTrajectoryGoal()
+    g.trajectory = JointTrajectory()
+    g.trajectory.joint_names = JOINT_NAMES
+    g.trajectory.points = [
+        JointTrajectoryPoint(positions=Q1, velocities=[0]*6, time_from_start=rospy.Duration(2.0)),
+        JointTrajectoryPoint(positions=Q2, velocities=[0]*6, time_from_start=rospy.Duration(3.0)),
+        JointTrajectoryPoint(positions=Q3, velocities=[0]*6, time_from_start=rospy.Duration(4.0))]
+    
+    client.send_goal(g)
+    time.sleep(2.0)
+    print "Interrupting"
+    client.send_goal(g)
+    try:
+        client.wait_for_result()
+    except KeyboardInterrupt:
+        client.cancel_goal()
+        raise
 
 def main():
     global client
@@ -76,8 +95,10 @@ def main():
         print "Waiting for server..."
         client.wait_for_server()
         print "Connected to server"
-        move_repeated()
+        #move1()
+        #move_repeated()
         #move_disordered()
+        move_interrupt()
     except KeyboardInterrupt:
         rospy.signal_shutdown("KeyboardInterrupt")
         raise