Manually wait for Traj finish using status monitoring (#10)

src/Controller.py

@@ -167,8 +167,12 @@ class Controller(object):
         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
+        t = traj.points[-1].time_from_start.to_sec()
+        while not self.traj_complete:
+            if time.time() - s > t + 3:
+                Exception("traj_complete failed")
+                break
+        self.traj_complete = False
 
         return True
 
@@ -290,6 +294,8 @@ class Controller(object):
         if len(msg.status_list)>0:
             self.traj_complete = (msg.status_list[-1].status==3 or
                 msg.status_list[-1].status==2)
+            if self.traj_complete:
+                print("Trajectory Execution Completed")
 
 
 def getR_ET():