Exemplo n.º 1
0
    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
Exemplo n.º 2
0
    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))