Commit dcc5df41 authored by Jayant Khatkar's avatar Jayant Khatkar
Browse files

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

parent 2e3651c9
...@@ -167,8 +167,12 @@ class Controller(object): ...@@ -167,8 +167,12 @@ class Controller(object):
self.group.execute(plan, wait=False) self.group.execute(plan, wait=False)
s = time.time() s = time.time()
while time.time() - s < traj.points[-1].time_from_start.to_sec(): #not self.traj_complete: t = traj.points[-1].time_from_start.to_sec()
pass while not self.traj_complete:
if time.time() - s > t + 3:
Exception("traj_complete failed")
break
self.traj_complete = False
return True return True
...@@ -290,6 +294,8 @@ class Controller(object): ...@@ -290,6 +294,8 @@ class Controller(object):
if len(msg.status_list)>0: if len(msg.status_list)>0:
self.traj_complete = (msg.status_list[-1].status==3 or self.traj_complete = (msg.status_list[-1].status==3 or
msg.status_list[-1].status==2) msg.status_list[-1].status==2)
if self.traj_complete:
print("Trajectory Execution Completed")
def getR_ET(): def getR_ET():
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment