diff --git a/ur5_driver/driver.py b/ur5_driver/driver.py
index 3d5ee73a22d5f8d05df6f7c153a47b3c1f219b87..6cd63da3e261f146a826410cf340156df20103eb 100755
--- a/ur5_driver/driver.py
+++ b/ur5_driver/driver.py
@@ -21,6 +21,7 @@ MSG_QUIT = 2
 MSG_JOINT_STATES = 3
 MSG_MOVEJ = 4
 MSG_WAYPOINT_FINISHED = 5
+MSG_STOPJ = 6
 MULT_jointstate = 1000.0
 MULT_time = 1000000.0
 MULT_blend = 1000.0
@@ -138,6 +139,10 @@ class CommanderTCPHandler(SocketServer.BaseRequestHandler):
         with self.socket_lock:
             self.request.send(buf)
 
+    def send_stopj(self):
+        with self.socket_lock:
+            self.request.send(struct.pack("!i", MSG_STOPJ))
+
     def set_waypoint_finished_cb(self, cb):
         self.waypoint_finished_cb = cb
     
@@ -228,15 +233,25 @@ class UR5TrajectoryFollower(object):
             self.tracking_i = 0
             self.pending_i = 1
 
-    def on_cancel(self):
+    def on_cancel(self, goal_handle):
         log("on_cancel")
-        print "TODO: on_cancel"
+        if goal_handle == self.goal_handle:
+            with self.following_lock:
+                self.robot.send_stopj()
+                self.goal_handle.set_canceled()
+                self.first_waypoint_id += len(self.goal_handle.get_goal().trajectory.points)
+                self.goal_handle = None
+        else:
+            goal_handle.set_canceled()
 
     # The URScript program sends back waypoint_finished messages,
     # which trigger this callback.
     def on_waypoint_finished(self, waypoint_id):
         with self.following_lock:
             log("Waypoint finished: %i" % waypoint_id)
+            if not self.goal_handle:
+                return
+            
             index = waypoint_id - self.first_waypoint_id
             if index != self.tracking_i:
                 rospy.logerr("Completed waypoint %i (id=%i), but tracking %i (id=%i)" % \
diff --git a/ur5_driver/prog b/ur5_driver/prog
index 30280d0af3d343307c7776de2e9ee7d671e64ca6..0164d4a2916a20d04c323067233ba3658081879f 100644
--- a/ur5_driver/prog
+++ b/ur5_driver/prog
@@ -5,6 +5,7 @@ def driverProg():
   MSG_JOINT_STATES = 3
   MSG_MOVEJ = 4
   MSG_WAYPOINT_FINISHED = 5
+  MSG_STOPJ = 6
   MULT_jointstate = 1000.0
   MULT_time = 1000000.0
   MULT_blend = 1000.0
@@ -108,6 +109,9 @@ def driverProg():
 	movej(q, a, v, t, r)
 	send_waypoint_finished(waypoint_id)
 	send_out("movej finished")
+      elif mtype == MSG_STOPJ:
+        send_out("Received stopj")
+	stopj(1.0)
       else:
         send_out("Received unknown message type")
       end
diff --git a/ur5_driver/test_move.py b/ur5_driver/test_move.py
index a3d6ec088f16a1915b81ae077cd8feee99fddf08..8a2719a3490908990f1345c09dd675374d6b0b5b 100755
--- a/ur5_driver/test_move.py
+++ b/ur5_driver/test_move.py
@@ -22,7 +22,11 @@ def move1():
         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)
-    client.wait_for_result()
+    try:
+        client.wait_for_result()
+    except KeyboardInterrupt:
+        client.cancel_goal()
+        raise
 
 def move_disordered():
     order = [4, 2, 3, 1, 5, 0]
@@ -39,6 +43,30 @@ def move_disordered():
     client.send_goal(g)
     client.wait_for_result()
     
+def move_repeated():
+    g = FollowJointTrajectoryGoal()
+    g.trajectory = JointTrajectory()
+    g.trajectory.joint_names = JOINT_NAMES
+    
+    d = 2.0
+    g.trajectory.points = []
+    for i in range(10):
+        g.trajectory.points.append(
+            JointTrajectoryPoint(positions=Q1, velocities=[0]*6, time_from_start=rospy.Duration(d)))
+        d += 1
+        g.trajectory.points.append(
+            JointTrajectoryPoint(positions=Q2, velocities=[0]*6, time_from_start=rospy.Duration(d)))
+        d += 1
+        g.trajectory.points.append(
+            JointTrajectoryPoint(positions=Q3, velocities=[0]*6, time_from_start=rospy.Duration(d)))
+        d += 2
+    client.send_goal(g)
+    try:
+        client.wait_for_result()
+    except KeyboardInterrupt:
+        client.cancel_goal()
+        raise
+
 
 def main():
     global client
@@ -48,7 +76,7 @@ def main():
         print "Waiting for server..."
         client.wait_for_server()
         print "Connected to server"
-        move1()
+        move_repeated()
         #move_disordered()
     except KeyboardInterrupt:
         rospy.signal_shutdown("KeyboardInterrupt")