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 __map_2_occupancy_map(self, filename):
map_planning = np.loadtxt(filename, dtype="str")
enterindexs = np.where(map_planning == 'S')
if len(enterindexs[0]) == 0:
raise Exception(format(f"file {filename} no enterence found"))
if len(enterindexs[0]) > 1:
raise Exception(
format(f"file {filename} several enterences found"))
self.__Enterence = Position(enterindexs[1][0], enterindexs[0][0])
exitindexs = np.where(map_planning == 'F')
if len(exitindexs[0]) == 0:
raise Exception(format(f"file {filename} no exits found"))
exittupels = list(zip(exitindexs[0], exitindexs[1]))
for tup in exittupels:
self.__Exits.append(Position(tup[1], tup[0]))
''' Takes the matrix and converts it into a float array '''
map_arr = np.copy(map_planning)
map_arr[map_arr == 'O'] = 0
map_arr[map_arr == 'E'] = 1
map_arr[map_arr == 'S'] = 1
map_arr[map_arr == 'F'] = 1
self.__Maze = map_arr.astype(np.int)
def __init__(self):
self.__Maze = np.ones((5, 5))
self.__Maze[0][1] = 0
self.__Maze[2][1] = 0
self.__Maze[0][3] = 0
self.__Maze[2][3] = 0
self.__Enterence = Position(0, 0)
self.__Exits = [Position(4, 4), Position(4, 2)]
def test_GetAntWorldImage_NoObs_Center_2(self):
mazeparser = DIYMazeParser(5)
mazeparser.SetExits([Position(4, 4)])
mazefacade = MazeFacade(mazeparser)
WorldImageProvider = SimpleWorldImageProvider(self.__Config,
mazefacade)
ant = RemoteControlledAnt(1, self.__Config)
ant.UpdatePosition(position=Position(2, 1))
antworldimage = WorldImageProvider.GetAntWorldImage(ant)
self.assertTrue(len(antworldimage.VisibleNodes) == 20)
def added_ants(self, num_of_ants_produced, world_image):
ants_to_add = []
if not num_of_ants_produced >= self.__max_num_of_ants:
startingPosition = Position(x=self.__InitialPosition.X,
y=self.__InitialPosition.Y)
for id in [num_of_ants_produced]:
ant = SimpleRandomMemoryLessAnt(id, self._Config)
ant.UpdatePosition(position=Position(
x=self.__InitialPosition.X, y=self.__InitialPosition.Y))
ants_to_add.append(ant)
return ants_to_add
def test_isInRange_Method(self):
position1 = Position(3, 5)
position2 = Position(8, 10)
radius = 2
in_radius = isInRadius(radius, position1, position2)
fake_maze = FakeMazeCreator(12)
is_los = isLOS(fake_maze, position1, position2)
in_range = isInRange(fake_maze, radius, position1, position2)
self.assertTrue(not in_radius)
self.assertTrue(not is_los)
self.assertTrue(not in_range)
def GetNeighbours(Source: Position, Width: int, Height: int) -> list:
result = []
if Source.X > 0:
result.append(Position(Source.X - 1, Source.Y))
if Source.Y > 0:
result.append(Position(Source.X, Source.Y - 1))
if Source.X < Width - 1:
result.append(Position(Source.X + 1, Source.Y))
if Source.Y < Height - 1:
result.append(Position(Source.X, Source.Y + 1))
return result
def added_ants(self, num_of_ants_produced, world_image):
ants_to_add = []
ids = self.__server_comm.GetAntsIds()
startingPosition = Position(x=self.__InitialPosition.X,
y=self.__InitialPosition.Y)
for id in ids:
if next(
(x for x in self.__AntsList if x.ID == int(id)), None) is None:
ant = AlgoAnt(id, self._Config, startingPosition)
ant.UpdatePosition(position=Position(
x=self.__InitialPosition.X, y=self.__InitialPosition.Y))
ants_to_add.append(ant)
self.__AntsList.append(ant)
return ants_to_add
def test_UpdateStep_SeeObs(self):
mazeparser = DIYMazeParser(5)
mazeparser.SetExits([Position(4, 4)])
mazeparser.SetObs([Position(3, 3)])
mazefacade = MazeFacade(mazeparser)
WorldImageProvider = SimpleWorldImageProvider(self.__Config,
mazefacade)
ant = RemoteControlledAnt(1, self.__Config)
ant.UpdatePosition(position=Position(4, 4))
WorldImageProvider.ProcessStep(ant, AntStep(1, Position(4, 4)))
WorldImageProvider.UpdatePositionsAccordingToMoves()
antworldimage = WorldImageProvider.GetAntWorldImage(ant)
obsList = list(
filter(lambda x: x.NodeState == NodeStateEnum.Obs,
antworldimage.VisibleNodes))
self.assertTrue(len(obsList) == 1)
def test__ConvertWorldImageToWeightedMatrix_EmptyGrid(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, {})
result = Planner._AntPathPlanner__ConvertWorldImageToWeightedMatrix(StartPosition, singleAntWorldImage)
Width, Height = result.shape
for i in range(0, Width):
for j in range(0, Height):
if i == 0 and j == 0:
self.assertEqual(result[i][j], weights[NodeStateEnum.Clear])
else:
self.assertEqual(result[i][j], weights[NodeStateEnum.UnExplored])
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 CreateAnts(self):
ids=range(self.__number_to_produce)
for id in ids:
ant=mesh_ant(str(id),self.__maze,self.__connectivty_calculator,self._Config)
ant.UpdatePosition(position=Position(x=self.__InitialPosition.X,y=self.__InitialPosition.Y))
self.__AntsList.append(ant)
def CreateAnts(self):
ids=self.__server_comm.GetAntsIds()
for id in ids:
ant=mesh_ant(str(id),self.__maze,self.__connectivty_calculator,self._Config)
ant.UpdatePosition(position=Position(x=self.__InitialPosition.X,y=self.__InitialPosition.Y))
self.__AntsList.append(ant)
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 ProcessAntStep(self, step, ant: BasicAnt,
antworldimage: BaseSingleAntWorldImage, move,
aditionaldata):
if (ant.ID == 0):
return
if (ant.CurrentPosition.X == 0):
return
if (ant.CurrentPosition.Y == 0):
return
msg = TM()
msg.id = int(ant.ID)
msg.battery = int(100)
msg.x = ant.CurrentPosition.X
msg.y = ant.CurrentPosition.Y
msg.angle = int(0)
pos_ll = Position(ant.CurrentPosition.X - 1, ant.CurrentPosition.Y)
pos_ul = Position(ant.CurrentPosition.X, ant.CurrentPosition.Y - 1)
pos_rl = Position(ant.CurrentPosition.X + 1, ant.CurrentPosition.Y)
pos_bl = Position(ant.CurrentPosition.X, ant.CurrentPosition.Y + 1)
msg.ul = 'open'
msg.ll = 'open'
msg.rl = 'open'
msg.bl = 'open'
if ant.Type() == AntType.Transmission:
msg.type = self._TransAntType
else:
msg.type = self._ScoutAntType
for node in self.__interperter.Interpert(antworldimage.VisibleNodes):
if (node.Position == pos_ll):
msg.ll = self._GetNodeStr(node.NodeState)
if (node.Position == pos_ul):
msg.ul = self._GetNodeStr(node.NodeState)
if (node.Position == pos_rl):
msg.rl = self._GetNodeStr(node.NodeState)
if (node.Position == pos_bl):
msg.bl = self._GetNodeStr(node.NodeState)
serialized = msg.SerializeToString(msg)
# Create a socket (SOCK_STREAM means a TCP socket)
self._socket.sendto(serialized, (self._MulticastAddr, self._Port))
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 CreateAnts(self):
ids = self.__server_comm.GetAntsIds()
for id in ids:
ant = SimpleRandomMemoryLessAnt(str(id), self._Config)
ant.UpdatePosition(position=Position(x=self.__InitialPosition.X,
y=self.__InitialPosition.Y))
self.__AntsList.append(ant)
def CreateAnts(self):
ids = self.__server_comm.GetAntsIds()
for id in ids:
startingPosition = Position(x=self.__InitialPosition.X,
y=self.__InitialPosition.Y)
ant = AlgoAnt(id, self._Config, startingPosition)
self.__AntsList.append(ant)
def CreateAnts(self):
numbertoproduce = int(
self._Config.GetConfigValueForSectionAndKey(
"AlgoAnt", "NumToProduce", 10))
for id in range(numbertoproduce):
startingPosition = Position(x=self.__InitialPosition.X,
y=self.__InitialPosition.Y)
ant = AlgoAnt(id, self._Config, startingPosition)
self.__AntsList.append(ant)
def Interpert(self, antworldstate) -> List[NodeState]:
nodes=[]
size_of_world_image=np.shape(antworldstate)
for x in range(size_of_world_image[0]):
for y in range(size_of_world_image[1]):
state=(NodeStateEnum)(antworldstate[y][x])
if(state!=NodeStateEnum.UnExplored):
nodes.append(NodeState(state,Position(x,y)))
return nodes
def test_SingleScout(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}
ant = AlgoAnt(id=1, config=DictionaryConfigProvider(), position=Position(3, 3))
Provider.ProcessStep(ant, AntStep(ant.ID, ant.CurrentPosition))
Provider.UpdatePositionsAccordingToMoves()
Planner = AntPathPlanner(safetyRadius=0, cellTypeWeights=weights)
result = Planner._AntPathPlanner__ConvertWorldImageToWeightedMatrix(Position(0, 0),
Provider.GetAntWorldImage(ant))
print(result)
def CreateAnts(self):
numbertoproduce = int(
self._Config.GetConfigValueForSectionAndKey(
"SimpleAnt", "NumToProduce", 100))
for id in range(numbertoproduce):
ant = SimpleRandomMemoryLessAnt(id, self._Config)
ant.UpdatePosition(position=Position(x=self.__InitialPosition.X,
y=self.__InitialPosition.Y))
self.__AntsList.append(ant)
def added_ants(self, num_of_ants_produced, world_image):
ants_to_add = []
ids = self.__server_comm.GetAntsIds()
for id in ids:
if next((x for x in self.__AntsList if x.ID == int(id)), None) is None:
ant = mesh_ant(id,self.__maze,self.__connectivty_calculator,self._Config)
ant.UpdatePosition(position=Position(x=self.__InitialPosition.X, y=self.__InitialPosition.Y))
ants_to_add.append(ant)
self.__AntsList.append(ant)
return ants_to_add
def test__y_negetive_south_facing_axis_movment(self):
config = DictionaryConfigProvider()
config.SetValue('InitialPosition', 'InitialDirection', 1)
config.SetValue('InitialPosition', 'InitialPosition_x', 1)
config.SetValue('InitialPosition', 'InitialPosition_y', 1)
processor = AntStepProcesser(config)
processor.process_ant_step(99, StepEnum.NoStep)
translate = TranslateStep(config, processor)
step = AntStep(99, Position(1, 0))
movment = translate.TranlateStep(step)
self.assertTrue(movment[0] == StepEnum.Back)
def _internalGetStep(self, antworldstate: BaseSingleAntWorldImage):
# try to keep this direction
targetPosition = Position(self.CurrentPosition.X + self.__Direction_X,
self.CurrentPosition.Y + self.__Direction_Y)
for node in antworldstate.VisibleNodes:
if node.Position == targetPosition and node.NodeState != NodeStateEnum.Obs:
return targetPosition
self.__Direction_X = random.choice([1, -1])
self.__Direction_Y = random.choice([1, -1])
targetPosition = Position(self.CurrentPosition.X + self.__Direction_X,
self.CurrentPosition.Y + self.__Direction_Y)
for node in antworldstate.VisibleNodes:
if node.Position == targetPosition and node.NodeState != NodeStateEnum.Obs:
return targetPosition
return self.CurrentPosition, {}
def __CreateCandidateDestinationsList(self, PriceMatrix, WeightedMatrix):
CandidateDestionationsPrices = {}
[height, width] = WeightedMatrix.shape
# TODO use np.where, np.array.toList instead of iteration
for pos_x in range(0, width):
for pos_y in range(0, height):
if (PriceMatrix[pos_y][pos_x] != np.inf) and (
WeightedMatrix[pos_y][pos_x] == self.__CellWeights[NodeStateEnum.UnExplored]):
CandidateDestionationsPrices[Position(pos_x, pos_y)] = (PriceMatrix[pos_y][pos_x])
return CandidateDestionationsPrices
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 added_ants(self, num_of_ants_produced, world_image):
ants_to_add = []
# make sure we always have enough scout ants
if not self._CountAntType(AntType.Scout) >= self.__max_num_of_ants:
startingPosition = Position(x=self.__InitialPosition.X,
y=self.__InitialPosition.Y)
for id in [num_of_ants_produced]:
ant = AlgoAnt(id, self._Config, startingPosition)
ants_to_add.append(ant)
return ants_to_add
def test__x_positive_east_facing_axis_movment(self):
config = DictionaryConfigProvider()
config.SetValue('InitialPosition', 'InitialDirection', 2)
config.SetValue('InitialPosition', 'InitialPosition_x', 1)
config.SetValue('InitialPosition', 'InitialPosition_y', 1)
processor = AntStepProcesser(config)
processor.process_ant_step(99, StepEnum.NoStep)
translate = TranslateStep(config, processor)
step = AntStep(99, Position(2, 1))
movment = translate.TranlateStep(step)
self.assertTrue(len(movment) == 1)
self.assertTrue(movment[0] == StepEnum.Forward)
def Listner(self, socket, queue, event):
Commands = {}
Commands['MarkNotRelevant'] = AlgCommandEnum.NotRelevent
Commands['MarkClear'] = AlgCommandEnum.Clear
Commands['MarkBlocked'] = AlgCommandEnum.Blocked
Commands['MarkPriority'] = AlgCommandEnum.Priority
while not event.is_set():
data = socket.recv(1024)
msg = CM()
msg.ParseFromString(data)
command = AlgExternalCommand(position=Position(msg.x, msg.y),
command=Commands[msg.action])
queue.put(command)