read status of joint trajectory from controller in order to do things while executing (#10)

src/Controller.py

@@ -21,6 +21,7 @@ from trajectory_msgs.msg import (
     JointTrajectoryPoint,
     JointTrajectory
 )
+from actionlib_msgs.msg import GoalStatusArray
 
 # MOVEit
 import moveit_commander
@@ -76,6 +77,8 @@ class Controller(object):
             queue_size=1)
         self.scaledPosTraj_pub = rospy.Publisher('scaled_pos_traj_controller/command',JointTrajectory,
             queue_size=1)
+        self.motion_feedback_sub = rospy.Subscriber('scaled_pos_traj_controller/follow_joint_trajectory/status',GoalStatusArray,
+            self._update_motion_status)
 
         time.sleep(0.1)
         if not disable_extruder:
@@ -101,6 +104,7 @@ class Controller(object):
         self.retractionDist_mm = retractionDist_mm
         self.retractionTime_s = retractionDist_mm/retractionSpeed_mmpmin * 60.0
         self.extruderReady = False
+        self.traj_complete = False
 
         # Start extruder
         if not disable_extruder:
@@ -160,7 +164,11 @@ class Controller(object):
 
         plan = RobotTrajectory()
         plan.joint_trajectory = scaledTraj
-        self.group.execute(plan)
+        self.group.execute(plan, wait=False)
+
+        s = time.time()
+        while time.time() - s < traj.points[-1].time_from_start.to_sec(): #not self.traj_complete:
+            pass
 
         return True
 
@@ -278,6 +286,11 @@ class Controller(object):
     def _extruderReady_cb(self,ready_boolMsg):
         self.extruderReady = ready_boolMsg.data
 
+    def _update_motion_status(self, msg):
+        if len(msg.status_list)>0:
+            self.traj_complete = (msg.status_list[-1].status==3 or
+                msg.status_list[-1].status==2)
+
 
 def getR_ET():
     return Rotation.from_euler('XYZ',[-45,-90,0],