Exemple #1
0
 def __init__(self):
     self.__shapes = []
     self.limit = Square("limit", np.array([[]], dtype=np.int32))
     self.robot = Robot(Shape("robot", np.array([[]], dtype=np.int32)), Shape("orientation", np.array([[]], dtype=np.int32)))
     self.target = None
     self.treasures = []
     self.orientationForTreasure = 0
Exemple #2
0
    def setMapLimit(self, contour):
        cornerList = []
        minX = 0
        maxX = 960
        minY = 105
        maxY = 587

#table 1 : (0,110), (960,590)

#table 2 : (0,109), (960,597)

#table 3 : (0,105), (960,587)

#table 5 : (0,125), (960,605)

#table 6 : (0,92), (960,580)

        newFoundLimit = Square("limit", np.array([[[minX,minY + 5]],[[minX,maxY - 5]],[[maxX, maxY - 5]],[[maxX,minY + 5]]], dtype=np.int32))

        if newFoundLimit.getArea() < self.limit.getArea() or len(self.limit.getContour()) == 0:
            self.limit = newFoundLimit
Exemple #3
0
class Map:

    SAFE_MARGIN = 110
    SAFE_MARGIN_FOR_TREASURE = 100

    def __init__(self):
        self.__shapes = []
        self.limit = Square("limit", np.array([[]], dtype=np.int32))
        self.robot = Robot(Shape("robot", np.array([[]], dtype=np.int32)), Shape("orientation", np.array([[]], dtype=np.int32)))
        self.target = None
        self.treasures = []
        self.orientationForTreasure = 0

    def getShapesList(self):
        return self.__shapes

    def getContourList(self):
        contourList = []
        for shape in self.__shapes:
            contourList.append(shape.getContour())
        return contourList

    def getMapLimit(self):
        return self.limit

    def getAverageShapeSize(self):
        averageSize = 0
        if len(self.__shapes) > 0:
            for shape in self.__shapes:
                averageSize += shape.getArea()
            averageSize = averageSize/len(self.__shapes)
        return averageSize

    def getEdgeGradiant(self, edge):
        if float(edge[1].item(0) - edge[0].item(0)) != 0 and float(edge[1].item(1) - edge[0].item(1)) != 0:
            edgePerpendicularGradient = float(-1 / (float(float(edge[1].item(1) - edge[0].item(1)) / float(edge[1].item(0) - edge[0].item(0)))))
        elif float(edge[1].item(0) - edge[0].item(0)) == 0:
            edgePerpendicularGradient = float(-1 / (float(float(edge[1].item(1) - edge[0].item(1)) / 0.0001)))
        else:
            edgePerpendicularGradient = float(-1 / 0.00001 / float(edge[1].item(0) - edge[0].item(0)))

        return edgePerpendicularGradient

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



    def setMapLimit(self, contour):
        cornerList = []
        minX = 0
        maxX = 960
        minY = 105
        maxY = 587

#table 1 : (0,110), (960,590)

#table 2 : (0,109), (960,597)

#table 3 : (0,105), (960,587)

#table 5 : (0,125), (960,605)

#table 6 : (0,92), (960,580)

        newFoundLimit = Square("limit", np.array([[[minX,minY + 5]],[[minX,maxY - 5]],[[maxX, maxY - 5]],[[maxX,minY + 5]]], dtype=np.int32))

        if newFoundLimit.getArea() < self.limit.getArea() or len(self.limit.getContour()) == 0:
            self.limit = newFoundLimit

    def setShapesColor(self, frame):
        for shape in self.__shapes:
            shape.findColor(copy.copy(frame))

    def setTarget(self, target):
        self.target =  target.getObstacle(self.__shapes)
        print "Target = ", self.target.getName()

    def setTreasures(self, relativeAngles):
        rightAngle = 90
        yDistanceFromPurpleCircle = 25
        cameraDistanceFromBackgroundWall = self.robot.purpleCircle.findCenterOfMass()[0] - 20 - self.limit.getMinCorner()[0]
        cameraDistanceFromLowerWall = self.limit.getMaxCorner()[1] - self.robot.purpleCircle.findCenterOfMass()[1] + 20
        cameraDistanceFromUpperWall = self.robot.purpleCircle.findCenterOfMass()[1] + 105 - self.limit.getMinCorner()[1]
        for cameraAngle in relativeAngles:

            lowerWall = True
            if cameraAngle < rightAngle:
                xDistanceOfTreasureFromCamera = math.tan(math.radians(cameraAngle))*cameraDistanceFromLowerWall
                treasurePosition = (cameraDistanceFromBackgroundWall - xDistanceOfTreasureFromCamera, self.limit.getMaxCorner()[1])
            else:
                lowerWall = False
                cameraAngle = 180 - cameraAngle
                xDistanceOfTreasureFromCamera = math.tan(math.radians(cameraAngle))*cameraDistanceFromUpperWall
                treasurePosition = (cameraDistanceFromBackgroundWall - xDistanceOfTreasureFromCamera, self.limit.getMinCorner()[1])

            treasureDistanceFromBackground = cameraDistanceFromBackgroundWall - xDistanceOfTreasureFromCamera
            if treasureDistanceFromBackground < 0:
                yDistanceFromCamera = (-1 * (treasureDistanceFromBackground)) / math.tan(math.radians(cameraAngle))
                if lowerWall:
                    treasurePosition = (self.limit.getMinCorner()[0], self.limit.getMaxCorner()[1] - yDistanceFromCamera)
                else:
                    treasurePosition = (self.limit.getMinCorner()[0], self.limit.getMinCorner()[1] + yDistanceFromCamera)

            print "Tresor ajoute ", treasurePosition
            self.treasures.append(treasurePosition)

    def findSimilarShape(self, newPossibleShape):

        newContourCenterOfMassX, newContourCenterOfMassY = newPossibleShape.findCenterOfMass()
        for shapeAlreadyFound in self.__shapes:
            oldContourCenterOfMassX, oldContourCenterOfMassY = shapeAlreadyFound.findCenterOfMass()
            if(abs(newContourCenterOfMassX - oldContourCenterOfMassX) < 20 and abs(newContourCenterOfMassY - oldContourCenterOfMassY) < 20):
                return shapeAlreadyFound
        return None

    def addShape(self, shapeToAdd):

        if abs(shapeToAdd.getArea() - self.getAverageShapeSize()) < 1100 or len(self.__shapes) < 3:
            similarShape = self.findSimilarShape(shapeToAdd)
            if similarShape != None:
                if similarShape.getArea() < shapeToAdd.getArea() and similarShape.getName() == shapeToAdd.getName():
                    self.__shapes.remove(similarShape)
                    self.__shapes.append(shapeToAdd)
            if similarShape == None:
                self.__shapes.append(shapeToAdd)

    def deleteBlackShapes(self):
        for shape in self.__shapes:
            if shape.getColorName() == "Black":
                self.__shapes.remove(shape)

    def deleteOutsiderShapes(self):
        shapesToDelete = []
        for shape in self.__shapes:
            if shape.isOutsideLimit(self.limit):
                shapesToDelete.append(shape)

        for shape in shapesToDelete:
            self.__shapes.remove(shape)

    def filterRobot(self):
        shapes = self.__shapes
        for shape in shapes:
            if(shape.color.colorName == "Black" or shape.color.colorName == "Purple"):
                self.__shapes.remove(shape)