Пример #1
0
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])
Пример #2
0
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))
Пример #3
0
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)
Пример #4
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)
Пример #5
0
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])