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()