print which trajectory fails for manual resume, interface for resuming from nth traj(#17)

src/main.py

@@ -18,20 +18,21 @@ from Controller import Controller
 import gcode2contour as gc
 
 
-def execute_plan(plan, controller, contours=None, confirm=False):
+def execute_plan(plan, controller, contours=None, confirm=False, skip_first=1):
     """
     Executes a plan on MoveIt
 
     Single arm only
+    skips the first n trajectories in the plan
     """
     # plan to the start position of the plan
-    controller.group.go(plan.trajs[0][0].positions[0])
+    controller.group.go(plan.trajs[0][skip_first].positions[0])
 
     # execute trajectories in plan
     for i, t in enumerate(plan.trajs[0]):
 
-        if i==0:
-            print("skipping first traj")
+        if i < skip_first:
+            print("skipping traj {}".format(i))
             continue
 
         if confirm:
@@ -40,8 +41,10 @@ def execute_plan(plan, controller, contours=None, confirm=False):
                 break
 
         if t.contour is not None:
+            print("Printing {}th trajectory in the plan, which is contour {}".format(i, t.contour))
             controller.exec_ctraj(speed_multiplier(t, 5), contour=contours[t.contour])
         else:
+            print("Moving arm, no extrusion")
             controller.exec_ctraj(t)
     return