def test__CalculatePathToDestination_UniformGrid(self):
Maze = MazeFacade(DIYMazeParser(8))
Provider = UnifiedWorldImageProvider(maze=Maze, config=DictionaryConfigProvider())
weights = {NodeStateEnum.Clear: 2,
NodeStateEnum.Obs: np.inf,
NodeStateEnum.UnExplored: 1,
NodeStateEnum.Ant: np.inf}
Planner = AntPathPlanner(safetyRadius=2, cellTypeWeights=weights, stabilityFactor=1)
StartPosition = Position(0, 0)
mazeMatrix = Maze.GetMatrix()
manuallyAdjustedMaze = np.where(mazeMatrix == 1, NodeStateEnum.UnExplored, NodeStateEnum.Clear)
manuallyAdjustedMaze[0, 0] = NodeStateEnum.Clear
singleAntWorldImage = SimpleSingleAntWorldImage(manuallyAdjustedMaze, {})
weightedMatrix = Planner._AntPathPlanner__ConvertWorldImageToWeightedMatrix(StartPosition, singleAntWorldImage)
pathMatrix = Dijkstra(weightedMatrix, StartPosition)
resultRoute = Planner._AntPathPlanner__CalculatePathToDestination(StartPosition, Position(4, 4), pathMatrix)
weightSum = 0
for position in resultRoute:
weightSum += weightedMatrix[position.X][position.Y]
self.assertEqual(pathMatrix[position.X][position.Y], weightSum)
def printRoute(route):
for position in route:
print(f'[{position.X}, {position.Y}] ')
def test_UniformGrid(self):
InputGrid = np.full((5, 5), 1)
ResultGrid = Dijkstra(InputGrid, Position(0, 0))
for i in range(5):
for j in range(5):
self.assertTrue(ResultGrid[i, j] == i + j)
def test_BlockedGrid(self):
InputGrid = np.full((5, 5), 1.0)
InputGrid[2:4, 2:4] = np.inf
ResultGrid = Dijkstra(InputGrid, Position(0, 0))
self.assertTrue(
np.all(np.equal(ResultGrid[2:4, 2:4], np.full((2, 2), np.inf))),
str(ResultGrid[2:4, 2:4]))
def test_UnreachableNodeGrid(self):
InputGrid = np.full((5, 5), 1.0)
InputGrid[2:4, 2:4] = np.inf
InputGrid[4, 4] = 1.0
ResultGrid = Dijkstra(InputGrid, Position(0, 0))
self.assertTrue(
np.all(np.equal(ResultGrid[2:4, 2:4], np.full((2, 2), np.inf))),
str(ResultGrid[2:4, 2:4]))
def test_NonUniformGrid(self):
InputGrid = np.zeros((5, 5))
for i in range(5):
for j in range(5):
InputGrid[i, j] = float(i + j)
ResultGrid = Dijkstra(InputGrid, Position(0, 0))
for j in range(5):
for i in range(5):
self.assertTrue(ResultGrid[i, j] == ((i + j) * (i + j + 1.0)) /
2.0)
def PlanPath(self, worldImage: BaseSingleAntWorldImage, startingPosition: Position) :
WeightedMatrix = self.__ConvertWorldImageToWeightedMatrix(startingPosition, worldImage)
PriceMatrix = Dijkstra(WeightedMatrix, startingPosition)
self.__CurrentDestinationPrice = PriceMatrix[
self.__CurrentDestinationPosition.Y, self.__CurrentDestinationPosition.X]
self.__CurrentDestinationPosition, self.__CurrentDestinationPrice = self.__SelectDestination(PriceMatrix,
WeightedMatrix,
startingPosition)
# calculate a path to the destination
if startingPosition == self.__CurrentDestinationPosition:
return startingPosition, {}
Path = self.__CalculatePathToDestination(startingPosition, self.__CurrentDestinationPosition, PriceMatrix)
return Path[0], {}
def test_LongPathGrid(self):
InputGrid = np.full((5, 5), 1.0)
InputGrid[1:4, 0:4] = np.inf
ResultGrid = Dijkstra(InputGrid, Position(0, 0))
print(InputGrid)
self.assertEqual(ResultGrid[4, 0], 12.0, str(ResultGrid))