Following trajectories sent to the action server. Still brittle

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"

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>
 

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)

ur5_driver/test.launch

+<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>

ur5_driver/test_move.py

+#!/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()