def execute_cb(self, goal):
self._goalDonei = 0 #contador de goals
self._goal = goal #saving our goal in self for more comfortable usage
success = False #flag to know when we have succeed
self._world = new_world(range(len(get_list('LOCATIONS')))) #generates a new world
print self._world.robot.locId
self.print_goal()
#"-----------------------------------------------------------orders list------------------------------"
rospy.logwarn (str(self._goal.orderList))
while not success:
soarResult = interface.main(self._world)
if soarResult == 'aborted':
self._result.outcome = 'aborted'
self._as.set_succeeded(self._result)
success = True
if TEST:
rospy.logerr("THE SENTENCE IS: "+str(self._goal.orderList)+"\n")
self.targfile.write(str(self._goal.orderList).replace('\n','\t').replace(',','\n')+"\n\n")
else:
if len(self._goal.orderList) == self._goalDonei:
self._result.outcome = 'succeeded'
rospy.loginfo('%s: Succeeded' % self._action_name)
self._as.set_succeeded(self._result)
success = True
else:
self._world.update_world(self._last_goal)
self.print_goal()
"""
def execute_cb(self, goal):
self._goalDonei = 0
self._goal = goal
success = False
self._world = new_world(range(len(get_list('LOCATIONS'))))
print self._world.robot.locId
self.print_goal()
while not success:
soarResult = interface.main()
if soarResult == 'aborted':
self._result.outcome = 'aborted'
self._as.set_succeeded(self._result)
success = True
else:
if len(self._goal.orderList) == self._goalDonei:
# if 3 == self._goalDonei:
self._result.outcome = 'succeeded'
rospy.loginfo('%s: Succeeded' % self._action_name)
self._as.set_succeeded(self._result)
success = True
else:
self._world.update_world(self._last_goal)
self.print_goal()
"""
def grammarFileWriter2(
wordset,
path='/mnt_flash/etc/interaction/sphinx/model/gram/en_US/' + GRAMATICA +
'.gram'):
# print 'aqui entra?'
# if path == '':
# path = roslib.packages.get_pkg_dir('gpsrSoar') + '/src/gentest.gram'
# # should be changed by a try-except statement
# ff = open(path, 'r')
# # l = ff.readline()
# # finished = False
# text = ff.read()
# ff.close()
print wordset.keys()
# n = len(wordset)
found = True
# fcheck = False
wordlist = {}
for item in wordset.keys():
# if not found:
# found = True
# fcheck = True
# it = '<' + wordset[item] + '> ='
# st = string.find(text, it)
# if st >= 0:
List = get_list((wordset[item] + 's').upper())
# to_replace = text[st:]
# to_replace = to_replace[:string.find(to_replace, '\n')]
# new_data = it + ' ( ' + ' | '.join(List) + ' );'
# text = text.replace(to_replace, new_data)
wordlist[item] = List
# print 'found found'
# else:
# found = False
# print 'Word Set not found. \n Please revise that '
# print '\n'.join(wordset[item])
# print 'are included in the grammar with this exact names. \nIf not, please change them'
print found
if not found:
print 'Word Set not found. \n Please revise that '
print '\n'.join(wordset.keys())
print 'are included in the grammar with this exact names. \nIf not, please change them'
return ''
else:
# f = open(path,'w')
# f.write(text)
# f.close()
return wordlist
return 'ok'
def grammarFileWriter2(wordset, path='/mnt_flash/etc/interaction/sphinx/model/gram/en_US/' + GRAMATICA + '.gram'):
# print 'aqui entra?'
# if path == '':
# path = roslib.packages.get_pkg_dir('gpsrSoar') + '/src/gentest.gram'
# # should be changed by a try-except statement
# ff = open(path, 'r')
# # l = ff.readline()
# # finished = False
# text = ff.read()
# ff.close()
print wordset.keys()
# n = len(wordset)
found = True
# fcheck = False
wordlist = {}
for item in wordset.keys():
# if not found:
# found = True
# fcheck = True
# it = '<' + wordset[item] + '> ='
# st = string.find(text, it)
# if st >= 0:
List = get_list((wordset[item] + 's').upper())
# to_replace = text[st:]
# to_replace = to_replace[:string.find(to_replace, '\n')]
# new_data = it + ' ( ' + ' | '.join(List) + ' );'
# text = text.replace(to_replace, new_data)
wordlist[item] = List
# print 'found found'
# else:
# found = False
# print 'Word Set not found. \n Please revise that '
# print '\n'.join(wordset[item])
# print 'are included in the grammar with this exact names. \nIf not, please change them'
print found
if not found:
print 'Word Set not found. \n Please revise that '
print '\n'.join(wordset.keys())
print 'are included in the grammar with this exact names. \nIf not, please change them'
return ''
else:
# f = open(path,'w')
# f.write(text)
# f.close()
return wordlist
return 'ok'
def execute_cb(self, goal):
self._goalDonei = 0 #contador de goals
self._goal = goal #saving our goal in self for more comfortable usage
success = False #flag to know when we have succeed
self._world = new_world(range(len(
get_list('LOCATIONS')))) #generates a new world
print self._world.robot.locId
self.print_goal()
self._world.set_current_position('referee')
#"-----------------------------------------------------------orders list------------------------------"
rospy.logwarn(str(self._goal.orderList))
while not success:
soarResult = interface.main(self._world)
self.gpsr_category_flag_hack = 1
rospy.logwarn(self.gpsr_category_flag_hack)
rospy.logwarn(
'BBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBB'
)
if soarResult == 'aborted':
self._result.outcome = 'aborted'
self._as.set_succeeded(self._result)
success = True
if TEST:
rospy.logerr("THE SENTENCE IS: " +
str(self._goal.orderList) + "\n")
self.targfile.write(
str(self._goal.orderList).replace('\n', '\t').replace(
',', '\n') + "\n\n")
else:
if len(self._goal.orderList) == self._goalDonei:
self._result.outcome = 'succeeded'
rospy.loginfo('%s: Succeeded' % self._action_name)
self._as.set_succeeded(self._result)
success = True
else:
self._world.update_world(self._last_goal)
self.print_goal()
self.gpsr_category_flag_hack = 0
rospy.logwarn("FINISHING SENTENCE")
call_go_to('referee', self._world)
"""
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 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 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
