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 AGMExecutiveTopic_structuralChange(self, w):
print('AGMExecutiveTopic_structuralChange')
self.mutex.lock()
self.worldModel = AGMModelConversion.fromIceToInternal_model(w)
self.mutex.unlock()
print('--- End structuralChange ---')
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):
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 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 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 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 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