def addManipulation(func, name, parent=None):
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)
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