sending Trajectory

controller.py

@@ -5,6 +5,8 @@ from trajectory_msgs.msg import (
 )
 from sensor_msgs.msg import JointState
 import time
+from copy import copy, deepcopy
+
 
 class controller:
 
@@ -13,17 +15,17 @@ class controller:
         Initiate connection to UR5
         """
         self.node = rospy.init_node("py_controller", anonymous = False)
-        self.traj = rospy.Publisher('scaled_pos_traj_controller/command', JointTrajectory, queue_size=10)
+        self.traj = rospy.Publisher("scaled_pos_traj_controller/command", JointTrajectory, queue_size=1)
         self.joint_state = None
+        time.sleep(1)
 
 
     def exec_traj(self, traj):
         """
         execute a JointTrajectory object
-
         """
         rospy.loginfo("sending trajectroy to UR5")
-        pub.publish(traj)
+        self.traj.publish(traj)
 
 
     def get_joints(self):
@@ -31,20 +33,52 @@ class controller:
         Get current state of UR5
         """
         def joint_state_setter(data):
-                self.joint_state = data.position
+                self.joint_state = data
                 self.sb.unregister()
 
         self.sb = rospy.Subscriber("joint_states", JointState, joint_state_setter)
 
         time.sleep(0.1)
-        return self.joint_state
+        return self.joint_state.position
+
+
+    def empty_traj(self):
+        """
+        Empty trajectory with Joint names filled in
+        """
+        t = JointTrajectory()
+        t.joint_names = self.joint_state.name
+        return t
 
 
 if __name__=="__main__":
     
-    print "Testing joint angle"
+    ########################################
+    print "Testing joint angle subscriber"
 
     con = controller()
 
     print con.get_joints()
 
+    ########################################
+    print "Testing Joint Trajectory controller"
+
+    t = con.empty_traj()
+    js1 = (-1.3825197219848633,-1.7300764522948207,1.800080776214599,-1.4899586004069825,1.5598526000976562,0.7699174880981445)
+    js2 = (-1.3825197219848633,-1.7300764522948207,2.323414109547933,-1.4899586004069825,1.5598526000976562,0.7699174880981445)
+
+    p1 = JointTrajectoryPoint()
+    p1.positions = copy(js1)
+    p1.time_from_start = rospy.Time(0)
+    p2 = JointTrajectoryPoint()
+    p2.positions = copy(js2)
+    p2.time_from_start = rospy.Time(3)
+    p3 = JointTrajectoryPoint()
+    p3.positions = copy(js1)
+    p3.time_from_start = rospy.Time(6)
+    t.points.append(p1)
+    t.points.append(p2)
+    t.points.append(p3)
+
+    print t
+    con.exec_traj(t)