def addPlanAndExecute(name, planFunc):
old_folder = self.folder
addFolder(name, parent=self.folder)
addFunc(name, planFunc)
addTask(robottasks.DelayTask(name='wait', delayTime=0.25))
addTask(robottasks.CheckPlanInfo())
addFunc('execute', self.planner.commitManipPlan)
addFunc('wait for execute', self.planner.waitForExecute)
self.folder = old_folder
def addManipulation(func, name, parent=None, confirm=False):
group = self.taskTree.addGroup(name, parent=parent)
addFunc(func, name='plan motion', parent=group)
addTask(rt.CheckPlanInfo(name='check manip plan info'), parent=group)
addFunc(v.commitManipPlan, name='execute manip plan', parent=group)
addTask(rt.WaitForManipulationPlanExecution(name='wait for manip execution'),
parent=group)
if confirm:
addTask(rt.UserPromptTask(name='Confirm execution has finished', message='Continue when plan finishes.'),
parent=group)
def addManipTask(name, planFunc, userPrompt=False):
prevFolder = self.folder
addFolder(name, prevFolder)
addFunc('plan', planFunc)
if not userPrompt:
addTask(rt.CheckPlanInfo(name='check manip plan info'))
else:
addTask(rt.UserPromptTask(name='approve manip plan', message='Please approve manipulation plan.'))
addFunc('execute manip plan', self.switchPlanner.commitManipPlan)
addTask(rt.WaitForManipulationPlanExecution(name='wait for manip execution'))
self.folder = prevFolder