update plan loading to include contour data for extrusion (#10)

src/main.py

@@ -38,8 +38,8 @@ def execute_plan(plan, controller):
 
 
 if __name__ == '__main__':
-    plan1 = load_plan('plans/1.plan')
-    plan2 = load_plan('plans/2.plan')
+    plan1 = load_plan('origin')
+    plan2, flexi_contours = load_plan('flexirex', contours=True)
     #con = Controller()
     con = Controller(disable_extruder=True)
     execute_plan(plan2, con)

src/utils/plan.py

 import numpy as np
 import pickle
 from . import JTrajectory
+import gcode2contour as gc
 
 
 def _emptyTraj(joints, arm):
@@ -64,16 +65,24 @@ class Plan:
         return self.jointsAtTime(self.cumul_time[arm][-1], arm)
 
 
-def load_plan(filename):
+def load_plan(model, contours=False):
     """
     load a saved plan
 
     You cannot have existing pybullet instance going
     """
-    with open(filename, 'rb') as f:
+    plan_file = 'plans/' + model + ".plan"
+
+    with open(plan_file, 'rb') as f:
         _, _, start_joints, ctime, trajs = pickle.load(f)
 
     plan = Plan(start_joints)
     plan.trajs = trajs
     plan.cumul_time = ctime
-    return plan
+
+    if contours:
+        # Read gcode file
+        contours = gc.decode_gcode("plans/" + model + ".gcode")
+        return plan, contours
+    else:
+        return plan