def updatingDSR(self):
try:
newModel = AGMModelConversion.fromInternalToIce(self.worldModel)
self.agmexecutive_proxy.structuralChangeProposal(newModel, "component_name", "Log_fileName")
w = self.agmexecutive_proxy.getModel()
self.worldModel = AGMModelConversion.fromIceToInternal_model(w)
print("AGM successfully updated")
except:
print("Exception moving in AGM")
def compute(self):
print('SpecificWorker.compute...')
id_robot = '1' #Leer del DSR ?
(people_near, people_id) = self.searchPeopleNearRobot()
print('prev ', self.prev_person_near)
print('curr ', people_id)
self.newModel = AGMGraph()
self.newModel = AGMModelConversion.fromIceToInternal_model(
self.agmexecutive_proxy.getModel())
edges_changed = False
if self.prev_person_near != people_id or self.faceStateChanged:
for id_to_remove in self.prev_person_near:
if id_to_remove in people_id and self.faceDetected:
continue
if self.worldModel.getEdge(id_to_remove, id_robot,
'front') is not None:
print('The edge exists -- edgesChanged = True')
self.newModel.removeEdge(id_to_remove, id_robot, 'front')
edges_changed = True
else:
print(id_to_remove, 'the edge doesnt exists')
for id_to_add in people_id:
if self.faceDetected:
print('Face Detected', id_to_add)
if self.worldModel.getEdge(id_to_add, id_robot,
'front') is None:
print('The edge doesnt exist --- edgesChanged = True')
self.newModel.addEdge(id_to_add, id_robot, 'front')
edges_changed = True
else:
print(id_to_add, 'the edge already exists')
self.prev_person_near = people_id
self.faceStateChanged = False
if edges_changed:
try:
print('Updating world')
newModel_ice = AGMModelConversion.fromInternalToIce(
self.newModel)
self.agmexecutive_proxy.structuralChangeProposal(
newModel_ice, 'faceDetector', '')
except Exception as e:
print('Exception updating AGM -> ', e)
return True
def commitAction(self):
try:
self.mutex.lock()
try:
plan = self.plan
except:
self.ui.currentLabel.setText('no plan yet')
print 'No plan yet'
return
try:
model = self.internalModel
except:
print 'No model yet'
return
try:
action = plan.data[0]
except IndexError:
print 'No action to execute'
return
print 'Action: ', action.name
self.ui.currentLabel.setText(action.name)
print 'Paramters:', action.parameters
result = self.triggers[action.name](WorldStateHistory(
[model, self.triggers.keys()]), action.parameters).graph
print 'type(result)', type(result)
resultIce = AGMModelConversion.fromInternalToIce(result)
self.agmexecutive_proxy.structuralChangeProposal(
resultIce, 'agmagentsim', action.name)
finally:
self.mutex.unlock()
def compute(self):
try:
plan = self.plan
except:
print 'No plan yet'
self.ui.fullPlanText.setText('')
self.ui.currentLabel.setText('')
self.requestPlan()
return
try:
self.mutex.lock()
model = self.internalModel
except:
print 'No model yet'
w = self.agmexecutive_proxy.getModel()
self.internalModel = AGMModelConversion.fromIceToInternal_model(
w, ignoreInvalidEdges=True)
return
finally:
self.mutex.unlock()
self.ui.fullPlanText.setText(self.planText)
if len(self.plan.data) > 0:
self.ui.currentLabel.setText(self.plan.data[0].name)
return True
def structuralChangeProposal(self, worldModelICE, sender, log):
#
# H E R E W E S H O U L D C H E C K T H E M O D I F I C A T I O N I S V A L I D
#
print 'structuralChangeProposal', sender, log
# Get structuralChange mutex
print 'structuralChangeProposal acquire() a'
for iixi in xrange(5):
gotMutex = self.mutex.acquire(blocking=False)
if gotMutex == True:
break
else:
if iixi == 4:
print 'structuralChangeProposal acquire() IT WAS LOCKED'
raise RoboCompAGMExecutive.Locked()
else:
time.sleep(0.03)
print 'CURRENT VERSION', self.lastModification.version
try:
print 'structuralChangeProposal acquire() z'
# Ignore outdated modifications
if worldModelICE.version != self.lastModification.version:
print 'outdated!??! self='+str(self.lastModification.version)+' ice='+str(worldModelICE.version)
raise RoboCompAGMExecutive.OldModel()
print 'inside'
# Here we're OK with the modification, accept it, but first check if replanning is necessary
worldModelICE.version += 1
internalModel = AGMModelConversion.fromIceToInternal_model(worldModelICE, ignoreInvalidEdges=True) # set internal model
try: # is replanning necessary?
avoidReplanning = False
if internalModel.equivalent(self.lastModification):
avoidReplanning = True
except AttributeError:
pass
self.lastModification = internalModel # store last modification
self.modifications += 1
self.worldModelICE = worldModelICE
# Store the model in XML
#try:
#internalModel.toXML('modification'+str(self.modifications).zfill(4)+'_'+sender+'.xml')
#except:
#print 'There was some problem updating internal model to xml'
# Set and broadst the model
try:
self.setAndBroadcastModel(internalModel)
except:
print traceback.print_exc()
print 'There was some problem broadcasting the model'
sys.exit(-1)
# Force replanning
try:
if not (avoidReplanning or self.doNotPlan):
self.updatePlan()
except:
print traceback.print_exc()
print 'There was some problem updating internal model to xml'
finally:
self.mutex.release()
return
def toDSR(self):
print("todsr")
personID = str(self.ui.id_list.currentText())
print(personID)
print(type(personID))
tempNode = self.worldModel.getNode(personID)
# adding person information into attributes
tempNode.attributes["name"] = str(self.ui.H_name.text())
tempNode.attributes["imGender"] = str(self.ui.H_gender.currentText())
tempNode.attributes["imAge"] = str(self.ui.H_age.currentText())
tempNode.attributes["userType"] = str(self.ui.H_userType.currentText())
tempNode.attributes["PhysicalDep"] = str(self.ui.H_phyDep.value())
tempNode.attributes["CognitiveDep"] = str(self.ui.H_cogDep.value())
tempNode.attributes["emotionalState"] = str(
self.ui.H_emoSate.currentText())
tempNode.attributes["activity"] = str(self.ui.H_activity.currentText())
tempNode.attributes["pose.x"] = str(self.ui.x_sb.value())
tempNode.attributes["pose.z"] = str(self.ui.z_sb.value())
tempNode.attributes["pose.rx"] = str(self.ui.rot_sb.value())
tempNode.attributes["photo"] = str(self.currentImagePath)
self.newModel = AGMModelConversion.fromInternalToIce(self.worldModel)
self.nodeUpdate(personID, tempNode.attributes)
try:
self.agmexecutive_proxy.structuralChangeProposal(
self.newModel, "gui1", "logger")
except:
print("Exception moving in AGM")
print(self.persons)
def removeEdgeAGM(self):
print("remove")
itemR = self.ui.interaction_lw.currentRow()
item1 = self.ui.interaction_lw.currentItem()
if not item1 is None:
edge = item1.data(Qt.UserRole)
print(edge)
# print(self.worldModel)
src = self.worldModel
print(src.links[-1], type(src.links[0].a))
print(edge, type(edge.a))
try:
if self.worldModel.removeEdge(edge.a, edge.b, edge.linkType):
print("success")
self.newModel = AGMModelConversion.fromInternalToIce(
self.worldModel)
self.agmexecutive_proxy.structuralChangeProposal(
self.newModel, "gui1", "log2")
self.ui.interaction_lw.takeItem(itemR)
del item1
del itemR
else:
print("fail")
except:
print("error deleting edge from worldModel")
else:
msgBox = QMessageBox()
msgBox.setText('No interaction is selected')
msgBox.exec_()
class Executive(object):
def __init__(self, agglPath, initialModelPath, initialMissionPath, doNotPlan, executiveTopic, executiveVisualizationTopic):
self.doNotPlan = doNotPlan
self.mutex = threading.RLock()
self.agents = dict()
self.plan = AGGLPlannerPlan('', planFromText=True)
self.modifications = 0
self.agglplannerclient = make_client(agglplanner_thrift.AGGLPlanner, '127.0.0.1', 6000)
self.plannerCaller = PlannerCaller(self, agglPath)
self.plannerCaller.start()
print '--- setMission ---------------------------------------------'
self.setMission(initialMissionPath, avoidUpdate=True)
print '--- setMission ---------------------------------------------'
# Set proxies
self.executiveTopic = executiveTopic
self.executiveVisualizationTopic = executiveVisualizationTopic
self.agglPath = agglPath
self.initialModelPath = initialModelPath
try:
self.initialModel = xmlModelParser.graphFromXMLFile(initialModelPath)
except Exception, e:
print 'Can\'t open ' + initialModelPath + '.'
os._exit(-1)
print 'INITIAL MODEL: ', self.initialModel
self.lastModification = self.initialModel
print 'initial model version', self.lastModification.version
self.backModelICE = None
self.setAndBroadcastModel(xmlModelParser.graphFromXMLFile(initialModelPath))
self.worldModelICE = AGMModelConversion.fromInternalToIce(self.currentModel)
def AGMExecutiveTopic_structuralChange(self, w):
print('AGMExecutiveTopic_structuralChange')
self.mutex.lock()
self.worldModel = AGMModelConversion.fromIceToInternal_model(w)
self.mutex.unlock()
print('--- End structuralChange ---')
def structuralChangeProposal(self, worldModelICE, sender, log):
print '\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n'
#
# H E R E W E S H O U L D C H E C K T H E M O D I F I C A T I O N I S V A L I D
#
print 'Executive::structuralChangeProposal:', sender, log
# Get structuralChange mutex
print 'Executive::structuralChangeProposal: structuralChangeProposal acquire() a'
for iixi in xrange(5):
gotMutex = self.mutex.acquire(blocking=False)
if gotMutex == True:
break
else:
if iixi == 4:
print 'Executive::structuralChangeProposal: structuralChangeProposal acquire() IT WAS LOCKED'
raise RoboCompAGMExecutive.Locked()
else:
time.sleep(0.03)
print 'Executive::structuralChangeProposal:CURRENT VERSION', self.lastModification.version
try:
# Ignore outdated modifications
if worldModelICE.version != self.lastModification.version:
print 'Executive::structuralChangeProposal: outdated!??! self='+str(self.lastModification.version)+' ice='+str(worldModelICE.version)
raise RoboCompAGMExecutive.OldModel()
# Here we're OK with the modification, accept it, but first check if replanning is necessary
worldModelICE.version += 1
internalModel = AGMModelConversion.fromIceToInternal_model(worldModelICE, ignoreInvalidEdges=True) # set internal model
avoidReplanning = False
# UNCOMMENT THIS!!! WARNING TODO ERROR CUIDADO
#try: # is replanning necessary?
#if internalModel.equivalent(self.lastModification):
#avoidReplanning = True
#except AttributeError:
#pass
self.lastModification = internalModel # store last modification
self.modifications += 1
self.worldModelICE = worldModelICE
# Store the model in XML
#try:
#internalModel.toXML('modification'+str(self.modifications).zfill(4)+'_'+sender+'.xml')
#except:
#print 'There was some problem updating internal model to xml'
# Set and broadst the model
try:
self.setAndBroadcastModel(internalModel)
except:
print traceback.print_exc()
print 'Executive::structuralChangeProposal: There was some problem broadcasting the model'
sys.exit(-1)
# Force replanning
try:
if not (avoidReplanning or self.doNotPlan):
self.updatePlan()
except:
print traceback.print_exc()
print 'Executive::structuralChangeProposal: There was some problem updating internal model to xml'
finally:
self.mutex.release()
return
def AGMExecutiveTopic_structuralChange(self, w):
self.mutex.lock()
# print("before")
# print(type(self.worldModel), type(w))
self.worldModel = AGMModelConversion.fromIceToInternal_model(w)
# print("after")
# print(type(self.worldModel), type(w))
# print(self.worldModel)
# fromIceToInternal(w, worldModel)
self.mutex.unlock()
def structuralChange(self, w):
self.mutex.lock()
self.internalModel = AGMModelConversion.fromIceToInternal_model(
w, ignoreInvalidEdges=True)
ev_ident = QtCore.QTime.currentTime().toString(
"hh:mm:ss.zzz") + " (model)"
self.events[ev_ident] = self.internalModel.toXMLString()
self.ui.historyWidgetList.addItem(ev_ident)
self.mutex.unlock()
def broadcastModel(self): self.mutex.acquire( ) try: print 'Executive::broadcastModel: <<<broadcastinnn' self.executiveTopic.structuralChange(AGMModelConversion.fromInternalToIce(self.currentModel)) print 'Executive::broadcastModel: broadcastinnn>>>' except: print traceback.print_exc() print 'Executive::broadcastModel:There was some problem broadcasting the model' sys.exit(1) self.mutex.release()
def AGMinit(self):
try:
w = self.agmexecutive_proxy.getModel()
# print(w)
self.mutex.lock()
self.worldModel = AGMModelConversion.fromIceToInternal_model(w)
self.mutex.unlock()
print("The executive is running")
return True
except:
print("The executive is probably not running, waiting for first AGM model publication...")
return False
def broadcastModel(self): self.mutex.acquire( ) try: print '<<<broadcastinnn' print self.currentModel.filterGeometricSymbols() self.executiveTopic.structuralChange(AGMModelConversion.fromInternalToIce(self.currentModel)) print 'broadcastinnn>>>' except: print traceback.print_exc() print 'There was some problem broadcasting the model' sys.exit(1) self.mutex.release()
def updatingDSR(self):
print('updating DSR')
try:
newModelICE = AGMModelConversion.fromInternalToIce(self.worldModel)
self.agmexecutive_proxy.structuralChangeProposal(
newModelICE, "conversationalAgent", "")
# w = self.agmexecutive_proxy.getModel()
# self.worldModel = AGMModelConversion.fromIceToInternal_model(w)
print("AGM successfully updated")
return True
except:
print("Exception in AGM")
return False
def symbolsUpdate(self, symbols): try: self.mutex.acquire() try: for symbol in symbols: internal = AGMModelConversion.fromIceToInternal_node(symbol) self.currentModel.nodes[internal.name] = copy.deepcopy(internal) except: print traceback.print_exc() print 'Executive::symbolsUpdate: There was some problem with update node' sys.exit(1) self.executiveTopic.symbolsUpdated(symbols) finally: self.mutex.release()
def includeInAGM(self, Id, pose, mesh):
print("includeInAGM begins\n")
# name = "person"
imName = "person" + str(Id)
# personSymbolId = -1
# idx = 0
attribute2 = dict()
attribute2["imName"] = imName
attribute2["imType"] = "transform"
self.worldModel.addNode(0, 0, Id, "person", attribute2)
self.worldModel.addEdge(Id, 3, "in")
edgeRTAtrs2 = dict()
edgeRTAtrs2["tx"] = "0"
edgeRTAtrs2["ty"] = "0"
edgeRTAtrs2["tz"] = "0"
edgeRTAtrs2["rx"] = "0"
edgeRTAtrs2["ry"] = "0"
edgeRTAtrs2["rz"] = "0"
self.worldModel.addEdge(100, Id, "RT", edgeRTAtrs2)
attribute = dict()
attribute["collidable"] = "false"
attribute["imName"] = imName + "_Mesh"
attribute["imType"] = "mesh"
meshPath = "/home/robocomp/robocomp/components/robocomp-viriato/files/osgModels/" + mesh
attribute["path"] = str(meshPath)
attribute["render"] = "NormalRendering"
attribute["scalex"] = str(12)
attribute["scaley"] = str(12)
attribute["scalez"] = str(12)
self.worldModel.addNode(0, 0, Id + 1, "mesh_person", attribute)
# self.worldModel.addNode(0, 0, temp_id + 1, "personMesh")
edgeRTAtrs = dict()
edgeRTAtrs["tx"] = "0"
edgeRTAtrs["ty"] = "0"
edgeRTAtrs["tz"] = "0"
edgeRTAtrs["rx"] = "1.570796326794"
edgeRTAtrs["ry"] = "0"
edgeRTAtrs["rz"] = "3.1415926535"
# id used in addEdge should be int type
self.worldModel.addEdge(Id, Id + 1, "RT", edgeRTAtrs)
self.newModel = AGMModelConversion.fromInternalToIce(self.worldModel)
self.agmexecutive_proxy.structuralChangeProposal(
self.newModel, "gui1", "log2")
print("includeInAGM ends\n")
return Id
def symbolsUpdate(self, symbols): try: #print 'symbolsUpdate acquire() a' self.mutex.acquire() #print 'symbolsUpdate acquire() z' try: for symbol in symbols: internal = AGMModelConversion.fromIceToInternal_node(symbol) self.currentModel.nodes[internal.name] = copy.deepcopy(internal) except: print traceback.print_exc() print 'There was some problem with update node' sys.exit(1) self.executiveTopic.symbolsUpdated(symbols) finally: #print 'symbolsUpdate release() a' self.mutex.release()
class Executive(object):
def __init__(self, agglPath, initialModelPath, initialMissionPath,
doNotPlan, executiveTopic, executiveVisualizationTopic,
speech):
self.doNotPlan = doNotPlan
self.executiveActive = True
self.mutex = threading.RLock()
self.agents = dict()
self.plan = None
self.modifications = 0
self.plannerCaller = PlannerCaller(self, agglPath)
self.plannerCaller.start()
# Set proxies
self.executiveTopic = executiveTopic
self.executiveVisualizationTopic = executiveVisualizationTopic
self.speech = speech
self.agglPath = agglPath
self.initialModelPath = initialModelPath
try:
self.initialModel = xmlModelParser.graphFromXML(initialModelPath)
except Exception, e:
print 'Can\'t open ' + initialModelPath + '.'
sys.exit(-1)
print 'INITIAL MODEL: ', self.initialModel
self.lastModification = self.initialModel
print self.lastModification.version
print self.lastModification.version
print self.lastModification.version
print self.lastModification.version
print self.lastModification.version
print self.lastModification.version
self.backModelICE = None
self.setAndBroadcastModel(
xmlModelParser.graphFromXML(initialModelPath))
self.worldModelICE = AGMModelConversion.fromInternalToIce(
self.currentModel)
self.setMission(initialMissionPath, avoidUpdate=True)
def setPose(self):
if self.ui.id_list.currentText() == "":
print("No selected person to move")
else:
temp_id = int(self.ui.id_list.currentText())
name = "person" + str(temp_id)
pose = Pose3D()
pose.x = self.ui.x_sb.value()
pose.y = 0
pose.z = self.ui.z_sb.value()
pose.rx = 0
pose.ry = self.ui.rot_sb.value()
pose.rz = 0
# move the person in RCIS
try:
self.innermodelmanager_proxy.setPoseFromParent(name, pose)
except:
print("Exception moving person in RCIS: ")
return
# move the person in AGM
edgeRTAtrs = dict()
edgeRTAtrs["tx"] = str(self.ui.x_sb.value())
edgeRTAtrs["ty"] = "0"
edgeRTAtrs["tz"] = str(self.ui.z_sb.value())
edgeRTAtrs["rx"] = "0"
edgeRTAtrs["ry"] = str(self.ui.rot_sb.value())
edgeRTAtrs["rz"] = "0"
self.newModel = AGMModelConversion.fromInternalToIce(
self.worldModel)
self.edgeUpdate(100, temp_id, "RT", edgeRTAtrs)
try:
self.agmexecutive_proxy.structuralChangeProposal(
self.newModel, "gui1", "logger")
except:
print("Exception moving in AGM")
def AGMExecutiveTopic_symbolsUpdated(self, modifications):
for modification in modifications:
AGMModelConversion.fromIceToInternal_node(modification)
def addInteraction(self):
curr_text = self.ui.interaction_cb.currentText()
id1 = self.ui.int1_cb.currentText()
id2 = self.ui.int2_cb.currentText()
robotID = "1"
if curr_text == "isBusy":
self.ui.int2_cb.setEnabled(False)
id2 = id1
listEntry = id1 + " => " + curr_text
elif curr_text == "block":
self.ui.int2_cb.setEnabled(False)
id2 = robotID
listEntry = id1 + " => " + curr_text + " " + id2
elif curr_text == "softBlock":
self.ui.int2_cb.setEnabled(False)
id2 = robotID
listEntry = id1 + " => " + curr_text + " " + id2
elif curr_text == "affordanceBlock":
self.ui.int2_cb.setEnabled(False)
id2 = robotID
listEntry = id1 + " => " + curr_text + " " + id2
elif curr_text == "interacting":
self.ui.int2_cb.setEnabled(True)
if id1 == id2:
msgBox = QMessageBox()
msgBox.setText('Person could not interact with himself')
msgBox.exec_()
return
listEntry = id1 + " => " + curr_text + " " + id2
else:
print("Unknown interaction selected")
return
# check if the link is already added or not
listNum = self.ui.interaction_lw.findItems(listEntry, Qt.MatchExactly)
if len(listNum) > 0:
msgBox = QMessageBox()
msgBox.setText('Interaction is already used')
msgBox.exec_()
else:
if self.ui.interaction_cb.currentText() == "interacting":
attrs = {
'type': str(self.ui.interaction_cb_2.currentText()),
'timeStarted': str(datetime.now()),
}
else:
attrs = None
self.worldModel.addEdge(id1, id2, curr_text, attrs)
try:
edge = AGMLink(id1, id2, curr_text, attrs, enabled=True)
self.newModel = AGMModelConversion.fromInternalToIce(
self.worldModel)
item = QListWidgetItem(listEntry)
# adding the edge information to the item
item.setData(Qt.UserRole, edge)
self.agmexecutive_proxy.structuralChangeProposal(
self.newModel, "gui1", "log2")
self.ui.interaction_lw.addItem(item)
self.ui.rinteraction_pb.setEnabled(True)
except:
print("error retrieving edge from newModel")
