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