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