class PathfindingTest(TestCase): def setUp(self): self.map = MagicMock() self.limit = MagicMock() self.limit.getMinCorner.return_value = (100, 100) self.limit.getMaxCorner.return_value = (1000, 900) self.map.getMapLimit.return_value = self.limit blueSquare = MagicMock() blueSquare.findCenterOfMass.return_value = 370, 370 redSquare = MagicMock() redSquare.findCenterOfMass.return_value = 400, 400 obstacleList = [blueSquare, redSquare] self.map.getShapesList.return_value = obstacleList self.pathfinder = Pathfinder(self.map) def test_whenFindPathIsCalledThenReturnedPathIsNotGoingThroughtAnObstacle( self): goodpath = self.pathfinder.findPath((80, 80), (500, 500)) self.assertTrue(goodpath.__len__() > 2) def test_whenFindPathIsCalledThenReturnedPathIsTheShortest(self): goodpath = self.pathfinder.findPath((80, 80), (500, 500)) for compteur in range(0, self.pathfinder.goodPaths.__len__()): self.assertTrue(goodpath.totalDistance <= self.pathfinder.goodPaths[compteur].totalDistance)
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
class PathfindingTest(TestCase): def setUp(self): self.map = MagicMock() self.limit = MagicMock() self.limit.getMinCorner.return_value = (100,100) self.limit.getMaxCorner.return_value = (1000,900) self.map.getMapLimit.return_value = self.limit blueSquare = MagicMock() blueSquare.findCenterOfMass.return_value = 370,370 redSquare = MagicMock() redSquare.findCenterOfMass.return_value = 400,400 obstacleList = [blueSquare, redSquare] self.map.getShapesList.return_value = obstacleList self.pathfinder = Pathfinder(self.map) def test_whenFindPathIsCalledThenReturnedPathIsNotGoingThroughtAnObstacle(self): goodpath = self.pathfinder.findPath((80,80), (500,500)) self.assertTrue(goodpath.__len__() > 2) def test_whenFindPathIsCalledThenReturnedPathIsTheShortest(self): goodpath = self.pathfinder.findPath((80,80), (500,500)) for compteur in range(0, self.pathfinder.goodPaths.__len__()): self.assertTrue(goodpath.totalDistance <= self.pathfinder.goodPaths[compteur].totalDistance)
def initialiseWorldData(self): self.world.initializeRound() image, map = self.world.getCurrentImage() self.pathfinder = Pathfinder(map) mapCoordinatesAdjuster = MapCoordinatesAjuster(map) convertedPoint = mapCoordinatesAdjuster.convertPoint(map.robot.center) self.sequencer = seq(self.pathfinder) return convertedPoint, map.robot.orientation
def setUp(self): self.map = MagicMock() self.limit = MagicMock() self.limit.getMinCorner.return_value = (100, 100) self.limit.getMaxCorner.return_value = (1000, 900) self.map.getMapLimit.return_value = self.limit blueSquare = MagicMock() blueSquare.findCenterOfMass.return_value = 370, 370 redSquare = MagicMock() redSquare.findCenterOfMass.return_value = 400, 400 obstacleList = [blueSquare, redSquare] self.map.getShapesList.return_value = obstacleList self.pathfinder = Pathfinder(self.map)
def setUp(self): self.map = MagicMock() self.limit = MagicMock() self.limit.getMinCorner.return_value = (100,100) self.limit.getMaxCorner.return_value = (1000,900) self.map.getMapLimit.return_value = self.limit blueSquare = MagicMock() blueSquare.findCenterOfMass.return_value = 370,370 redSquare = MagicMock() redSquare.findCenterOfMass.return_value = 400,400 obstacleList = [blueSquare, redSquare] self.map.getShapesList.return_value = obstacleList self.pathfinder = Pathfinder(self.map)
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
class BaseStationDispatcher(): def __init__(self, worldVision): self.world = worldVision self.pathfinder = None self.path = None def initialiseWorldData(self): self.world.initializeRound() image, map = self.world.getCurrentImage() self.pathfinder = Pathfinder(map) mapCoordinatesAdjuster = MapCoordinatesAjuster(map) convertedPoint = mapCoordinatesAdjuster.convertPoint(map.robot.center) self.sequencer = seq(self.pathfinder) return convertedPoint, map.robot.orientation def handleCurrentSequencerState(self): image, map = self.world.getCurrentImage() self.path, signal, angleToRotateTo = self.sequencer.handleCurrentState(map) return self.path, signal, angleToRotateTo def getCurrentWorldInformation(self): image, map = self.world.getCurrentImage() if(self.pathfinder != None): self.pathfinder.drawPath(image) cv2.resize(image, (320,240)) convertedImage = cv2.imencode('.png',image)[1] base64ConvertedImage = base64.encodestring(convertedImage) mapCoordinatesAdjuster = MapCoordinatesAjuster(map) convertedPoint = mapCoordinatesAdjuster.convertPoint(map.robot.center) if map.target != None: informationToSend = {"robotPosition":convertedPoint, "robotOrientation":map.robot.orientation, "encodedImage":base64ConvertedImage, "targetColor":map.target.color.getName()} else: informationToSend = {"robotPosition":convertedPoint, "robotOrientation":map.robot.orientation, "encodedImage":base64ConvertedImage} return informationToSend def startFromBegining(self): self.sequencer.setState(SendingBotToChargingStationState()) def startFromTreasure(self): self.sequencer.setState(DetectTreasureState()) def startFromTarget(self): self.sequencer.setState(SendingBotToTargetState()) def setTargetOnMap(self, jsonTarget): targetFactory = TargetFactory() target = targetFactory.constructTarget(jsonTarget) self.world.setTarget(target) def setTreasuresOnMap(self, data): self.world.setTreasures(data) def setTimer(self, function,seconds): if self.timer != None: self.timer.cancel() def func_wrapper(): self.setTimer(function, seconds) function() self.timer = threading.Timer(seconds, func_wrapper) self.timer.start() return self.timer #debug section def setSequencerStateToSendChargingStation(self): self.sequencer.setState(SendingBotToChargingStationState()) def setSequencerStateToDetectTreasures(self): self.sequencer.setState(DetectTreasureState()) def setSequencerStateToSendToTreasure(self): self.sequencer.setState(SendingBotToTreasureState()) def setSequencerStateToSendToTarget(self): self.sequencer.setState(SendingBotToTargetState())