From 499bbab3b1638309d3d0f83ac1e56d01bf173bec Mon Sep 17 00:00:00 2001
From: Stuart Glaser <sglaser@willowgarage.com>
Date: Tue, 27 Mar 2012 16:37:16 -0700
Subject: [PATCH] Canceling goals now works
---
ur5_driver/driver.py | 19 +++++++++++++++++--
ur5_driver/prog | 4 ++++
ur5_driver/test_move.py | 32 ++++++++++++++++++++++++++++++--
3 files changed, 51 insertions(+), 4 deletions(-)
diff --git a/ur5_driver/driver.py b/ur5_driver/driver.py
index 3d5ee73..6cd63da 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 30280d0..0164d4a 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 a3d6ec0..8a2719a 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")
--
GitLab