def receivePlanSyncResponse(self, sender, otherPlan): """callback for the plan synchronisation""" with self.mutex: myID = self.plan.ids[-1] otherID = otherPlan["ID"]["value"] if myID == otherID: return logger.info("I'm executing plan %s. %s is executing %s" % (self.plan.ids, sender, otherPlan["ID"])) if self.newPlanToImport is not None: newID = json.loads(self.newPlanToImport)["ID"]["value"] if newID != otherID: logger.warning("I'm executing %s. I received a new plan %s and a previous plan %s. Ignoring this one" % (myID,otherID,newID)) else: logger.info("Ignoring this plan since I have already ask for its use") return if myID in otherPlan["ID"]["parents"]: logger.info("The other has repaired and I was not notified. I need to update my plan") agents = set([a["agent"] for a in otherPlan["actions"].values() if "agent" in a]) logger.info("List of agents in this plan : %s " % agents) plansDict = {} #Prevent the removal of coms time otherPlan["current-time"] = (time.time() - self.beginDate) p = Plan(json.dumps(otherPlan), self.agent) #Check that all my communications are still in the plan droppedComs = set() #In my current plan foreignDroppedCom = set() #In the remote plan for k,a in self.plan.actions.items(): if a["agent"] == self.agent: if k in p.actions and a["name"] == p.actions[k]["name"]: continue #Ok, my action is still here elif k not in p.actions and "communicate" in a["name"]: #self.dropCommunication(a["name"]) droppedComs.add(a["name"]) else: logger.error("My action %s (%s) is not in the new plan" % (a["name"],k)) for k,a in p.actions.items(): if a["agent"] == self.agent: if k in self.plan.actions and a["name"] == self.plan.actions[k]["name"]: continue #Ok, my action is still here if "communicate-meta" in a["name"]: comName = a["name"] if comName in self.droppedComs or a["name"]: #I already dropped this com. logger.warning("They kept a com that I dropped %s (%s)" % (a["name"],k)) foreignDroppedCom.add(k) else: logger.error("They added an com action for me %s (%s)" % (a["name"],k)) logger.error("%s not in %s" % (comName, self.droppedComs)) else: logger.error("They added an action for me %s (%s)" % (a["name"],k)) for k1,a1 in self.plan.actions.items(): if a1["name"] == a["name"]: logger.error("I have this action with key %s" % k1) for k,a in self.plan.actions.items(): if "communicate-meta" in a["name"] and k not in p.actions: droppedComs.add(a["name"]) for c in droppedComs: self.dropCommunication(c) if foreignDroppedCom: logger.info("Imported plan before removing a foreign com: %s" % json.dumps(otherPlan)) for c in foreignDroppedCom: logger.info("Removing action %s (%s)" % (otherPlan["actions"][c], c)) logger.info("Length before %s" % len(otherPlan["actions"])) otherPlan = Plan.removeAction(otherPlan, c) #remove the com meta for actions that I dropped logger.info("Length after %s" % len(otherPlan["actions"])) logger.info("Imported plan after removing a foreign com: %s" % json.dumps(otherPlan)) p = Plan(json.dumps(otherPlan), self.agent) for a in agents: if a != self.agent: plansDict[a] = p.getLocalJsonPlan(a) plansDict[self.agent] = self.plan.getLocalJsonPlan(self.agent, currentTime=(time.time() - self.beginDate)) p = Plan.mergeJsonPlans(plansDict, idAgent = sender) p["current-time"] = plansDict[self.agent]["current-time"] # See if the plan is still temporally valid. It could be a problem if I added a ub for a com # while the other robot was late : both constraints are problematic. In this case, drop my # current com or the com of the other robot try: _ = Plan(json.dumps(p), self.agent) except PlanImportError as e: logger.warning("The fused plan will not be valid. Try to drop a current com") for action in p["actions"].values(): #for name,_,_ in self.ongoingActions: name = action["name"] if self.agent not in name: continue #if not (action["startTp"] in p["absolute-time"] and not action["endTp"] in p["absolute-time"]):continue if "communicate " in name: logger.info("I want to drop (%s,%s,%s)" % (name,action["startTp"],action["endTp"])) logger.info("%s" % (action)) logger.info(action["startTp"] in p["absolute-time"] ) logger.info(action["endTp"] in p["absolute-time"] ) logger.info([a["name"] for a in self.plan.actions.values()]) logger.info([a["name"] for a in otherPlan["actions"].values()]) if action["agent"] == self.agent: #Find the com meta name robot1,robot2 = name.split(" ")[1:3] nameIndex = None for k,a in self.plan.actions.items(): if a["name"].startswith("communicate-meta %s %s" % (robot1,robot2)) or\ a["name"].startswith("communicate-meta %s %s" % (robot2,robot1)): name = a["name"] nameIndex = k break if nameIndex is None: logger.error("Could not find the index of the com meta action ! %s" % name) continue #self.state = State.ERROR #return else: logger.warning("Dropping %s (%s)" % (name, nameIndex)) self.dropCommunication(name) else: robot1,robot2 = name.split(" ")[1:3] nameIndex = None for k,a in otherPlan["actions"].items(): if a["name"].startswith("communicate-meta %s %s" % (robot1,robot2)) or\ a["name"].startswith("communicate-meta %s %s" % (robot2,robot1)): name = a["name"] nameIndex = k break if nameIndex is None: logger.error("Could not find the index of the com meta action ! %s" % name) continue #self.state = State.ERROR #return logger.info("Length before %s" % len(otherPlan["actions"])) otherPlan = Plan.removeAction(otherPlan, nameIndex) #remove the com meta for actions that I dropped logger.info("Length after %s" % len(otherPlan["actions"])) p = Plan(json.dumps(otherPlan), self.agent) for a in agents: if a != self.agent: plansDict[a] = p.getLocalJsonPlan(a) plansDict[self.agent] = self.plan.getLocalJsonPlan(self.agent, currentTime=(time.time() - self.beginDate)) p = Plan.mergeJsonPlans(plansDict, idAgent = sender) p["current-time"] = plansDict[self.agent]["current-time"] self.newPlanToImport = json.dumps(p) #logger.info("Other plans are : %s" % plansDict) #logger.info("New plan to import next is %s" % self.newPlanToImport) return elif otherID in self.plan.ids: logger.info("I'm more up to date. Do nothing") return else: logger.info("We are not on the same branch : repair the plan") self.triggerRepair = True
def repairCallback(self, data): """Receives a message on the repair topic""" if self.state == State.DEAD: self.repair_sub.unregister() return type = data.type time = data.time sender = data.sender msg = data.data if self.agent == sender: return if self.state == State.DEAD: return with self.mutex: if type == "repairRequest": if self.state == State.REPAIRINGACTIVE: #Another robot is trying to repair. Abort the repair for one of them if self.agent < sender: logger.warning("%s is also trying to repair. He has priority. Canceling my repair" % sender) pass #cancel my reparation else: logger.warning("%s is also trying to repair. I have priority. Ignoring its message" % sender) return elif self.state == State.TRACKING: p = self.plan.getLocalJsonPlan(self.agent) for k in list(p["actions"].keys()): if "executed" not in p["actions"][k] or not "executed" not in p["actions"][k]: Plan.removeAction(p, k) p["state"] = "tracking" self.sendNewStatusMessage("repairResponse", json.dumps(p)) return elif self.state not in [State.RUNNING, State.TRACKINGCONFIRMATION, State.DONE]: logger.error("Received a repair request not when running. Ignoring it") return logger.info("Received a repair request. Pausing the execution") self.state = State.REPAIRINGPASSIVE self.sendVisuUpdate() self.sendNewStatusMessage("repairResponse", json.dumps(self.plan.getLocalJsonPlan(self.agent))) elif type == "repairResponse": try: plan = json.loads(msg) except TypeError: logger.error("Receive a repair message with msg not a json string : %s" % msg) return if "repairResponse" not in dir(self): return #I'm not currently repairing if sender not in self.repairResponse: self.repairResponse[sender] = plan logger.info("Receive a repair response from %s " % sender) else: logger.error("Received several response from %s. Keeping only the first one" % sender) elif type == "repairDone": logger.info("Receiving a new plan to execute from %s" % sender) planStr = msg if self.state in [State.REPAIRINGPASSIVE, State.TRACKING]: self.init(planStr, self.agent) else: logger.warning("I'm not is the right state but I received a new plan. Ignoring it, will sync later if needed") if self.state == State.REPAIRINGPASSIVE: self.state = State.RUNNING self.sendVisuUpdate() elif type == "targetFound": with self.mutex: self.targetFound(json.loads(msg), selfDetection = False) elif type == "planSyncRequest": self.receivePlanSyncRequest(sender) elif type == "planSyncResponse": msg = json.loads(msg) if "plan" not in msg: logger.error("Received an ill-formated planSyncResponse : %s" % msg) otherPlan = msg["plan"] self.receivePlanSyncResponse(sender, otherPlan) else: logger.warning("Received unsupported message of type %s from %s : %s" % (type, sender, msg))