def call_go_to(loc_name,world):
tosay = "I'm going to the "+str(loc_name)
speak = speaker(tosay,wait=False)
speak.execute()
rospy.logwarn('call_go_to '+ loc_name + ' from :')
#############################################################################
if SKILLS :
if (time.time()-TIME_INIT) > 270:
return "succeeded"
out = 'aborted'
tries = 0
while(out=='aborted' and tries<3):
tries = tries+1
if world.get_current_position() == loc_name:
out = 'succeeded'
else:
if loc_name == "exit":
sm = nav_to_poi(poi_name = "door_B")
out = sm.execute()
sm = nav_to_poi(poi_name = loc_name)
out = sm.execute()
if out=='aborted':
tosay = "I can't reach the " + loc_name + ". The door is closed. I'm going to inform"
speak = speaker(tosay)
speak.execute()
rospy.logwarn('FAIL IN REACHING ' + loc_name)
time.sleep(SLEEP_TIME)
sm = nav_to_poi(poi_name = 'referee')
out = sm.execute()
tosay = "I can't reach the " + loc_name + ". The door is closed. The sentence is from category 3"
speak = speaker(tosay)
speak.execute()
return "aborted"
else:
tosay = "I arrived to the " + loc_name
speak = speaker(tosay)
speak.execute()
#############################################################################
world.set_current_position(loc_name)
rospy.logwarn(world.get_current_position())
time.sleep(SLEEP_TIME)
return "succeeded"
def call_go_to(loc_name, world):
tosay = "I'm going to the " + str(loc_name)
speak = speaker(tosay, wait=False)
speak.execute()
rospy.logwarn('call_go_to ' + loc_name + ' from :')
#############################################################################
if SKILLS:
if (time.time() - TIME_INIT) > 270:
return "succeeded"
out = 'aborted'
tries = 0
while (out == 'aborted' and tries < 3):
tries = tries + 1
if world.get_current_position() == loc_name:
out = 'succeeded'
else:
if loc_name == "exit":
sm = nav_to_poi(poi_name="door_B")
out = sm.execute()
sm = nav_to_poi(poi_name=loc_name)
out = sm.execute()
if out == 'aborted':
tosay = "I can't reach the " + loc_name + ". The door is closed. I'm going to inform"
speak = speaker(tosay)
speak.execute()
rospy.logwarn('FAIL IN REACHING ' + loc_name)
time.sleep(SLEEP_TIME)
sm = nav_to_poi(poi_name='referee')
out = sm.execute()
tosay = "I can't reach the " + loc_name + ". The door is closed. The sentence is from category 3"
speak = speaker(tosay)
speak.execute()
return "aborted"
else:
tosay = "I arrived to the " + loc_name
speak = speaker(tosay)
speak.execute()
#############################################################################
world.set_current_position(loc_name)
rospy.logwarn(world.get_current_position())
time.sleep(SLEEP_TIME)
return "succeeded"
def call_find_object(object_name,world): #TODO
tosay = "I'm going to search for " + object_name
speak = speaker(tosay)
speak.execute()
rospy.logwarn('call_find_object '+object_name)
#############################################################################
if SKILLS :
if (time.time()-TIME_INIT) > 270:
return "succeeded"
out = 'aborted'
tries = 0
current_position = world.get_current_position()
if current_position in ROOMS:
room = rospy.get_param('/robocup_params/room/' + current_position)
#while(out=='aborted' and tries<3):
for table in room :
if out == 'succeeded' and tries == 3:
break
call_go_to(table)
sm = object_detect_sm()#
out = sm.execute() # #PROVABLY WE WILL HAVE TO CHECK IF THERE IS A TABLE NEARBY BEFORE STARTING TO SEARCH
object_position = sm.userdata.object_pose #
tries = tries+1#
if out=='aborted':
tosay = "I couldn't find the " + object_name + " you asked for. It isn't here. I'm going to the referee to inform"
speak = speaker(tosay)
speak.execute()
rospy.logwarn('FAIL IN FINDING ' + object_name)
time.sleep(SLEEP_TIME)
call_go_to('referee',world)
tosay = "I couldn't find the " + object_name + " you asked for. It isn't there. I'm afraid that sentence was from category 3"
speak = speaker(tosay)
speak.execute()
return "aborted"
#############################################################################
time.sleep(SLEEP_TIME)
return "succeeded"
def main(world):
print "******************************\n******************************\nNew goal\n******************************\n******************************\n"
first_time = time.time()
kernel = create_kernel()
agent = create_agent(kernel, "agent")
agent_load_productions(agent, SOAR_GP_PATH)
agent.SpawnDebugger()
# p_cmd = 'learn --on'
# res = agent.ExecuteCommandLine(p_cmd)
# res = kernel.ExecuteCommandLine(p_cmd, agent.GetAgentName)
kernel.CheckForIncomingCommands()
p_cmd = 'watch --learning 2'
res = agent.ExecuteCommandLine(p_cmd)
print str(res)
goal_achieved = False
while not goal_achieved:
agent.Commit()
agent.RunSelfTilOutput()
agent.Commands()
numberCommands = agent.GetNumberCommands()
print "Numero de comandos recibidos del agente: %s" % (numberCommands)
i = 0
if numberCommands == 0:
print 'KABOOOOOOOOOOOOOOOOOOM!!!!!!!!!!!!!!!'
return 'aborted'
else:
while i < numberCommands:
command = agent.GetCommand(i)
command_name = command.GetCommandName()
print "El nombre del commando %d/%d es %s" % (
i + 1, numberCommands, command_name)
rospy.logwarn(world.get_current_position())
rospy.logwarn(str(time.time()) + ' ' + str(TIME_INIT))
if (time.time() - TIME_INIT) > 270:
tosay = "time out."
speak = speaker(tosay)
speak.execute()
call_go_to('referee', world)
return "succeeded"
out = "NULL"
if command_name == "navigate":
loc_to_navigate = command.GetParameterValue("loc")
loc = idx2obj(int(loc_to_navigate), 'LOCATIONS')
print loc
if (loc == "NULL"):
print "ERROR: la loacalizacion %s no existe" % (
loc_to_navigate)
out = call_go_to(loc, world)
elif command_name == "grasp":
obj_to_grasp = command.GetParameterValue("obj")
obj = idx2obj(int(obj_to_grasp), 'ITEMS')
print obj
if (obj == "NULL"):
print "ERROR: el objeto %s no existe" % (obj_to_grasp)
out = call_grasp(obj, world)
elif command_name == "deliver": #to Person
try:
to_pers = command.GetParameterValue("pers")
pers = idx2obj(int(to_pers), 'PERSONS')
print pers
if (pers == "NULL"):
print "ERROR: la persona %s no existe" % (to_pers)
out = call_bring_to(pers)
except: #or to Loc
to_loc = command.GetParameterValue("loc")
try:
loc = idx2obj(int(to_loc), 'LOCATIONS')
print loc
if (loc == "NULL"):
print "ERROR: l'objecte %s no existe" % (
to_loc)
out = call_bring_to_loc(loc)
except:
loc = ''
print loc
out = call_bring_to_loc(loc)
elif command_name == "search-object":
obj_to_search = command.GetParameterValue("obj")
obj = idx2obj(int(obj_to_search), 'ITEMS')
print obj
if (obj == "NULL"):
print "ERROR: el objeto %s no existe" % (obj_to_search)
out = call_find_object(obj, world)
elif command_name == "search-person":
pers_to_search = command.GetParameterValue("pers")
pers = idx2obj(int(pers_to_search), 'PERSONS')
print pers
if (pers == "NULL"):
print "ERROR: la persona %s no existe" % (
pers_to_search)
out = call_find_person(pers)
elif command_name == "point-obj":
to_loc = command.GetParameterValue("loc")
loc = idx2obj(int(to_loc), 'LOCATIONS')
out = call_point_at(loc)
elif command_name == "ask-name":
out = call_ask_name()
elif command_name == "follow":
to_pers = command.GetParameterValue("pers")
pers = idx2obj(int(to_pers), 'PERSONS')
out = call_follow(pers)
elif command_name == "introduce-me":
out = call_introduce_me()
elif command_name == "memorize-person":
to_pers = command.GetParameterValue("pers")
pers = idx2obj(int(to_pers), 'PERSONS')
out = call_learn_person(pers)
elif command_name == "recognize-person":
to_pers = command.GetParameterValue("pers")
pers = idx2obj(int(to_pers), 'PERSONS')
out = call_recognize_person(pers)
elif command_name == "guide":
loc_to_navigate = command.GetParameterValue("loc")
loc = idx2obj(int(loc_to_navigate), 'LOCATIONS')
print loc
if (loc == "NULL"):
print "ERROR: la loacalizacion %s no existe" % (
loc_to_navigate)
out = call_guide_to(loc, world)
elif command_name == "achieved":
goal_achieved = True
#call_go_to('referee', world)
out = "succeeded"
else:
print "ERROR: El commando %s no existe" % (command_name)
command.AddStatusComplete()
print "SM return: %s \n\n" % (out)
if out == "succeeded":
command.AddStatusComplete()
elif out == "aborted":
command.AddStatusError()
else:
print "gpsrSoar interface: unknown ERROR"
exit(1)
i += 1
command.AddStatusComplete()
return 'succeeded'
kernel.DestroyAgent(agent)
kernel.Shutdown()
del kernelCommit
def call_bring_to_loc(
location_name
): #TODO #Improve toSay, add realese and, may be add some human recognition to avoid throwing stuff to the ground
# if location_name == '':
tosay = "I'm leaving this here"
# else:
# tosay = "I took this item here as requested. Referee I know you are here, if no one else is going to pick this proably you will want to take it before I throw it to the floor, thanks"
speak = speaker(tosay)
speak.execute()
rospy.logwarn('call_bring_to_loc ' + location_name)
#############################################################################
if SKILLS:
if (time.time() - TIME_INIT) > 270:
return "succeeded"
#param_place_high_name = "/robocup_params/" + location_name.replace(" ","_") + "_heigh"
param_place_high_name = "/robocup_params/place/" + location_name.replace(
" ", "_")
loc_object_position = PoseStamped() #PoseWithCovarianceStamped()
loc_object_position.header.frame_id = "base_link"
loc_object_position.pose.position.z = 1.25
loc_object_position.pose.position.x = 0.25
loc_object_position.pose.orientation.w = 1.0
current_position = world.get_current_position()
rospy.logwarn(current_position)
rospy.logwarn(ROOMS)
if current_position == "kitchen":
loc_object_position.pose.position.z = rospy.get_param(
"/robocup_params/place/bar")[3]
loc_object_position.pose.position.x = rospy.get_param(
"/robocup_params/place/bar")[2]
call_go_to("bar", world)
m = place_object_sm(loc_object_position)
out = sm.execute()
if current_position == "living room":
loc_object_position.pose.position.z = rospy.get_param(
"/robocup_params/place/dinner_table")[3]
loc_object_position.pose.position.x = rospy.get_param(
"/robocup_params/place/dinner_table")[2]
call_go_to("dinner table", world)
m = place_object_sm(loc_object_position)
out = sm.execute()
if current_position == "hallway":
loc_object_position.pose.position.z = rospy.get_param(
"/robocup_params/place/hallway_table")[3]
loc_object_position.pose.position.x = rospy.get_param(
"/robocup_params/place/hallway_table")[2]
call_go_to("hallway table", world)
m = place_object_sm(loc_object_position)
out = sm.execute()
if current_position == "bedroom":
loc_object_position.pose.position.z = rospy.get_param(
"/robocup_params/place/bed")[3]
loc_object_position.pose.position.x = rospy.get_param(
"/robocup_params/place/bed")[2]
call_go_to("bed", world)
m = place_object_sm(loc_object_position)
out = sm.execute()
if current_position.replace("_", " ") in TABLES:
loc_object_position.pose.position.z = rospy.get_param(
"/robocup_params/place/" +
current_position.replace("_", " "))[3]
loc_object_position.pose.position.x = rospy.get_param(
"/robocup_params/place/" +
current_position.replace("_", " "))[2]
sm = place_object_sm(loc_object_position)
out = sm.execute()
#############################################################################
time.sleep(SLEEP_TIME)
return "succeeded"
def call_find_object(object_name, world): #TODO
tosay = "I'm going to search for " + object_name
speak = speaker(tosay)
speak.execute()
rospy.logwarn('call_find_object ' + object_name)
#############################################################################
if SKILLS:
if (time.time() - TIME_INIT) > 270:
return "succeeded"
out = 'aborted'
tries = 0
world.item.object_pose = PoseWithCovarianceStamped()
current_position = world.get_current_position()
rospy.logwarn(current_position)
rospy.logwarn(ROOMS)
rospy.loginfo('-----------------------------------------')
if current_position in ROOMS:
rospy.loginfo('-----------------------------------------')
room = rospy.get_param('/robocup_params/room/' + current_position)
for table in room:
rospy.loginfo('-----------------------------------------')
if world.item.object_pose.header.frame_id != '':
rospy.logwarn(
"NO ESTEM BUSCAN PERK YA HEM TROBAT EL QUE VOLIEM")
break
call_go_to(table.replace(" ", "_"), world)
tries = 0
while (world.item.object_pose.header.frame_id == ''
and tries < 3):
sm = recognize_object()
sm.userdata.object_name = object_name[0].upper(
) + object_name[1:]
out = sm.execute() #
names = sm.userdata.object_detected_name
poses = sm.userdata.object_position
rospy.loginfo('-----------------------------------------')
rospy.loginfo(names)
rospy.loginfo('-----------------------------------------')
rospy.loginfo(poses)
rospy.loginfo('-----------------------------------------')
tries = tries + 1
i = 0
while i < len(names):
if names[i] == (object_name[0].upper() +
object_name[1:]):
world.item.object_pose = poses[i]
i = i + 1
if current_position.replace("_", " ") in TABLES:
while (world.item.object_pose.header.frame_id == '' and tries < 3):
sm = recognize_object()
sm.userdata.object_name = object_name[0].upper(
) + object_name[1:]
out = sm.execute()
names = sm.userdata.object_detected_name
poses = sm.userdata.object_position
rospy.loginfo('-----------------------------------------')
rospy.loginfo(names)
rospy.loginfo('-----------------------------------------')
rospy.loginfo(poses)
rospy.loginfo('-----------------------------------------')
tries = tries + 1
i = 0
while i < len(names):
if names[i] == (object_name[0].upper() + object_name[1:]):
world.item.object_pose = poses[i]
i = i + 1
rospy.logwarn(world.item.object_pose)
if out == 'aborted':
tosay = "I couldn't find the " + object_name + " you asked for. It isn't here. I'm going to the referee to inform"
speak = speaker(tosay)
speak.execute()
rospy.logwarn('FAIL IN FINDING ' + object_name)
time.sleep(SLEEP_TIME)
call_go_to('referee', world)
tosay = "I couldn't find the " + object_name + " you asked for. It isn't there. This sentence is from category 3"
speak = speaker(tosay)
speak.execute()
return "aborted"
#############################################################################
time.sleep(SLEEP_TIME)
return "succeeded"
def main(world):
print "******************************\n******************************\nNew goal\n******************************\n******************************\n"
first_time = time.time()
kernel = create_kernel()
agent = create_agent(kernel, "agent")
agent_load_productions(agent,SOAR_GP_PATH)
agent.SpawnDebugger()
# p_cmd = 'learn --on'
# res = agent.ExecuteCommandLine(p_cmd)
# res = kernel.ExecuteCommandLine(p_cmd, agent.GetAgentName)
kernel.CheckForIncomingCommands()
p_cmd = 'watch --learning 2'
res = agent.ExecuteCommandLine(p_cmd)
print str(res)
goal_achieved = False
while not goal_achieved:
agent.Commit()
agent.RunSelfTilOutput()
agent.Commands()
numberCommands = agent.GetNumberCommands()
print "Numero de comandos recibidos del agente: %s" % (numberCommands)
i=0
if numberCommands == 0:
print 'KABOOOOOOOOOOOOOOOOOOM!!!!!!!!!!!!!!!'
return 'aborted'
else:
while i<numberCommands:
command = agent.GetCommand(i)
command_name = command.GetCommandName()
print "El nombre del commando %d/%d es %s" % (i+1,numberCommands,command_name)
rospy.logwarn(world.get_current_position())
rospy.logwarn(str(time.time()) + ' ' + str(TIME_INIT))
if (time.time()-TIME_INIT) > 270:
tosay = "time out."
speak = speaker(tosay)
speak.execute()
call_go_to('referee',world)
return "succeeded"
out = "NULL"
if command_name == "navigate":
loc_to_navigate = command.GetParameterValue("loc")
loc = idx2obj(int(loc_to_navigate), 'LOCATIONS')
print loc
if (loc =="NULL"):
print "ERROR: la loacalizacion %s no existe" % (loc_to_navigate)
out = call_go_to(loc,world)
elif command_name == "grasp":
obj_to_grasp = command.GetParameterValue("obj")
obj = idx2obj(int(obj_to_grasp),'ITEMS')
print obj
if (obj =="NULL"):
print "ERROR: el objeto %s no existe" % (obj_to_grasp)
out = call_grasp(obj,world)
elif command_name == "deliver": #to Person
try:
to_pers = command.GetParameterValue("pers")
pers = idx2obj(int(to_pers),'PERSONS')
print pers
if (pers =="NULL"):
print "ERROR: la persona %s no existe" % (to_pers)
out = call_bring_to(pers)
except: #or to Loc
to_loc = command.GetParameterValue("loc")
try:
loc = idx2obj(int(to_loc),'LOCATIONS')
print loc
if (loc =="NULL"):
print "ERROR: l'objecte %s no existe" % (to_loc)
out = call_bring_to_loc(loc)
except:
loc = ''
print loc
out = call_bring_to_loc(loc)
elif command_name == "search-object":
obj_to_search = command.GetParameterValue("obj")
obj = idx2obj(int(obj_to_search), 'ITEMS')
print obj
if (obj =="NULL"):
print "ERROR: el objeto %s no existe" % (obj_to_search)
out = call_find_object(obj,world)
elif command_name == "search-person":
pers_to_search = command.GetParameterValue("pers")
pers = idx2obj(int(pers_to_search), 'PERSONS')
print pers
if (pers =="NULL"):
print "ERROR: la persona %s no existe" % (pers_to_search)
out = call_find_person(pers)
elif command_name == "point-obj":
to_loc = command.GetParameterValue("loc")
loc = idx2obj(int(to_loc),'LOCATIONS')
out = call_point_at(loc)
elif command_name == "ask-name":
out = call_ask_name()
elif command_name == "follow":
to_pers = command.GetParameterValue("pers")
pers = idx2obj(int(to_pers),'PERSONS')
out = call_follow(pers)
elif command_name == "introduce-me":
out = call_introduce_me()
elif command_name == "memorize-person":
to_pers = command.GetParameterValue("pers")
pers = idx2obj(int(to_pers),'PERSONS')
out = call_learn_person(pers)
elif command_name == "recognize-person":
to_pers = command.GetParameterValue("pers")
pers = idx2obj(int(to_pers),'PERSONS')
out = call_recognize_person(pers)
elif command_name == "guide":
loc_to_navigate = command.GetParameterValue("loc")
loc = idx2obj(int(loc_to_navigate), 'LOCATIONS')
print loc
if (loc =="NULL"):
print "ERROR: la loacalizacion %s no existe" % (loc_to_navigate)
out = call_guide_to(loc,world)
elif command_name == "achieved":
goal_achieved = True
#call_go_to('referee', world)
out = "succeeded"
else:
print "ERROR: El commando %s no existe" % (command_name)
command.AddStatusComplete()
print "SM return: %s \n\n" % (out)
if out=="succeeded":
command.AddStatusComplete()
elif out=="aborted":
command.AddStatusError()
else:
print "gpsrSoar interface: unknown ERROR"
exit(1)
i+=1
command.AddStatusComplete()
return 'succeeded'
kernel.DestroyAgent(agent)
kernel.Shutdown()
del kernelCommit
def call_bring_to_loc(location_name): #TODO #Improve toSay, add realese and, may be add some human recognition to avoid throwing stuff to the ground
# if location_name == '':
tosay = "I'm leaving this here"
# else:
# tosay = "I took this item here as requested. Referee I know you are here, if no one else is going to pick this proably you will want to take it before I throw it to the floor, thanks"
speak = speaker(tosay)
speak.execute()
rospy.logwarn('call_bring_to_loc '+location_name)
#############################################################################
if SKILLS :
if (time.time()-TIME_INIT) > 270:
return "succeeded"
#param_place_high_name = "/robocup_params/" + location_name.replace(" ","_") + "_heigh"
param_place_high_name = "/robocup_params/place/" + location_name.replace(" ","_")
loc_object_position = PoseStamped() #PoseWithCovarianceStamped()
loc_object_position.header.frame_id = "base_link"
loc_object_position.pose.position.z = 1.25
loc_object_position.pose.position.x = 0.25
loc_object_position.pose.orientation.w = 1.0
current_position = world.get_current_position()
rospy.logwarn(current_position)
rospy.logwarn(ROOMS)
if current_position == "kitchen":
loc_object_position.pose.position.z = rospy.get_param("/robocup_params/place/bar")[3]
loc_object_position.pose.position.x = rospy.get_param("/robocup_params/place/bar")[2]
call_go_to("bar",world)
m = place_object_sm(loc_object_position)
out = sm.execute()
if current_position == "living room":
loc_object_position.pose.position.z = rospy.get_param("/robocup_params/place/dinner_table")[3]
loc_object_position.pose.position.x = rospy.get_param("/robocup_params/place/dinner_table")[2]
call_go_to("dinner table",world)
m = place_object_sm(loc_object_position)
out = sm.execute()
if current_position == "hallway":
loc_object_position.pose.position.z = rospy.get_param("/robocup_params/place/hallway_table")[3]
loc_object_position.pose.position.x = rospy.get_param("/robocup_params/place/hallway_table")[2]
call_go_to("hallway table",world)
m = place_object_sm(loc_object_position)
out = sm.execute()
if current_position == "bedroom":
loc_object_position.pose.position.z = rospy.get_param("/robocup_params/place/bed")[3]
loc_object_position.pose.position.x = rospy.get_param("/robocup_params/place/bed")[2]
call_go_to("bed",world)
m = place_object_sm(loc_object_position)
out = sm.execute()
if current_position.replace("_"," ") in TABLES:
loc_object_position.pose.position.z = rospy.get_param("/robocup_params/place/"+current_position.replace("_"," "))[3]
loc_object_position.pose.position.x = rospy.get_param("/robocup_params/place/"+current_position.replace("_"," "))[2]
sm = place_object_sm(loc_object_position)
out = sm.execute()
#############################################################################
time.sleep(SLEEP_TIME)
return "succeeded"
def call_find_object(object_name,world): #TODO
tosay = "I'm going to search for " + object_name
speak = speaker(tosay)
speak.execute()
rospy.logwarn('call_find_object '+object_name)
#############################################################################
if SKILLS :
if (time.time()-TIME_INIT) > 270:
return "succeeded"
out = 'aborted'
tries = 0
world.item.object_pose = PoseWithCovarianceStamped()
current_position = world.get_current_position()
rospy.logwarn(current_position)
rospy.logwarn(ROOMS)
rospy.loginfo('-----------------------------------------')
if current_position in ROOMS:
rospy.loginfo('-----------------------------------------')
room = rospy.get_param('/robocup_params/room/' + current_position)
for table in room :
rospy.loginfo('-----------------------------------------')
if world.item.object_pose.header.frame_id!='':
rospy.logwarn("NO ESTEM BUSCAN PERK YA HEM TROBAT EL QUE VOLIEM")
break
call_go_to(table.replace(" ","_"),world)
tries = 0
while(world.item.object_pose.header.frame_id=='' and tries<3):
sm = recognize_object()
sm.userdata.object_name = object_name[0].upper() + object_name[1:]
out = sm.execute()#
names = sm.userdata.object_detected_name
poses = sm.userdata.object_position
rospy.loginfo('-----------------------------------------')
rospy.loginfo(names)
rospy.loginfo('-----------------------------------------')
rospy.loginfo(poses)
rospy.loginfo('-----------------------------------------')
tries = tries+1
i = 0
while i<len(names):
if names[i] == (object_name[0].upper() + object_name[1:]):
world.item.object_pose = poses[i]
i = i +1
if current_position.replace("_"," ") in TABLES:
while(world.item.object_pose.header.frame_id=='' and tries<3):
sm = recognize_object()
sm.userdata.object_name = object_name[0].upper() + object_name[1:]
out = sm.execute()
names = sm.userdata.object_detected_name
poses = sm.userdata.object_position
rospy.loginfo('-----------------------------------------')
rospy.loginfo(names)
rospy.loginfo('-----------------------------------------')
rospy.loginfo(poses)
rospy.loginfo('-----------------------------------------')
tries = tries+1
i = 0
while i<len(names):
if names[i] == (object_name[0].upper() + object_name[1:]):
world.item.object_pose = poses[i]
i = i +1
rospy.logwarn(world.item.object_pose)
if out=='aborted':
tosay = "I couldn't find the " + object_name + " you asked for. It isn't here. I'm going to the referee to inform"
speak = speaker(tosay)
speak.execute()
rospy.logwarn('FAIL IN FINDING ' + object_name)
time.sleep(SLEEP_TIME)
call_go_to('referee',world)
tosay = "I couldn't find the " + object_name + " you asked for. It isn't there. This sentence is from category 3"
speak = speaker(tosay)
speak.execute()
return "aborted"
#############################################################################
time.sleep(SLEEP_TIME)
return "succeeded"
