add structure for moveIt execution (#11)

src/main.py

@@ -17,12 +17,13 @@ from trajectory_msgs.msg import (
 
 ## Our libraries
 from utils import load_plan, JTraj2ROS
+from Controller import Controller
 #import gcode2contour as gc
 #from extrudex.msg._ExtruderControl import ExtruderControl
 #from extrudex.srv._Hotend import Hotend
 
 
-def execute_plan(plan):
+def execute_plan(plan, controller):
     """
     Executes a plan on MoveIt
 
@@ -30,8 +31,15 @@ def execute_plan(plan):
     """
     for t in plan.trajs[0]:
         jt = JTraj2ROS(t)
+        try:
+            controller.exec_traj(jt)
+        except:
+            print("Failed trajectory Execution")
+            break
     return
 
 
 if __name__ == '__main__':
     plan = load_plan('plans/1.plan')
+    con = Controller()
+    execute_plan(plan, con)