implement conversion from JTraj to ROS msg (#11)

src/main.py

-import gcode2contour as gc
 import numpy as np
 import pickle
-from utils import load_plan
+import time
+import sys
+
+# ROS Libraries
+import rospy
+from std_msgs.msg import Bool
+from trajectory_msgs.msg import (
+    JointTrajectoryPoint,
+    JointTrajectory
+)
+
+# MOVEit
+#import moveit_commander
+#from moveit_msgs.msg._RobotTrajectory import RobotTrajectory
+
+## Our libraries
+from utils import load_plan, JTraj2ROS
+#import gcode2contour as gc
+#from extrudex.msg._ExtruderControl import ExtruderControl
+#from extrudex.srv._Hotend import Hotend
+
+
+def execute_plan(plan):
+    """
+    Executes a plan on MoveIt
+
+    Single arm only
+    """
+    for t in plan.trajs[0]:
+        jt = JTraj2ROS(t)
+    return
 
 
 if __name__ == '__main__':

src/utils/__init__.py

-from .planning_tools import JTrajectory
+from .planning_tools import *
 from .plan import load_plan

src/utils/plan.py

-import gcode2contour as gc
 import numpy as np
 import pickle
 from . import JTrajectory

src/utils/planning_tools.py

 """
-Useful functions for planning for the twins
+Useful functions
 """
 import numpy as np
 
+# ROS Libraries
+import rospy
+from std_msgs.msg import Bool
+from trajectory_msgs.msg import (
+    JointTrajectoryPoint,
+    JointTrajectory
+)
 
 class JTrajectory(object):
 
@@ -42,3 +49,18 @@ def joints_at_time(traj, t):
     t2 = traj.time[wp_i]
 
     return (t-t1)/(t2-t1)*(j2-j1) + j1
+
+
+def JTraj2ROS(jtraj):
+    """
+    convert JTrajectory object to ROS JointTrajectory
+    """
+    j = JointTrajectory()
+
+    for i in range(len(jtraj.positions)):
+        jp = JointTrajectoryPoint()
+        jp.positions = jtraj.positions[i]
+        jp.time_from_start = rospy.Duration(jtraj.time[i])
+        j.points.append(jp)
+
+    return j