def testDeDijsktra(): carte = Carte() carte.ajouterSommet('a') carte.ajouterSommet('b') carte.ajouterSommet('c') carte.ajouterSommet('d') carte.ajouterSommet('e') carte.ajouterSommet('f') carte.ajouterArete('a', 'b', 7) carte.ajouterArete('a', 'c', 9) carte.ajouterArete('a', 'f', 14) carte.ajouterArete('b', 'c', 10) carte.ajouterArete('b', 'd', 15) carte.ajouterArete('c', 'd', 11) carte.ajouterArete('c', 'f', 2) carte.ajouterArete('d', 'e', 6) carte.ajouterArete('e', 'f', 9) cheminement = Cheminement(carte) print 'Carte data:' for v in carte: for w in v.recupererConnection(): vid = v.recupererId() wid = w.recupererId() print '( %s , %s, %3d)' % ( vid, wid, v.recupererCout(w)) cheminement.dijkstra(carte.recupererSommet('a')) target = carte.recupererSommet('e') path = [target.recupererId()] cheminement.plusCourtChemin(target, path) print 'The shortest path : %s' % (path[::-1])
class TestUnitaire_Carte(unittest.TestCase): carte = Carte() point1 = (0, 0) point2 = (1, 1) def setUp(self): self.carte = Carte() self.positionXAleatoire = 100 self.positionYAleatoire = 100 self.point1 = (0, 0) self.point2 = (1, 1) 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_recupererSommetRetourneLeBonNoeud(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_ajouterAreteMetLesNoeudsVoisin(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_distanceEntreDeuxSommetsRetourneLaBonneDistance(self): valeurRetournee = self.carte.distanceEntrePoint(self.point1, self.point2) self.assertEqual(valeurRetournee, np.sqrt(2))
def testRemplissageDeCarte(): localisateurIle = LocalisationIle() localisationRepere = LocalisationRepere() localisationRobot = LocalisationRobot() carte = Carte() photo = cv2.imread("../photos/table_iles_robot14.jpg") repere,ratio = localisationRepere.trouverRepere(photo) listeCouleur,listeForme,listePosition = localisateurIle.trouverIles(photo) robot,orientation = localisationRobot.trouverRobot(photo,ratio) carte.genererCarte(repere, listePosition, ratio, photo) cv2.imshow("photo",photo) cv2.waitKey(0) planification = Cheminement(carte) trajectoire = planification.trouverTrajectoire(robot,(carte.recupererSommet(0).x,carte.recupererSommet(0).y)) trajectoire.append(carte.recupererSommet(0)) cv2.circle(photo,(int(robot[0]),int(robot[1])),5,(0,255,0),5) for point in trajectoire: cv2.circle(photo,(int(point.x),int(point.y)),5,(255,255,255),5) print listeForme print trajectoire cv2.imshow("photo",photo) cv2.waitKey(0)
class TestUnitaire_Cheminement(unittest.TestCase) : carte = Carte() cheminement = Cheminement(carte) sommetA = Sommet('a', 10, 20, False) sommetB = Sommet('b', 100, 100, False) sommetC = Sommet('c', 95, 125, False) sommetD = Sommet('d', 0, 140, False) sommetE = Sommet('e', 80, -60, False) sommetF = Sommet('f', -150, -20, False) def setUp(self): self.carte = Carte() self.sommetA = Sommet(0, 10, 20, False) self.sommetB = Sommet(1, 100, 100, False) self.sommetC = Sommet(2, 95, 125, False) self.sommetD = Sommet(3, 0, 140, False) self.sommetE = Sommet(4, 80, -60, False) self.sommetF = Sommet(5, -150, -20, False) self.carte.ajouterSommet(self.sommetA.id, self.sommetA.x, self.sommetA.y) self.carte.ajouterSommet(self.sommetB.id, self.sommetB.x, self.sommetB.y) self.carte.ajouterSommet(self.sommetC.id, self.sommetC.x, self.sommetC.y) self.carte.ajouterSommet(self.sommetD.id, self.sommetD.x, self.sommetD.y) self.carte.ajouterSommet(self.sommetE.id, self.sommetE.x, self.sommetE.y) self.carte.ajouterSommet(self.sommetF.id, self.sommetF.x, self.sommetF.y) self.carte.ajouterArete(self.carte.recupererSommet(0), self.carte.recupererSommet(1), 7) self.carte.ajouterArete(self.carte.recupererSommet(0), self.carte.recupererSommet(2), 9) self.carte.ajouterArete(self.carte.recupererSommet(0), self.carte.recupererSommet(5), 14) self.carte.ajouterArete(self.carte.recupererSommet(1), self.carte.recupererSommet(2), 10) self.carte.ajouterArete(self.carte.recupererSommet(1), self.carte.recupererSommet(3), 15) self.carte.ajouterArete(self.carte.recupererSommet(2), self.carte.recupererSommet(3), 11) self.carte.ajouterArete(self.carte.recupererSommet(2), self.carte.recupererSommet(5), 2) self.carte.ajouterArete(self.carte.recupererSommet(3), self.carte.recupererSommet(4), 9) self.carte.ajouterArete(self.carte.recupererSommet(4), self.carte.recupererSommet(5), 9) self.cheminement = Cheminement(self.carte) def tearDown(self): pass def test_dijkstraRetourneLePrecedentLePlusProche(self): self.cheminement.dijkstra(self.carte.recupererSommet(0)) '''print self.sommetB.recupererConnection() print self.sommetB.adjacent print self.sommetB.precedent print self.sommetB.distance print self.carte.recupererSommet(1)''' self.assertEqual(self.carte.recupererSommet(0).precedent, None) self.assertEqual(self.carte.recupererSommet(1).precedent.id, self.sommetA.id) self.assertEqual(self.carte.recupererSommet(2).precedent.id, self.sommetA.id) self.assertEqual(self.carte.recupererSommet(3).precedent.id, self.sommetC.id) self.assertEqual(self.carte.recupererSommet(4).precedent.id, self.sommetF.id) self.assertEqual(self.carte.recupererSommet(5).precedent.id, self.sommetC.id) def test_trouverTrajectoireDonneLePlusCourtChemin(self): debut = (self.sommetA.x, self.sommetA.y) cible = (self.sommetE.x, self.sommetE.y) trajectoire = self.cheminement.trouverTrajectoire(debut, cible) self.assertEqual(trajectoire[0].id, 0) self.assertEqual(trajectoire[1].id, 2) self.assertEqual(trajectoire[2].id, 5)
class StationDeBase(QThread): def distancePositionDestination(self): return np.sqrt(np.power((positionRobotTrouvee[0] - self.destination[0]), 2) + np.power((positionRobotTrouvee[1] - self.destination[1]), 2)) def __init__(self): QThread.__init__(self) self.code = "" self.carte = Carte() self.planification = Cheminement(self.carte) self.voltageRestant = 0 self.ileCible = -1 self.positionTresor = [] self.destination = [] self.pause = True self.reset = False def reInitialiserVariables(self): self.code = "" self.carte = Carte() self.planification = Cheminement(self.carte) self.voltageRestant = 0 self.ileCible = -1 self.positionTresor = [] self.destination = [] self.pause = True self.reset = False repereCycle = [0,0] distanceMinimum = 15 cycle = True enMouvement = False possedeTresor = False rechargeActiver = False auTresor = False retour = "" variance = 0 def run(self): repereCycle = [0,0] distanceMinimum = 15 cycle = True enMouvement = False possedeTresor = False rechargeActiver = False auTresor = False retour = "" variance = 0 global trajectoireCalculer IpRobot = "192.168.0.48" port = 50007 buf = 1024 addr = (IpRobot, port) UDPSock = socket(AF_INET, SOCK_STREAM) UDPSock.connect(addr) while cycle: if self.reset == True: self.reInitialiserVariables() if len(repere) == 2 and len(listePositions) == 4 and len(positionRobotTrouvee) == 2 and self.pause == False: if repereCycle[0] != repere[0] or repereCycle[1] != repere[1]: self.carte.genererCarte(repere, listePositions, ratioCMPixel) self.planification = Cheminement(self.carte) repereCycle = repere if enMouvement: deltaX = trajectoireCalculer[0].x - positionRobotTrouvee[0] deltaY = positionRobotTrouvee[1] - trajectoireCalculer[0].y angle = degrees(atan2(deltaY,deltaX)) angleRotation = angle - orientationRobotTrouvee if angleRotation < 0: angleRotation = abs(360 + angleRotation) variance += (angleRotation % 180)/180 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 variance += distanceCalcule / 40 distanceText = str(int(distanceCalcule)) while len(distanceText) < 3: distanceText = '0' + distanceText demande = '1' + angleText + distanceText UDPSock.sendto(demande, addr) time.sleep(10) retour = UDPSock.recv(buf) trajectoireCalculer.pop(0) if variance > 3 or len(trajectoireCalculer) == 0: self.carte = Carte() self.carte.genererCarte(repere, listePositions, ratioCMPixel) self.planification = Cheminement(self.carte) trajectoireCalculer = self.planification.trouverTrajectoire(positionRobotTrouvee,self.destination) variance = 0 if self.distancePositionDestination() < distanceMinimum / ratioCMPixel: enMouvement = False variance = 0 elif self.voltageRestant == 0 and possedeTresor == False and enMouvement == False \ and rechargeActiver == False and len(self.code) == 0 and auTresor == False: self.destination = (self.carte.recupererSommet(0).x,self.carte.recupererSommet(0).y) trajectoireCalculer = self.planification.trouverTrajectoire(positionRobotTrouvee,self.destination) enMouvement = True rechargeActiver = True elif self.voltageRestant == 0 and possedeTresor == False and enMouvement == False \ and rechargeActiver == True and len(self.code) == 0 and auTresor == False: orientationRobotText = str(orientationRobotTrouvee) while orientationRobotText < 3: orientationRobotText = '0' + orientationRobotText positionRobotXText = str(int(positionRobotTrouvee[0])) while len(positionRobotXText) < 4: positionRobotXText = '0'+positionRobotXText positionRobotYText = str(positionRobotTrouvee[1]) while positionRobotYText < 4: positionRobotYText = '0'+positionRobotYText repereXText = str(int(repere[0])) while len(repereXText) < 4: repereXText = '0' + repereXText repereYText = str(repere[1]) while repereYText < 4: repereYText = '0' + repereYText ratioText = "%.2f" % round(ratioCMPixel, 2) while ratioText < 4: ratioText = '0' + ratioText demande = '3' + orientationRobotText + positionRobotXText + positionRobotYText + repereXText + repereYText + ratioText UDPSock.sendto(demande,addr) retour = UDPSock.recv(buf) if retour.find("couleur") > 0: for index in range(0,len(listeCouleurs)): if retour.find(listeCouleurs[index]) > 0: self.ileCible = index break if retour.find("forme") > 0: for index in range(0, len(listeFormes)): if retour.find(listeFormes[index]) > 0: self.ileCible = index break self.destination = (repere[0],repere[1]) trajectoireCalculer = self.planification.trouverTrajectoire(positionRobotTrouvee,self.destination) enMouvement = True rechargeActiver = False elif self.voltageRestant > 0 and possedeTresor == False and enMouvement == False \ and rechargeActiver == False and len(self.code) == 1 and auTresor == False: 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) retour = UDPSock.recv(buf) index = 0 while retour[index] != " ": positionXTresorText = positionXTresorText + retour[index] index += 1 index += 1 while index < len(retour) - 2: positionYTresorText = positionYTresorText + retour[index] index += 1 positionTresor.append(int(positionXTresorText)) positionTresor.append(int(positionYTresorText)) self.destination = (positionTresor[0],positionTresor[1]) trajectoireCalculer = self.planification.trouverTrajectoire(positionRobotTrouvee,self.destination) enMouvement = True auTresor = True if self.voltageRestant > 0 and possedeTresor == False and enMouvement == False \ and rechargeActiver == False and len(self.code) == 1 and auTresor == True: 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 = '4' + orientationRobotText + positionRobotXText + positionRobotYText + repereXText + repereYText + ratioText UDPSock.sendto(demande,addr) retour = UDPSock.recv(buf) index = 0 while retour[index] != " ": positionXTresorText = positionXTresorText + retour[index] index += 1 index += 1 while index < len(retour) - 2: positionYTresorText = positionYTresorText + retour[index] index += 1 positionTresor.append(int(positionXTresorText)) positionTresor.append(int(positionYTresorText)) self.destination = (listePositions[self.ileCible][0],listePositions[self.ileCible][1]) trajectoireCalculer = self.planification.trouverTrajectoire(positionRobotTrouvee,self.destination) possedeTresor = True enMouvement = True elif self.voltageRestant > 0 and possedeTresor == False and enMouvement == False and rechargeActiver == False \ and len(self.code) == 1 and auTresor == True and possedeTresor == True: deltaX = self.destination[0] - positionRobotTrouvee[0] deltaY = positionRobotTrouvee[1] - self.destination[1] angle = degrees(atan2(deltaY,deltaX)) angleRotation = angle - orientationRobotTrouvee if angleRotation < 0: angleRotation = abs(360 + angleRotation) variance += (angleRotation % 180)/180 angleText = str(int(angleRotation)) while len(angleText) <4: angleText = '0' + angleText distanceCalcule = np.sqrt(np.power((self.destination - positionRobotTrouvee[0]), 2) + np.power((self.destination[1] - positionRobotTrouvee[1]), 2)) * ratioCMPixel variance += distanceCalcule/40 distanceText = str(int(distanceCalcule)) while len(distanceText) < 3: distanceText = '0' + distanceText demande = '5' + angleText + distanceText UDPSock.sendto(demande,addr) time.sleep(15) retour = UDPSock.recv(buf) if retour != "": self.voltageRestant = int(retour[len(retour) - 3] + retour[len(retour) - 2] + retour[len(retour) - 1])