def ask_data(Type='LOCATIONS', objectName='cola'):
if TEST:
return idx2obj(1, Type)
ad = askMissingInfoSM(Type=Type, objectName=objectName)
out = ad.execute()
loc = ad.userdata._data['location_name']
print "EL loc ES AKET!!!!!!!!!!!!!!!!!!!!!!!!!!!ask_data!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!" + loc
return loc
def ask_data(Type='LOCATIONS', objectName='cola'):
if TEST:
return idx2obj(1,Type)
ad = askMissingInfoSM(Type=Type, objectName=objectName)
out = ad.execute()
loc = ad.userdata._data['location_name']
print "EL loc ES AKET!!!!!!!!!!!!!!!!!!!!!!!!!!!ask_data!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"+loc
return loc
def ask_category_loc(category):
if TEST:
return idx2obj(1, rospy.get_param('/robocup_params/locations'))
ad = askCategoryLocSM(GRAMMAR_NAME=category)
out = ad.execute()
obj = ad.userdata._data['location_name']
#ob = obj2idx(ad.userdata.object_name, 'ITEMS')
print "EL OBJ ES AKET!!!!!!!!!!!!!!!!!!!!!!!!!!!ask_category_loc!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!" + obj
return obj #---------------------'''
def ask_category_loc(category):
if TEST:
return idx2obj(1,rospy.get_param('/robocup_params/locations'))
ad = askCategoryLocSM(GRAMMAR_NAME = category)
out = ad.execute()
obj = ad.userdata._data['location_name']
#ob = obj2idx(ad.userdata.object_name, 'ITEMS')
print "EL OBJ ES AKET!!!!!!!!!!!!!!!!!!!!!!!!!!!ask_category_loc!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"+obj
return obj #---------------------'''
def ask_category(category):
if TEST:
return idx2obj(1,rospy.get_param('/robocup_params/it_category/'+category.replace("/gpsr","")))
print "category: " + category
ad = askCategorySM(GRAMMAR_NAME = category)
out = ad.execute()
obj = ad.userdata._data['object_name']
print "EL OBJ ES AKET!!!!!!!!!!!!!!!!!!!!!!!!!!!ask_category!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"+obj
return obj #--------------'''
def ask_category(category):
if TEST:
return idx2obj(
1,
rospy.get_param('/robocup_params/it_category/' +
category.replace("/gpsr", "")))
print "category: " + category
ad = askCategorySM(GRAMMAR_NAME=category)
out = ad.execute()
obj = ad.userdata._data['object_name']
print "EL OBJ ES AKET!!!!!!!!!!!!!!!!!!!!!!!!!!!ask_category!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!" + obj
return obj #--------------'''
def print_goal(self):
commands = self._goal.orderList
iperson = 0
iitem = 0
iloc = 0
perss = []
locc = []
itt = []
for c in commands:
if c.person != '':
if c.person not in perss:
iperson += 1
perss.append(c.person)
if c.item != '':
if c.item not in itt:
iitem += 1
itt.append(c.item)
if c.location != '':
if c.location not in locc:
iloc += 1
locc.append(c.location)
command = commands[self._goalDonei]
locc = get_list('LOCATIONS')
#this is a dummy for tests!!!
# command = commands[1]
# print command.action
# print command.location
# print command.item
# print command.person
# printNewGoal(oaction=command.action, oitem=items[command.item], operson=persons[command.person], olocation=locations[command.location])
# printNewGoal(oaction='go_to', oitem=items[command.item], operson=persons[command.person], olocation=1)
# compileInit(locations=iloc, persons=iperson, items=iitem, oaction=command.action, oitem=items[command.item], operson=persons[command.person], olocation=locations[command.location])
# checks that the item is or not a category
# print 'blablabal'
if command.item in categories:
objct = ask_category(command.item)
self._world.item.id = obj2idx(objct, 'ITEMS')
command.item = objct
print idx2obj(self._world.item.id, 'ITEMS')
# print command.location
# print 'here'
if command.location in loc_categories:
# print 'ther should be a print here'
loca = ask_category_loc(command.location)
# print loca
# print loca
# print self._world.location
# print len(self._world.location)
# self._world.location.id = obj2idx(loca, 'LOCATIONS')
command.location = loca
# print loca
# checks if the location of objects or persons are known
if self._world.person.locId == '-1' : # and command.person == self._world.person.id:
ploc = check_object_location(idx2obj(int(self._world.person.id), 'PERSONS'))
# if ploc != '-1':
# ploc = obj2idx(ploc,'LOCATIONS')
self._world.person.locId = ploc
# print command.item
try:
i = obj2idx(command.item, 'ITEMS')
self._world.item.id = str(i)
except ValueError:
print str(self._world.item.id)
print command.item
i = ''
try:
p = obj2idx(command.person, 'PERSONS')
self._world.person.id = str(p)
except ValueError:
p = ''
try:
l = obj2idx(command.location, 'LOCATIONS')
# print l
# print l
except ValueError:
l = ''
print self._world.robot.locId
# print command.location
# print idx2obj(l, 'LOCATIONS')
# print 'blabla'
# print 'here here ' + str(p)
if self._world.item.locId == '-1' and self._world.item.id != '-1': # and command.item == self._world.item.id:
iloc = check_object_location(idx2obj(int(self._world.item.id), 'ITEMS'))
# if iloc != '-1':
# iloc = obj2idx(iloc,'LOCATIONS')
self._world.item.locId = iloc
if command.action == 'bring_from':
self._world.item.locId = str(obj2idx(command.location, 'LOCATIONS'))
if (command.action == 'memorize' or command.action == 'recognize'):
command.person = self._world.person.id
# print self._world.item.id
# print self._world.item.locId
self._last_goal = compileInit(locations=locc, persons=perss, items=itt, oaction=command.action, oitem=i, operson=p, olocation=l, current_world=self._world)
# compileInit(locations=locc, persons=perss, items=itt, oaction=command.action, oitem=items[command.item], operson=persons[command.person], olocation=locations[command.location])
# printNewGoal(action, it, pers, loc, self._goalDonei)
self._goalDonei += 1
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 print_goal(self):
commands = self._goal.orderList
iperson = 0
iitem = 0
iloc = 0
perss = []
locc = []
itt = []
for c in commands: #extract elements from orderList and put them in the arrays previously initialized
if c.person != '':
if c.person not in perss:
iperson += 1
perss.append(c.person)
if c.item != '':
if c.item not in itt:
iitem += 1
itt.append(c.item)
if c.location != '':
if c.location not in locc:
iloc += 1
locc.append(c.location)
command = commands[
self._goalDonei] #Extract command indexed by goalDonei to command
locc = get_list('LOCATIONS') #Saves in locc all possible locations
#this is a dummy for tests!!!
# command = commands[1]
# print command.action
# print command.location
# print command.item
# print command.person
# printNewGoal(oaction=command.action, oitem=items[command.item], operson=persons[command.person], olocation=locations[command.location])
# printNewGoal(oaction='go_to', oitem=items[command.item], operson=persons[command.person], olocation=1)
# compileInit(locations=iloc, persons=iperson, items=iitem, oaction=command.action, oitem=items[command.item], operson=persons[command.person], olocation=locations[command.location])
# checks that the item is or not a category
# print 'blablabal'
if command.item in categories: #if the item is a category this ask for further information and updates it
objct = ask_category(command.item)
self._world.item.id = obj2idx(objct, 'ITEMS')
command.item = objct
if command.location in loc_categories: #if the location is a category this ask for further information and updates it
loca = ask_category_loc(command.location)
command.location = loca
# checks if the location of persons are known
if self._world.person.locId == '-1': # and command.person == self._world.person.id:
ploc = check_object_location(
idx2obj(int(self._world.person.id), 'PERSONS'))
# if ploc != '-1':
# ploc = obj2idx(ploc,'LOCATIONS')
print "LA LOCATION DE LA PERSON ES!!!!!!!!!!!!!!!!: " + str(ploc)
self._world.person.locId = ploc
try: #adds to world the item id
i = obj2idx(command.item, 'ITEMS')
self._world.item.id = str(i)
except ValueError:
print str(self._world.item.id)
print command.item
i = ''
try: #adds to world the person id
p = obj2idx(command.person, 'PERSONS')
self._world.person.id = str(p)
except ValueError:
p = ''
try:
l = obj2idx(command.location, 'LOCATIONS')
except ValueError:
l = ''
print self._world.robot.locId
if command.action == 'bring_from':
self._world.item.locId = str(obj2idx(command.location,
'LOCATIONS'))
if self._world.item.locId == '-1' and self._world.item.id != '-1': # If we know the item but not the location we check it and save it in world
iloc = check_object_location(
idx2obj(int(self._world.item.id), 'ITEMS'))
self._world.item.locId = iloc
if (command.action == 'memorize' or command.action == 'recognize'):
command.person = self._world.person.id
if (idx2obj(int(self._world.person.id),
'PERSONS') == 'person'): #prova per coneguir
self._world.person.locId = self._world.robot.locId
self._last_goal = compileInit(locations=locc,
persons=perss,
items=itt,
oaction=command.action,
oitem=i,
operson=p,
olocation=l,
current_world=self._world)
'''
locc: array with all the locations
perss: array with all the persons involved in the commands
itt: array with all the items involved in the commands
comand.action: the action we want to perform
i: the index of the object involved in this action
p: index of the person involved in this action
l: index of the location involved in this action
self._world: the world we are working on
'''
self._goalDonei += 1
def get_current_position(self):
rospy.logwarn(self.robot.locId)
#rospy.logwarn('typeof locId: ' + str(type(self.robot.locId)))
loc = idx2obj(int(self.robot.locId),'LOCATIONS')
return loc
def get_current_position(self):
loc = idx2obj(self.robot.locId,'LOCATIONS')
return loc
def print_goal(self):
commands = self._goal.orderList
iperson = 0
iitem = 0
iloc = 0
perss = []
locc = []
itt = []
for c in commands: #extract elements from orderList and put them in the arrays previously initialized
if c.person != '':
if c.person not in perss:
iperson += 1
perss.append(c.person)
if c.item != '':
if c.item not in itt:
iitem += 1
itt.append(c.item)
if c.location != '':
if c.location not in locc:
iloc += 1
locc.append(c.location)
command = commands[self._goalDonei] #Extract command indexed by goalDonei to command
locc = get_list('LOCATIONS') #Saves in locc all possible locations
#this is a dummy for tests!!!
# command = commands[1]
# print command.action
# print command.location
# print command.item
# print command.person
# printNewGoal(oaction=command.action, oitem=items[command.item], operson=persons[command.person], olocation=locations[command.location])
# printNewGoal(oaction='go_to', oitem=items[command.item], operson=persons[command.person], olocation=1)
# compileInit(locations=iloc, persons=iperson, items=iitem, oaction=command.action, oitem=items[command.item], operson=persons[command.person], olocation=locations[command.location])
# checks that the item is or not a category
# print 'blablabal'
if command.item in categories: #if the item is a category this ask for further information and updates it
objct = ask_category(command.item)
self._world.item.id = obj2idx(objct, 'ITEMS')
command.item = objct
if command.location in loc_categories: #if the location is a category this ask for further information and updates it
loca = ask_category_loc(command.location)
command.location = loca
# checks if the location of persons are known
if self._world.person.locId == '-1' : # and command.person == self._world.person.id:
ploc = check_object_location(idx2obj(int(self._world.person.id), 'PERSONS'))
# if ploc != '-1':
# ploc = obj2idx(ploc,'LOCATIONS')
print "LA LOCATION DE LA PERSON ES!!!!!!!!!!!!!!!!: "+str(ploc)
self._world.person.locId = ploc
try: #adds to world the item id
i = obj2idx(command.item, 'ITEMS')
self._world.item.id = str(i)
except ValueError:
print str(self._world.item.id)
print command.item
i = ''
try: #adds to world the person id
p = obj2idx(command.person, 'PERSONS')
self._world.person.id = str(p)
except ValueError:
p = ''
try:
l = obj2idx(command.location, 'LOCATIONS')
except ValueError:
l = ''
print self._world.robot.locId
if command.action == 'bring_from':
self._world.item.locId = str(obj2idx(command.location, 'LOCATIONS'))
if self._world.item.locId == '-1' and self._world.item.id != '-1': # If we know the item but not the location we check it and save it in world
iloc = check_object_location(idx2obj(int(self._world.item.id), 'ITEMS'))
self._world.item.locId = iloc
if (command.action == 'memorize' or command.action == 'recognize'):
command.person = self._world.person.id
if (idx2obj(int(self._world.person.id), 'PERSONS') == 'person'): #prova per coneguir
self._world.person.locId = self._world.robot.locId
self._last_goal = compileInit(locations=locc, persons=perss, items=itt, oaction=command.action,
oitem=i, operson=p, olocation=l, current_world=self._world)
'''
locc: array with all the locations
perss: array with all the persons involved in the commands
itt: array with all the items involved in the commands
comand.action: the action we want to perform
i: the index of the object involved in this action
p: index of the person involved in this action
l: index of the location involved in this action
self._world: the world we are working on
'''
self._goalDonei += 1
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)
out = "NULL"
if command_name == "navigate":
loc_to_navigate = command.GetParameterValue("loc")
# loc = trad_loc(loc_to_navigate)
# print str(loc_to_navigate)
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 = trad_obj(obj_to_grasp)
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)
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)
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
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()
else:
print "gpsrSoar interface: unknown ERROR"
exit(1)
i+=1
command.AddStatusComplete()
return 'succeeded'
kernel.DestroyAgent(agent)
kernel.Shutdown()
del kernelCommit
def get_current_position(self):
rospy.logwarn(self.robot.locId)
#rospy.logwarn('typeof locId: ' + str(type(self.robot.locId)))
loc = idx2obj(int(self.robot.locId), 'LOCATIONS')
return loc
