From 0065ad8cd939a13afaa67dcffbdc79502cec0801 Mon Sep 17 00:00:00 2001
From: Stuart Glaser <sglaser@willowgarage.com>
Date: Tue, 27 Mar 2012 16:50:55 -0700
Subject: [PATCH] Supports interruting in-progress trajectories
---
ur5_driver/driver.py | 9 ++++++---
ur5_driver/test_move.py | 23 ++++++++++++++++++++++-
2 files changed, 28 insertions(+), 4 deletions(-)
diff --git a/ur5_driver/driver.py b/ur5_driver/driver.py
index 6cd63da..d350e93 100755
--- a/ur5_driver/driver.py
+++ b/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:
diff --git a/ur5_driver/test_move.py b/ur5_driver/test_move.py
index 8a2719a..e0364b7 100755
--- a/ur5_driver/test_move.py
+++ b/ur5_driver/test_move.py
@@ -1,4 +1,5 @@
#!/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
--
GitLab