def enter(self):
print "entering execute"
while self.active and len(tasks) > 0:
interface.clear_goals()
for goal in tasks[0]:
interface.add_goal(goal)
interface.plan()
print "executing task"
rospy.sleep(3)
# print interface.get_dispatch_status()
while not interface.is_done(): # rosplan not done yet
# print interface.get_dispatch_status()
if not self.active:
return
rospy.sleep(.1)
print "completed task"
tasks.pop(0)
self.active = False
def leave(self):
print "leaving execute"
interface.cancel()
interface.clear_goals()
