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])