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