def __init__(self): QThread.__init__(self) self.code = "" self.carte = Carte() self.planification = Cheminement(self.carte) self.voltageRestant = "" self.ileCible = -1 self.positionTresor = [] self.destination = [] self.pause = True self.reset = False self.startCycle = False self.demiCercle = 180 self.tempAttenteRotation = 9 self.delaiAjustement = 4.5 self.metreSeconde = 15 self.metre = 100 self.actionPrecedente = "" self.actionCourrante = "" self.nouvelleAction = False self.finCycle = False self.connected = False self.noPathFound = False self.aIle = False self.diametreIle = 2.125 self.deltaPosition = 15 self.deltaAngle = 4 self.deltaDiametreIle = 0.125
def reInitialiserVariables(self): self.code = "" self.carte = Carte() self.planification = Cheminement(self.carte) self.ileCible = -1 self.destination = [] self.pause = True self.reset = False self.startCycle = False self.repereCycle = [0,0] self.distanceMinimum = 60 self.cycle = True self.enMouvement = False self.possedeTresor = False self.rechargeActiver = False self.auTresor = False self.nouvelleAction = False self.aIle = False self.retour = "" self.diametreIle = 2.125
def setUp(self): self.carte = Carte() self.positionXAleatoire = 100 self.positionYAleatoire = 100
class TestUnitaire_Carte(unittest.TestCase): carte = Carte() def setUp(self): self.carte = Carte() self.positionXAleatoire = 100 self.positionYAleatoire = 100 def tearDown(self): self.carte = None def test_ajouterDesSommetsAugmenteLaListe(self): self.carte.ajouterSommet('a', self.positionXAleatoire, self.positionYAleatoire) self.carte.ajouterSommet('b', self.positionXAleatoire, self.positionYAleatoire) self.carte.ajouterSommet('c', self.positionXAleatoire, self.positionYAleatoire) self.assertEqual(len(self.carte.recupererSommets()), 3) def test_retourneLeBonNoeud(self): self.carte.ajouterSommet('a', self.positionXAleatoire, self.positionYAleatoire) self.carte.ajouterSommet('b', self.positionXAleatoire, self.positionYAleatoire) self.carte.ajouterSommet('c', self.positionXAleatoire, self.positionYAleatoire) self.assertEqual(self.carte.recupererSommet(1).id, 'b') def test_ajouterUnArreteMetLesNoeudVoisin(self): self.carte.ajouterSommet('a', self.positionXAleatoire, self.positionYAleatoire) self.carte.ajouterSommet('b', self.positionXAleatoire, self.positionYAleatoire) self.carte.ajouterSommet('c', self.positionXAleatoire, self.positionYAleatoire) self.carte.ajouterArete(self.carte.recupererSommet(0), self.carte.recupererSommet(1), 1) self.carte.ajouterArete(self.carte.recupererSommet(1), self.carte.recupererSommet(2), 1) self.assertEqual(len(self.carte.recupererSommet(0).adjacent), 1) self.assertEqual(len(self.carte.recupererSommet(1).adjacent), 2) def test_SoitDeuxSommetsLaDistanceEntreLesDeuxSommetsEstExact(self): point1 = (0, 0) point2 = (1, 1) valeurRetournee = self.carte.distanceEntrePoint(point1, point2) self.assertEqual(valeurRetournee, np.sqrt(2))
class StationDeBase(QThread): def distancePositionDestination(self): if len(positionRobotTrouvee) == 2 and len(self.destination) == 2: return np.sqrt(np.power(positionRobotTrouvee[0] - self.destination[0], 2) + np.power(positionRobotTrouvee[1] - self.destination[1], 2)) else: return sys.maxint def __init__(self): QThread.__init__(self) self.code = "" self.carte = Carte() self.planification = Cheminement(self.carte) self.voltageRestant = "" self.ileCible = -1 self.positionTresor = [] self.destination = [] self.pause = True self.reset = False self.startCycle = False self.demiCercle = 180 self.tempAttenteRotation = 9 self.delaiAjustement = 4.5 self.metreSeconde = 15 self.metre = 100 self.actionPrecedente = "" self.actionCourrante = "" self.nouvelleAction = False self.finCycle = False self.connected = False self.noPathFound = False self.aIle = False self.diametreIle = 2.125 self.deltaPosition = 15 self.deltaAngle = 4 self.deltaDiametreIle = 0.125 def reInitialiserVariables(self): self.code = "" self.carte = Carte() self.planification = Cheminement(self.carte) self.ileCible = -1 self.destination = [] self.pause = True self.reset = False self.startCycle = False self.repereCycle = [0,0] self.distanceMinimum = 60 self.cycle = True self.enMouvement = False self.possedeTresor = False self.rechargeActiver = False self.auTresor = False self.nouvelleAction = False self.aIle = False self.retour = "" self.diametreIle = 2.125 def run(self): self.repereCycle = [0,0] self.distanceMinimum = 60 self.cycle = True self.enMouvement = False self.possedeTresor = False self.rechargeActiver = False self.auTresor = False self.retour = "" nombreRobotEntreLimitEtNoeudTresor = 1.3 global trajectoireCalculer IpRobot = "192.168.0.48" port = 50007 buf = 1024 addr = (IpRobot, port) UDPSock = socket(AF_INET, SOCK_STREAM) UDPSock.connect(addr) self.connected = True while self.cycle: if self.reset == True: UDPSock.sendto("6", addr) time.sleep(1) self.retour = UDPSock.recv(buf) print self.retour self.reInitialiserVariables() try: if len(repere) == 2 and len(listePositions) >= 1 and len(positionRobotTrouvee) == 2 and self.pause == False: if abs(repere[0] - self.repereCycle[0]) > 5 or abs(repere[1] - self.repereCycle[1]) > 5: self.carte.genererCarte(repere, listePositions, ratioCMPixel,listePositionsTresor,self.diametreIle) self.planification = Cheminement(self.carte) self.repereCycle = repere if self.enMouvement: self.actionPrecedente = self.actionCourrante self.actionCourrante = "Mouvement" self.nouvelleAction = True deltaX = trajectoireCalculer[0].x - positionRobotTrouvee[0] deltaY = positionRobotTrouvee[1] - trajectoireCalculer[0].y angle = degrees(atan2(deltaY,deltaX)) if angle < 0: angle = 360 + angle angleRotation = angle - orientationRobotTrouvee if angleRotation < 0: angleRotation = 360 + angleRotation angleText = str(int(angleRotation)) while len(angleText) < 4: angleText = '0' + angleText distanceCalcule = np.sqrt(np.power((trajectoireCalculer[0].x - positionRobotTrouvee[0]), 2) + np.power((trajectoireCalculer[0].y - positionRobotTrouvee[1]), 2)) * ratioCMPixel distanceText = str(int(distanceCalcule)) while len(distanceText) < 3: distanceText = '0' + distanceText demande = '1' + angleText + distanceText UDPSock.sendto(demande, addr) if angleRotation > 180: angleRotation = 360 - angleRotation tempAttente = (angleRotation)/self.demiCercle * self.tempAttenteRotation + distanceCalcule/self.metre * self.metreSeconde time.sleep(tempAttente) self.retour = UDPSock.recv(buf) if self.distancePositionDestination() < self.distanceMinimum: self.enMouvement = False trajectoireCalculer.pop(0) if len(trajectoireCalculer) == 0: self.carte.genererCarte(repere, listePositions, ratioCMPixel,listePositionsTresor,self.diametreIle) self.planification = Cheminement(self.carte) trajectoireCalculer = self.planification.trouverTrajectoire(positionRobotTrouvee,self.destination) if self.rechargeActiver == True: self.destination = (self.carte.recupererSommet(0).x,self.carte.recupererSommet(0).y) trajectoireCalculer.append(self.carte.recupererSommet(0)) elif self.auTresor == True: distanceTresor = sys.maxint for position in listePositionsTresor: distanceCalcule = np.sqrt(np.power((position[0] - positionRobotTrouvee[0]), 2) + np.power((position[1] - positionRobotTrouvee[1]), 2)) noeudTresorTemp = None for noeudId in self.carte.dictSommets: noeud = self.carte.recupererSommet(noeudId) if noeud.estTresor == True: if position[1] < repere[1]: if position[0] - self.deltaPosition < noeud.x < position[0] + self.deltaPosition and \ noeud.y - self.deltaPosition < position[1] + nombreRobotEntreLimitEtNoeudTresor * self.carte.robotPixel < noeud.y + self.deltaPosition: noeudTresorTemp = self.carte.recupererSommet(noeud.id) break else: if position[0] - self.deltaPosition < noeud.x < position[0] + self.deltaPosition and \ noeud.y - self.deltaPosition < position[1] - nombreRobotEntreLimitEtNoeudTresor * self.carte.robotPixel < noeud.y + self.deltaPosition: noeudTresorTemp = self.carte.recupererSommet(noeud.id) break if distanceCalcule < distanceTresor and noeudTresorTemp != None: self.destination = position distanceTresor = distanceCalcule self.noeudTresor = noeudTresorTemp self.carte.genererCarte(repere, listePositions, ratioCMPixel,listePositionsTresor,self.diametreIle) self.planification = Cheminement(self.carte) trajectoireCalculer = self.planification.trouverTrajectoire(positionRobotTrouvee,self.destination) trajectoireCalculer.append(self.noeudTresor) self.destination = (self.noeudTresor.x, self.noeudTresor.y) elif self.possedeTresor == True: self.carte.genererCarte(repere, listePositions, ratioCMPixel,listePositionsTresor,self.diametreIle) self.planification = Cheminement(self.carte) noeud = self.planification.trouverPointPlusProcheIle(listePositions[self.ileCible],positionRobotTrouvee) self.destination = (noeud.x,noeud.y) trajectoireCalculer = self.planification.trouverTrajectoire(positionRobotTrouvee,(self.destination[0],self.destination[1])) trajectoireCalculer.append(noeud) elif self.possedeTresor == False and self.enMouvement == False \ and self.rechargeActiver == False and len(self.code) == 0 and self.auTresor == False: self.carte.genererCarte(repere, listePositions, ratioCMPixel,listePositionsTresor,self.diametreIle) self.planification = Cheminement(self.carte) self.actionPrecedente = self.actionCourrante self.actionCourrante = "Calcule Trajectoire a station de base" self.nouvelleAction = True self.destination = (self.carte.recupererSommet(0).x,self.carte.recupererSommet(0).y) trajectoireCalculer = self.planification.trouverTrajectoire(positionRobotTrouvee,self.destination) if len(trajectoireCalculer) == 0: raise PathNotFoundException() trajectoireCalculer.append(self.carte.recupererSommet(0)) self.enMouvement = True self.rechargeActiver = True elif self.possedeTresor == False and self.enMouvement == False \ and self.rechargeActiver == True and len(self.code) == 0 and self.auTresor == False and self.aIle == False: self.actionPrecedente = self.actionCourrante self.actionCourrante = "Ajustement" self.nouvelleAction = True angle = 90 limitVoltage = 4.5 while angle - self.deltaAngle >= orientationRobotTrouvee or orientationRobotTrouvee >= angle + self.deltaAngle: angleRotation = angle - orientationRobotTrouvee print "angle: " + str(orientationRobotTrouvee) if angleRotation < 0: angleRotation = 360 + angleRotation angleText = str(int(angleRotation)) print "rotoation " + angleText while len(angleText) < 4: angleText = '0' + angleText demande = "1" + angleText + "000" UDPSock.sendto(demande, addr) if angleRotation > 180: angleRotation = 360 - angleRotation tempAttente = (angleRotation)/self.demiCercle * self.tempAttenteRotation + self.delaiAjustement time.sleep(tempAttente) self.retour = UDPSock.recv(buf) self.voltageRestant = self.retour[len(self.retour) - 4] + self.retour[len(self.retour) - 3] + self.retour[len(self.retour) - 2] + self.retour[len(self.retour) - 1] time.sleep(1) demande = '3' UDPSock.sendto(demande,addr) time.sleep(15) self.actionPrecedente = self.actionCourrante self.actionCourrante = "Recharge et request au serveur" self.nouvelleAction = True while float(self.voltageRestant) < limitVoltage or not(self.retour.find("couleur") > 0 or self.retour.find("forme") > 0): try: self.retour = UDPSock.recv(buf) self.voltageRestant = self.retour[len(self.retour) - 4] + self.retour[len(self.retour) - 3] + self.retour[len(self.retour) - 2] + self.retour[len(self.retour) - 1] except: pass self.code = self.retour[len(self.retour) - 5] if self.retour.find("couleur") > 0: for index in range(0,len(listeCouleurs)): if self.retour.find(listeCouleurs[index]) > 0: self.ileCible = index break if self.retour.find("forme") > 0: for index in range(0, len(listeFormes)): if self.retour.find(listeFormes[index]) > 0: self.ileCible = index break print self.ileCible time.sleep(10) self.rechargeActiver = False elif self.possedeTresor == False and self.enMouvement == False \ and self.rechargeActiver == False and len(self.code) == 1 and self.auTresor == False and self.aIle == False: if len(listePositionsTresor) < 0: positionXTresorText = "" positionYTresorText = "" orientationRobotText = str(int(orientationRobotTrouvee)) while len(orientationRobotText) < 3: orientationRobotText = '0' + orientationRobotText positionRobotXText = str(int(positionRobotTrouvee[0])) while len(positionRobotXText) < 4: positionRobotXText = '0'+positionRobotXText positionRobotYText = str(int(positionRobotTrouvee[1])) while len(positionRobotYText) < 4: positionRobotYText = '0'+positionRobotYText repereXText = str(int(repere[0])) while len(repereXText) < 4: repereXText = '0' + repereXText repereYText = str(int(repere[1])) while len(repereYText) < 4: repereYText = '0' + repereYText ratioText = "%.2f" % round(ratioCMPixel, 2) while ratioText < 4: ratioText = '0' + ratioText demande = '2' + orientationRobotText + positionRobotYText + repereXText + repereYText + ratioText UDPSock.sendto(demande,addr) self.retour = UDPSock.recv(buf) index = 0 while self.retour[index] != " ": positionXTresorText = positionXTresorText + self.retour[index] index += 1 index += 1 while index < len(self.retour) - 4: positionYTresorText = positionYTresorText + self.retour[index] index += 1 positionTresor.append(int(positionXTresorText)) positionTresor.append(int(positionYTresorText)) else: self.actionPrecedente = self.actionCourrante self.actionCourrante = "Calcule trajectoire au tresor" self.nouvelleAction = True distanceTresor = sys.maxint for position in listePositionsTresor: distanceCalcule = np.sqrt(np.power((position[0] - positionRobotTrouvee[0]), 2) + np.power((position[1] - positionRobotTrouvee[1]), 2)) noeudTresorTemp = None for noeudId in self.carte.dictSommets: noeud = self.carte.recupererSommet(noeudId) if noeud.estTresor == True: if position[1] < repere[1]: if position[0] - 15 < noeud.x < position[0] + 15 and noeud.y - 15 < position[1] + 1.3 * self.carte.robotPixel < noeud.y + 15: noeudTresorTemp = self.carte.recupererSommet(noeud.id) break else: if position[0] - 15 < noeud.x < position[0] + 15 and noeud.y - 15 < position[1] - 1.3 * self.carte.robotPixel < noeud.y + 15: noeudTresorTemp = self.carte.recupererSommet(noeud.id) break if distanceCalcule < distanceTresor and noeudTresorTemp != None: self.destination = position distanceTresor = distanceCalcule self.noeudTresor = noeudTresorTemp self.carte.genererCarte(repere, listePositions, ratioCMPixel,listePositionsTresor,self.diametreIle) self.planification = Cheminement(self.carte) trajectoireCalculer = self.planification.trouverTrajectoire(positionRobotTrouvee,self.destination) if len(trajectoireCalculer) == 0: raise PathNotFoundException() trajectoireCalculer.append(self.noeudTresor) self.destination = (self.noeudTresor.x, self.noeudTresor.y) self.enMouvement = True self.auTresor = True if self.possedeTresor == False and self.enMouvement == False \ and self.rechargeActiver == False and len(self.code) == 1 and self.auTresor == True and self.aIle == False: if self.destination[1] < repere[1]: angle = 90 else: angle = 270 self.actionPrecedente = self.actionCourrante self.actionCourrante = "Ajustement" self.nouvelleAction = True while angle-self.deltaAngle >= orientationRobotTrouvee or orientationRobotTrouvee >= angle+self.deltaAngle: angleRotation = angle - orientationRobotTrouvee print "angle: " + str(orientationRobotTrouvee) if angleRotation < 0: angleRotation = 360 + angleRotation angleText = str(int(angleRotation)) print "rotoation " + angleText while len(angleText) < 4: angleText = '0' + angleText demande = "1" + angleText + "000" UDPSock.sendto(demande, addr) if angleRotation > 180: angleRotation = 360 - angleRotation tempAttente = (angleRotation)/self.demiCercle * self.tempAttenteRotation + self.delaiAjustement time.sleep(tempAttente) self.retour = UDPSock.recv(buf) self.voltageRestant = self.retour[len(self.retour) - 4] + self.retour[len(self.retour) - 3] + self.retour[len(self.retour) - 2] + self.retour[len(self.retour) - 1] demande = '4' UDPSock.sendto(demande,addr) time.sleep(5) self.actionPrecedente = self.actionCourrante self.actionCourrante = "Prise du robot" self.nouvelleAction = True self.retour = UDPSock.recv(buf) self.possedeTresor = True elif self.enMouvement == False and self.rechargeActiver == False \ and len(self.code) == 1 and self.auTresor == True and self.possedeTresor == True and self.aIle == False: self.carte.genererCarte(repere, listePositions, ratioCMPixel,listePositionsTresor,self.diametreIle) self.planification = Cheminement(self.carte) noeud = self.planification.trouverPointPlusProcheIle(listePositions[self.ileCible],positionRobotTrouvee) self.destination = (noeud.x,noeud.y) trajectoireCalculer = self.planification.trouverTrajectoire(positionRobotTrouvee,(self.destination[0],self.destination[1])) if len(trajectoireCalculer) == 0: raise PathNotFoundException() trajectoireCalculer.append(noeud) self.auTresor = False self.enMouvement = True self.aIle = True elif self.enMouvement == False and self.rechargeActiver == False \ and len(self.code) == 1 and self.auTresor == False and self.possedeTresor == True and self.aIle == True: deltaX = listePositions[self.ileCible][0] - positionRobotTrouvee[0] deltaY = positionRobotTrouvee[1] - listePositions[self.ileCible][1] angle = degrees(atan2(deltaY,deltaX)) if angle < 0: angle = 360 + angle self.actionPrecedente = self.actionCourrante self.actionCourrante = "Ajustement" self.nouvelleAction = True while angle - self.deltaAngle >= orientationRobotTrouvee or orientationRobotTrouvee >= angle + self.deltaAngle: angleRotation = angle - orientationRobotTrouvee print "angle: " + str(orientationRobotTrouvee) if angleRotation < 0: angleRotation = 360 + angleRotation angleText = str(int(angleRotation)) print "rotoation " + angleText while len(angleText) < 4: angleText = '0' + angleText demande = "1" + angleText + "000" UDPSock.sendto(demande, addr) if angleRotation > 180: angleRotation = 360 - angleRotation tempAttente = (angleRotation)/self.demiCercle * self.tempAttenteRotation + self.delaiAjustement time.sleep(tempAttente) self.retour = UDPSock.recv(buf) self.voltageRestant = self.retour[len(self.retour) - 4] + self.retour[len(self.retour) - 3] + self.retour[len(self.retour) - 2] + self.retour[len(self.retour) - 1] couleur = "" if listeCouleurs[self.ileCible] == "bleu": couleur = "1" elif listeCouleurs[self.ileCible] == "vert": couleur = "2" elif listeCouleurs[self.ileCible] == "rouge": couleur = "3" elif listeCouleurs[self.ileCible] == "jaune": couleur = "4" demande = '5' + couleur UDPSock.sendto(demande,addr) time.sleep(15) self.actionPrecedente = self.actionCourrante self.actionCourrante = "Depot du robot" self.nouvelleAction = True self.retour = UDPSock.recv(buf) self.finCycle = True self.reset = True if self.retour != "": print self.retour self.voltageRestant = self.retour[len(self.retour) - 4] + self.retour[len(self.retour) - 3] + self.retour[len(self.retour) - 2] + self.retour[len(self.retour) - 1] except PathNotFoundException as e: self.noPathFound = True if self.diametreIle > 1.5: self.diametreIle -= self.deltaDiametreIle else: demiCercle = 179 demande = "10179010" UDPSock.sendto(demande, addr) tempAttente = (demiCercle)/self.demiCercle * self.tempAttenteRotation + 10/self.metre * self.metreSeconde time.sleep(tempAttente) self.retour = UDPSock.recv(buf) continue