def getPositionInFrontOfTreasure(self): myPathFinder = Pathfinder(self) myMapCoorDinateAjuster = MapCoordinatesAjuster(self) myBestPath = Path() myBestPath.totalDistance = 99999 bestInFrontPosition = (0,0) bestOrientationForTreasure = 0 for treasurePosition in self.treasures: if treasurePosition[1] == self.limit.getMaxCorner()[1]: newOrientationForTreasure = 90 newInFrontPosition = (treasurePosition[0], treasurePosition[1] - self.SAFE_MARGIN_FOR_TREASURE) elif treasurePosition[1] == self.limit.getMinCorner()[1]: newOrientationForTreasure = 270 newInFrontPosition = (treasurePosition[0], treasurePosition[1] + self.SAFE_MARGIN_FOR_TREASURE) else: newOrientationForTreasure = 180 newInFrontPosition = (treasurePosition[0] + self.SAFE_MARGIN_FOR_TREASURE, treasurePosition[1]) print newInFrontPosition myNewPath = myPathFinder.findPath(myMapCoorDinateAjuster.convertPoint((self.robot.center)), myMapCoorDinateAjuster.convertPoint(newInFrontPosition)) if myNewPath != False: if myNewPath.totalDistance < myBestPath.totalDistance: myBestPath = myNewPath bestOrientationForTreasure = newOrientationForTreasure bestInFrontPosition = newInFrontPosition return bestInFrontPosition,bestOrientationForTreasure
def findPath(self, positionRobot, pointToMoveTo): print( "@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@" ) goodPositionRobot = self.findGoodPoint(positionRobot) startingPathNode = self.graph.findGoodSafeNodeToGo(goodPositionRobot) endingPathNode = self.graph.findGoodSafeNodeToGo(pointToMoveTo) print pointToMoveTo, endingPathNode.positionX, endingPathNode.positionY self.theGoodPath = Path() self.pathsList = [] self.goodPaths = [] path = Path() path.append(Node(positionRobot)) path.append(startingPathNode) self.pathsList.append(path) self.__findAllPaths(path, endingPathNode) goodPath = Path() goodPath.append(Node((0, 0))) goodPath.totalDistance = 99999 for compteur in range(0, self.goodPaths.__len__()): currentPath = self.goodPaths[compteur] currentPath.append(Node(pointToMoveTo)) self.__polishGoodPaths() self.lineOfSightCalculator.tryStraightLine(self.goodPaths) for compteur in range(0, self.goodPaths.__len__()): currentPath = self.goodPaths[compteur] currentPath.ajustDistance() if currentPath.totalDistance < goodPath.totalDistance: goodPath = currentPath self.printPath(goodPath) if goodPath.totalDistance == 99999: self.__displayPathfinder(goodPath, positionRobot) return False self.theGoodPath = goodPath self.__displayPathfinder(goodPath, positionRobot) return goodPath
def findPath(self, positionRobot, pointToMoveTo): print("@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@") goodPositionRobot = self.findGoodPoint(positionRobot) startingPathNode = self.graph.findGoodSafeNodeToGo(goodPositionRobot) endingPathNode = self.graph.findGoodSafeNodeToGo(pointToMoveTo) print pointToMoveTo, endingPathNode.positionX, endingPathNode.positionY self.theGoodPath = Path() self.pathsList = [] self.goodPaths = [] path = Path() path.append(Node(positionRobot)) path.append(startingPathNode) self.pathsList.append(path) self.__findAllPaths(path, endingPathNode) goodPath = Path() goodPath.append(Node((0,0))) goodPath.totalDistance = 99999 for compteur in range(0, self.goodPaths.__len__()): currentPath = self.goodPaths[compteur] currentPath.append(Node(pointToMoveTo)) self.__polishGoodPaths() self.lineOfSightCalculator.tryStraightLine(self.goodPaths) for compteur in range(0, self.goodPaths.__len__()): currentPath= self.goodPaths[compteur] currentPath.ajustDistance() if currentPath.totalDistance < goodPath.totalDistance: goodPath = currentPath self.printPath(goodPath) if goodPath.totalDistance == 99999: self.__displayPathfinder(goodPath, positionRobot) return False self.theGoodPath = goodPath self.__displayPathfinder(goodPath, positionRobot) return goodPath
def getPositionInFrontOfIsland(self): myPathFinder = Pathfinder(self) fakePath = Path() fakePath.totalDistance = 99999 myPath = fakePath myMapCoorDinateAjuster = MapCoordinatesAjuster(self) myBestPosition = (0,0) orientation = 0 targetShape = self.target edgesList = self.target.getEdgesList() for edge in edgesList: xCenterOfEdge = edge[0].item(0) + (((edge[0].item(0) - edge[1].item(0)) / 2) * -1) yCenterOfEdge = edge[0].item(1) + (((edge[0].item(1) - edge[1].item(1)) / 2) * -1) edgePerpendicularGradient = self.getEdgeGradiant(edge) conversionGradient = 1 if abs(edgePerpendicularGradient) > 1: conversionGradient = 0.1 if abs(edgePerpendicularGradient) > 10: conversionGradient = 0.01 if self.target.isOutside((xCenterOfEdge + 1 * conversionGradient, yCenterOfEdge + 1 * edgePerpendicularGradient * conversionGradient)): positionToGo = (xCenterOfEdge + self.SAFE_MARGIN * conversionGradient, yCenterOfEdge + self.SAFE_MARGIN * edgePerpendicularGradient * conversionGradient) hypothenuse = 0 while hypothenuse < self.SAFE_MARGIN: positionToGo = (positionToGo[0] + 1, positionToGo[1] + edgePerpendicularGradient) opp = abs(yCenterOfEdge - positionToGo[1]) adj = abs(xCenterOfEdge - positionToGo[0]) hypothenuse = math.sqrt((opp * opp) + (adj * adj)) else: positionToGo = (xCenterOfEdge - self.SAFE_MARGIN * conversionGradient, yCenterOfEdge - self.SAFE_MARGIN * edgePerpendicularGradient * conversionGradient) opp = abs(yCenterOfEdge - positionToGo[1]) adj = abs(xCenterOfEdge - positionToGo[0]) hypothenuse = math.sqrt((opp * opp) + (adj * adj)) while hypothenuse < self.SAFE_MARGIN: positionToGo = (positionToGo[0] - 1* conversionGradient, positionToGo[1] - edgePerpendicularGradient* conversionGradient) opp = abs(yCenterOfEdge - positionToGo[1]) adj = abs(xCenterOfEdge - positionToGo[0]) hypothenuse = math.sqrt((opp * opp) + (adj * adj)) angle = math.degrees(math.atan2(opp,adj)) if positionToGo[0] > xCenterOfEdge and positionToGo[1] < yCenterOfEdge: angle = 180 - angle if positionToGo[0] > xCenterOfEdge and positionToGo[1] > yCenterOfEdge: angle = angle + 180 if positionToGo[0] < xCenterOfEdge and positionToGo[1] > yCenterOfEdge: angle = 360 - angle closePoint = myPathFinder.findClosePoint(myMapCoorDinateAjuster.convertPoint(positionToGo)) myNewPath = myPathFinder.findPath(myMapCoorDinateAjuster.convertPoint((self.robot.center)), closePoint) if myNewPath != False: if myNewPath.totalDistance < myPath.totalDistance: myPath = myNewPath myBestPosition = positionToGo orientation = angle if myBestPosition == (0, 0): print "aucun angle parallele" hypothenuse = 100 xCenterOfMass, yCenterOfMass = targetShape.findCenterOfMass() for angle in range (0, 90, 10): xValue = hypothenuse * math.cos(math.radians(angle)) yValue = hypothenuse * math.sin(math.radians(angle)) positionToGo = (xCenterOfMass - xValue, yCenterOfMass - yValue) myNewPath = myPathFinder.findPath(myMapCoorDinateAjuster.convertPoint((self.robot.center)), myMapCoorDinateAjuster.convertPoint(positionToGo)) if not isinstance(myNewPath, bool): myBestPosition = positionToGo orientation = angle myPath = myNewPath break if myPath.totalDistance == 99999: for angle in range (0, 90, 10): xValue = hypothenuse * math.sin(math.radians(angle)) yValue = hypothenuse * math.cos(math.radians(angle)) positionToGo = (xCenterOfMass + xValue, yCenterOfMass - yValue) myNewPath = myPathFinder.findPath(myMapCoorDinateAjuster.convertPoint((self.robot.center)), myMapCoorDinateAjuster.convertPoint(positionToGo)) if not isinstance(myNewPath, bool): myBestPosition = positionToGo orientation = angle + 90 myPath = myNewPath break if myPath.totalDistance == 99999: for angle in range (0, 91, 10): yValue = hypothenuse * math.sin(math.radians(angle)) xValue = hypothenuse * math.cos(math.radians(angle)) positionToGo = (xCenterOfMass + xValue, yCenterOfMass + yValue) myNewPath = myPathFinder.findPath(myMapCoorDinateAjuster.convertPoint((self.robot.center)), myMapCoorDinateAjuster.convertPoint(positionToGo)) if not isinstance(myNewPath, bool): myBestPosition = positionToGo orientation = angle + 180 myPath = myNewPath break if myPath.totalDistance == 99999: for angle in range (0, 91, 10): xValue = hypothenuse * math.sin(math.radians(angle)) yValue = hypothenuse * math.cos(math.radians(angle)) positionToGo = (xCenterOfMass - xValue, yCenterOfMass + yValue) myNewPath = myPathFinder.findPath(myMapCoorDinateAjuster.convertPoint((self.robot.center)), myMapCoorDinateAjuster.convertPoint(positionToGo)) if not isinstance(myNewPath, bool): myBestPosition = positionToGo orientation = angle + 270 myPath = myNewPath break print "MEILLEUR ",myBestPosition, ", Orientation :", orientation return myPath, orientation