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"