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