From a0b1c2c9abfea349cb049e0efb20f542ee10f45d Mon Sep 17 00:00:00 2001
From: Stuart Glaser <sglaser@willowgarage.com>
Date: Tue, 27 Mar 2012 15:51:08 -0700
Subject: [PATCH] Following trajectories sent to the action server. Still
brittle
---
ur5_driver/driver.py | 127 ++++++++++++++++++++++++++++++++++++----
ur5_driver/manifest.xml | 2 +
ur5_driver/prog | 10 ++--
ur5_driver/test.launch | 6 ++
ur5_driver/test_move.py | 37 ++++++++++++
5 files changed, 164 insertions(+), 18 deletions(-)
create mode 100644 ur5_driver/test.launch
create mode 100755 ur5_driver/test_move.py
diff --git a/ur5_driver/driver.py b/ur5_driver/driver.py
index c82e32c..48de91f 100755
--- a/ur5_driver/driver.py
+++ b/ur5_driver/driver.py
@@ -5,8 +5,11 @@ import datetime
import socket
import struct
import SocketServer
+
import rospy
+import actionlib
from sensor_msgs.msg import JointState
+from control_msgs.msg import FollowJointTrajectoryAction
HOSTNAME='ur-xx'
HOSTNAME="10.0.1.20"
@@ -60,9 +63,10 @@ class CommanderTCPHandler(SocketServer.BaseRequestHandler):
if not more:
raise EOF()
return more
-
+
def handle(self):
self.socket_lock = threading.Lock()
+ self.waypoint_finished_cb = None
setConnectedRobot(self)
#print "Handling a request"
try:
@@ -110,7 +114,8 @@ class CommanderTCPHandler(SocketServer.BaseRequestHandler):
buf = buf + self.recv_more()
waypoint_id = struct.unpack_from("!i", buf, 0)[0]
buf = buf[4:]
- log("Waypoint finished: %i" % waypoint_id)
+ if self.waypoint_finished_cb:
+ self.waypoint_finished_cb(waypoint_id)
else:
raise Exception("Unknown message type: %i" % mtype)
@@ -132,6 +137,10 @@ class CommanderTCPHandler(SocketServer.BaseRequestHandler):
with self.socket_lock:
self.request.send(buf)
+ def set_waypoint_finished_cb(self, cb):
+ self.waypoint_finished_cb = cb
+
+
class TCPServer(SocketServer.TCPServer):
allow_reuse_address = True # Allows the program to restart gracefully on crash
timeout = 5
@@ -143,6 +152,92 @@ def joinAll(threads):
for t in threads:
t.join(0.2)
+# Returns the duration between moving from point (index-1) to point
+# index in the given JointTrajectory
+def get_segment_duration(traj, index):
+ if index == 0:
+ return traj.points[0].time_from_start.to_sec()
+ return (traj.points[index].time_from_start - traj.points[index-1].time_from_start).to_sec()
+
+class UR5TrajectoryFollower(object):
+ def __init__(self, robot):
+ self.robot = robot
+ self.server = actionlib.ActionServer("follow_joint_trajectory",
+ FollowJointTrajectoryAction,
+ self.on_goal, self.on_cancel, auto_start=False)
+ self.robot.set_waypoint_finished_cb(self.on_waypoint_finished)
+
+ self.goal_handle = None
+ self.first_waypoint_id = 10
+ self.tracking_i = 0
+ self.pending_i = 0
+ self.goal_start_time = rospy.Time(0)
+
+ self.following_lock = threading.Lock()
+
+ def start(self):
+ self.server.start()
+
+ def on_goal(self, goal_handle):
+ log("on_goal")
+ if self.goal_handle:
+ rospy.logerr("Already have a goal in progress! Rejecting. (TODO)")
+ goal_handle.set_rejected()
+ return
+
+ # TODO: Verify that the goal has the correct joints. Reorder if necessary
+
+ with self.following_lock:
+ self.goal_handle = goal_handle
+ self.tracking_i = 0
+ self.pending_i = 0
+ self.goal_start_time = rospy.get_rostime()
+
+ # Sends a tracking point and a pending point to the robot
+ self.goal_handle.set_accepted()
+ traj = self.goal_handle.get_goal().trajectory
+ # TODO: joints may have a different order
+ self.robot.send_movej(self.first_waypoint_id, traj.points[0].positions,
+ t=get_segment_duration(traj, 0))
+ self.robot.send_movej(self.first_waypoint_id + 1, traj.points[1].positions,
+ t=get_segment_duration(traj, 1))
+ self.tracking_i = 0
+ self.pending_i = 1
+
+ def on_cancel(self):
+ log("on_cancel")
+ print "TODO: on_cancel"
+
+ # The URScript program sends back waypoint_finished messages,
+ # which trigger this callback.
+ def on_waypoint_finished(self, waypoint_id):
+ log("Waypoint finished: %i" % waypoint_id)
+ index = waypoint_id - self.first_waypoint_id
+ if index != self.tracking_i:
+ rospy.logerr("Completed waypoint %i (id=%i), but tracking %i (id=%i)" % \
+ (index, waypoint_id, self.tracking_i,
+ self.first_waypoint_id + self.tracking_i))
+ # TODO: Probably need to fail here
+
+ traj = self.goal_handle.get_goal().trajectory
+ traj_len = len(traj.points)
+
+ # Checks if we've completed the trajectory
+ if index == traj_len - 1:
+ self.goal_handle.set_succeeded()
+ self.first_waypoint_id += traj_len
+ self.goal_handle = None
+
+ # Moves onto the next segment
+ self.tracking_i += 1
+ if self.pending_i + 1 < traj_len:
+ self.pending_i += 1
+ # TODO: reorder joint positions
+ self.robot.send_movej(self.first_waypoint_id + self.pending_i,
+ traj.points[self.pending_i],
+ t=get_segment_duration(traj, self.pending_i))
+
+
sock = None
def main():
global sock
@@ -162,17 +257,25 @@ def main():
r = getConnectedRobot(wait=True)
print "Robot connected"
- log("movej Q1")
- r.send_movej(1, Q1, t=2.0)
- log("movej Q2")
- r.send_movej(2, Q2, t=1.0)
- time.sleep(3)
-
- print "Sending quit"
- r.send_quit()
+ action_server = UR5TrajectoryFollower(r)
+ action_server.start()
+
+ try:
+
+ #log("movej Q1")
+ #r.send_movej(1, Q1, t=2.0)
+ #log("movej Q2")
+ #r.send_movej(2, Q2, t=1.0)
+ #time.sleep(3)
+
+ #r.send_quit()
- # Waits for the threads to finish
- joinAll([thread_commander])
+ # Waits for the threads to finish
+ joinAll([thread_commander])
+ except KeyboardInterrupt:
+ r.send_quit()
+ rospy.signal_shutdown("KeyboardInterrupt")
+ raise
'''
print "Dump"
diff --git a/ur5_driver/manifest.xml b/ur5_driver/manifest.xml
index 4917f4b..6cedb0d 100644
--- a/ur5_driver/manifest.xml
+++ b/ur5_driver/manifest.xml
@@ -10,6 +10,8 @@
<url>http://ros.org/wiki/ur5_driver</url>
<depend package="rospy" />
+ <depend package="actionlib" />
+ <depend package="control_msgs" />
</package>
diff --git a/ur5_driver/prog b/ur5_driver/prog
index 9f5cff9..30280d0 100644
--- a/ur5_driver/prog
+++ b/ur5_driver/prog
@@ -5,7 +5,7 @@ def driverProg():
MSG_JOINT_STATES = 3
MSG_MOVEJ = 4
MSG_WAYPOINT_FINISHED = 5
- MULT_jointstate = 1000
+ MULT_jointstate = 1000.0
MULT_time = 1000000.0
MULT_blend = 1000.0
@@ -70,13 +70,12 @@ def driverProg():
#movej([2.2,0,-1.57,0,0,0],2)
#movej([1.5,0,-1.57,0,0,0],2)
- i = 0
- while i < 100:
- send_out("Listening")
+ while True:
+ #send_out("Listening")
ll = socket_read_binary_integer(1)
if ll[0] == 0:
- send_out("Received nothing")
+ #send_out("Received nothing")
elif ll[0] > 1:
send_out("Received too many things")
else:
@@ -115,7 +114,6 @@ def driverProg():
end
#movej([2.2,0,-1.57,0,0,0],2)
#movej([1.5,0,-1.57,0,0,0],2)
- i = i + 1
end
#sleep(1)
diff --git a/ur5_driver/test.launch b/ur5_driver/test.launch
new file mode 100644
index 0000000..b49ed2b
--- /dev/null
+++ b/ur5_driver/test.launch
@@ -0,0 +1,6 @@
+<launch>
+ <include file="$(find ur5_description)/launch/load.launch" />
+ <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
+ <node name="rviz" pkg="rviz" type="rviz" args="-d $(find urdf_tutorial)/urdf.vcg" />
+
+</launch>
diff --git a/ur5_driver/test_move.py b/ur5_driver/test_move.py
new file mode 100755
index 0000000..4812d7a
--- /dev/null
+++ b/ur5_driver/test_move.py
@@ -0,0 +1,37 @@
+#!/usr/bin/env python
+import roslib; roslib.load_manifest('ur5_driver')
+import rospy
+import actionlib
+from control_msgs.msg import *
+from trajectory_msgs.msg import *
+
+JOINT_NAMES = ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
+ 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint']
+Q1 = [2.2,0,-1.57,0,0,0]
+Q2 = [1.5,0,-1.57,0,0,0]
+
+client = None
+
+def move1():
+ 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))]
+ client.send_goal(g)
+
+def main():
+ global client
+ try:
+ rospy.init_node("test_move", anonymous=True, disable_signals=True)
+ client = actionlib.SimpleActionClient('follow_joint_trajectory', FollowJointTrajectoryAction)
+ print "Waiting for server..."
+ client.wait_for_server()
+ print "Connected to server"
+ move1()
+ except KeyboardInterrupt:
+ rospy.signal_shutdown("KeyboardInterrupt")
+ raise
+
+if __name__ == '__main__': main()
--
GitLab