def call_follow(): #TODO print "SM : follow-me" tosay = "I'm going to follow you" speak = SpeakActionState(text=tosay) speak.execute(ud=None) sm =FollowAndStop(0.9, 'gpsr_follow') out = sm.execute(ud=None) return succeeded
def call_point_at(): #TO TEST out = aborted tries = 0 while(out==aborted and tries<3): print "SM : point_at" tosay = "I'm going to point" speak = SpeakActionState(text=tosay) speak.execute(ud=None) sm =PointAtSM() out = sm.execute() tries = tries+1 return succeeded
def call_bring_to(person_name): #Solo hace release TODO out = aborted tries = 0 while(out==aborted and tries<3): print "SM : bring_to %s" % (person_name) tosay = "Take it please" speak = SpeakActionState(text=tosay) speak.execute(ud=None) r = ReleaseSM() r.userdata.releasing_position = None; out = r.execute() return succeeded '''
def call_grasp(obj):
out = aborted
tries = 0
while(out==aborted and tries<5):
print "SM : grasp %s" % (obj)
tosay = "I'm going to grasp the " + obj
speak = SpeakActionState(text=tosay)
speak.execute(ud=None)
grasp = GraspSM()
grasp.userdata._data = {'object_to_search_for': obj, 'ask_for_help_key': False}
out = grasp.execute()
tries = tries+1
return succeeded
'''
def call_recognize_person(): out = aborted tries = 0 while(out==aborted and tries<3): print "SM : recognize_person" tosay = "I'm going to recognize the person in front of me" speak = SpeakActionState(text=tosay) speak.execute(ud=None) rp = RecognizePersonSM() out = rp.execute() PersonName = rp.userdata.out_person_name tries = tries+1 return succeeded #(out, PersonName) '''
def call_go_to(loc_name):
print "SM : go_to %s" % (loc_name)
tosay = "I'm going to the "+loc_name
SAS = SpeakActionState(text=tosay)
out = SAS.execute(ud=None)
# mr = MoveToRoomSM()
# # mr.userdata._data = {'room_name': loc_name}
# mr.userdata.room_name = loc_name
# out = mr.execute()
# return out
time.sleep(3)
return "succeeded"
def call_learn_person(): out = aborted tries = 0 while(out==aborted and tries<3): print "SM : learn_person" tosay = "I'm going to learn the person in front of me" speak = SpeakActionState(text=tosay) speak.execute(ud=None) lp = LearnPersonSM() out = lp.execute() #PersonName = lp.userdata.out_person_name tries = tries+1 #time.sleep(3) return succeeded #(out, PersonName) '''
def call_go_to(loc_name):
out = aborted
tries = 0
while(out==aborted and tries<3):
if loc_name == 'exit':
out = call_exit()
else:
print "SM : go_to %s" % (loc_name)
tosay = "I'm going to the "+loc_name
speak = SpeakActionState(text=tosay)
speak.execute(ud=None)
mr = MoveToRoomSM()
mr.userdata._data = {'room_name': loc_name.replace(' ', '_')}
#mr.userdata.room_name = loc_name
out = mr.execute()
tries = tries+1
return succeeded
def call_exit(): #TODO well
out = aborted
tries = 0
while(out==aborted and tries<3):
print "SM : go_to exit"
tosay = "I'm going to the exit"
speak = SpeakActionState(text=tosay)
speak.execute(ud=None)
mr = MoveToRoomSM()
mr.userdata._data = {'room_name': 'pre_exit'} #preguntar gerard
#mr.userdata.room_name = loc_name
out = mr.execute()
# sr = ServiceState('/alive_engine/stop', Empty)
# out = sr.execute(ud=None)
mr.userdata._data = {'room_name': 'exit'}
out = mr.execute()
tries = tries+1
return succeeded
'''time.sleep(3)
def call_find_object(object_name, loc_name): out = aborted tries = 0 while(out==aborted and tries<3): print "SM : find_object %s" % (object_name) tosay = "I'm going to search for "+object_name speak = SpeakActionState(text=tosay) speak.execute(ud=None) sm = SearchObjSM() sm.userdata.object_to_search_for = object_name sm.execute() for i in range(len(sm.userdata.object_found.object_list)): try: sm.userdata.object_found.object_list[i].name == object_name return succeeded except IndexError: tries = tries+1 if (out == aborted): tosayn = "Here it should be the " + object_name + " but I can't see it" speakn = SpeakActionState(text=tosayn) speakn.execute(ud=None) return succeeded '''
