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)
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 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_()
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 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 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 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 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 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 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
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 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")