def evaluateMessage(self,message):
task_spec = task_specification()
result = re.findall('[DST][0-9]',message)
task_spec.set_initial_place(result[0])
print 'Initial Place: ' + result[0]
task_spec.set_source(result[1])
print 'Source Place: ' + result[1]
task_spec.set_destination(result[2])
print 'Destination Place: ' + result[2]
task_spec.set_final_place(result[3])
print 'Final Place: ' + result[3]
ObjectPositions = re.findall('[a-z]*[(].*[)]',message)
result.append(re.findall('[a-z]*',ObjectPositions[0])[0])
task_spec.set_formation(result[4])
#globalData.objectsPosition = result[4]
print 'Object Position: ' + result[4]
result = result + re.findall('[MSFRV][2-4][0]_?[1-4]?[0]+?_?[BG]?', ObjectPositions[0])
print 'Objects to grasp:'
task_spec.set_objects(re.findall('[MSFRV][2-4][0]_?[1-4]?[0]+?_?[BG]?', ObjectPositions[0]))
print re.findall('[MSFRV][2-4][0]_?[1-4]?[0]+?_?[BG]?', ObjectPositions[0])
return task_spec
def main():
rospy.init_node("youbot_manipulation_ai_re")
manip_ai = YoubotManipulationAI()
manip_ai.setup_introspection()
#Example Task Spec
spec = task_specification()
spec.set_initial_place("T4")
spec.set_source("S5")
spec.set_destination("D2")
spec.set_final_place("T4")
spec.set_formation("line")
spec.set_objects(["R20", "V20", "F20_20_B"])
manip_ai.get_statemachine().userdata.task_specification_manip_in = spec
manip_ai.run()
def parse_task(self, task_str):
rospy.loginfo("BMT Taskspec: %s", task_str)
test, spec = task_str.split("<")
assert test == "BMT"
spec = spec.strip()[:-1] #remove trailing '>'
initial, source, dest, remaining_spec = spec.split(",", 3)
place_str, final = remaining_spec.split(")")
final = final.replace(",", "").strip()
align, obj_str = place_str.split("(")
objects = obj_str.split(",")
taskspec = task_specification()
print "[ParseBMTTask] initial: %s, source: %s, dest: %s, formation: %s, objects: %s, final: %s" % (initial, source, dest, align, objects, final)
taskspec.initial_place = initial
taskspec.source = source
taskspec.destination = dest
taskspec.formation = align
taskspec.objects = objects
taskspec.final_place = final
return taskspec