separate function for extrusion travel (#10)

src/Controller.py

@@ -30,6 +30,7 @@ from moveit_msgs.msg._RobotTrajectory import RobotTrajectory
 ## Our libraries
 from extrudex.msg._ExtruderControl import ExtruderControl
 from extrudex.srv._Hotend import Hotend
+from utils import load_plan, JTraj2ROS
 
 class Controller(object):
     """
@@ -132,6 +133,41 @@ class Controller(object):
         return t
 
 
+    def exec_ctraj(self,jtraj, wait=True):
+        """
+        Using the
+        jtraj: JTrajectory object to execute
+        Returns True if the JointTrajectory is executed; False otherwise.
+        """
+        plan = RobotTrajectory()
+        plan.joint_trajectory = JTraj2ROS(jtraj)
+#        plan.joint_trajectory.points = plan.joint_trajectory.points[1:]
+
+        rospy.loginfo('Sending trajectory to UR5e')
+        self.traj_complete = False
+        self.group.execute(plan, wait=False)
+
+        #if jtraj.contour is not None:
+        #    print("---")
+        #    print("THIS NEXT TRAJ REQUIRES EXTRUSION")
+        #    print(jtraj.contour.Ext)
+        #    print(jtraj.contour.time)
+        #    print("---")
+            #self.extrude()
+
+
+        if wait:
+            s = time.time()
+            t = jtraj.time[-1]
+            while not self.traj_complete:
+                if time.time() - s > t + 3:
+                    Exception("traj_complete failed")
+                    break
+            self.traj_complete = False
+
+        return True
+
+
     def exec_traj(self,traj):
         """
         Executes a JointTrajectory object
@@ -164,15 +200,7 @@ class Controller(object):
 
         plan = RobotTrajectory()
         plan.joint_trajectory = scaledTraj
-        self.group.execute(plan, wait=False)
-
-        s = time.time()
-        t = traj.points[-1].time_from_start.to_sec()
-        while not self.traj_complete:
-            if time.time() - s > t + 3:
-                Exception("traj_complete failed")
-                break
-        self.traj_complete = False
+        self.group.execute(plan, wait=True)
 
         return True
 

src/KeyboardController.py

@@ -592,7 +592,8 @@ class KeyboardController(Controller):
 
 
 if __name__=="__main__":
-    con = KeyboardController(disableExtruder=True)
+    #con = KeyboardController(disableExtruder=True)
+    con = KeyboardController()
 
     print('Collecting configrations...')
     configuations = con.prompt_configurations()

src/main.py

@@ -34,13 +34,12 @@ def execute_plan(plan, controller):
     controller.group.go(plan.trajs[0][0].positions[0])
 
     # execute trajectories in plan
-    for t in plan.trajs[0]:
-        jt = JTraj2ROS(t)
-        try:
-            controller.exec_traj(jt)
-        except:
-            print("Failed trajectory Execution")
-            break
+    for i, t in enumerate(plan.trajs[0]):
+        i+=1
+        if i==1:
+            print("skipping first traj")
+            continue
+        controller.exec_ctraj(t)
     return