def __init__(self):
self.cell_matrix = [[None for y in range(MAP_ROW)] for x in range(MAP_COL)] #initialise the matrix
for y in range(MAP_ROW): #create Cell objects for each cell in the matrix
for x in range(MAP_COL):
coord = Coord(x, y)
self.cell_matrix[coord.get_x()][coord.get_y()] = Cell(coord)
self.mark_border_cells_dangerous()
def orientation_to_unit_displacement(orientation: Orientation) -> Coord:
if orientation == Orientation.EAST: return Coord(1, 0)
elif orientation == Orientation.WEST: return Coord(-1, 0)
elif orientation == Orientation.NORTH: return Coord(0, 1)
elif orientation == Orientation.SOUTH: return Coord(0, -1)
else:
raise Exception(
f'Invalid orientation: tried moving {orientation.name}. orientation_to_unit_displacement only implemented for cardinal directions'
)
def calculate_rwh_vantage(self, ue: Coord) -> list:
# vantage points are cells where robot will stand next to an obstacle
rwh_vantage_points = []
for disp in [(-2,0), (0,2), (0,-2), (2,0)]:
coord = Coord(disp[0], disp[1])
rwh_vantage_points.append(ue.add(coord))
return rwh_vantage_points
def calculate_vantage_points(self, ue: Coord) -> list:
# vantage points are cells where robot can see unexplored cells
vantage_points_matrix = []
for i in range(SURE_VIEW_RANGE):
vantage_points = []
for disp in [(0,i+2), (i+2,0), (0,-i-2), (-i-2,0), (-1,i+2), (1,i+2), (i+2,1), (i+2,-1), (1,-i-2), (-1,-i-2), (-i-2,-1), (-i-2,1)]:
coord = Coord(disp[0], disp[1])
vantage_points.append(ue.add(coord))
vantage_points_matrix.append(vantage_points)
return vantage_points_matrix
def displacement_to_orientation(displacement: Coord) -> Orientation:
x = displacement.get_x()
y = displacement.get_y()
# this if list should be refactored into a constant, along with the one in Arena.py
if x > 0 and y == 0: return Orientation.EAST
elif x < 0 and y == 0: return Orientation.WEST
elif x == 0 and y > 0: return Orientation.NORTH
elif x == 0 and y < 0: return Orientation.SOUTH
else:
raise Exception(
f'Invalid displacement: tried moving {x}, {y}. displacement_to_orientation only implemented for cardinal directions'
)
def print_mdf(self):
explored_bin_str = "11"
obstacle_bin_str = ""
for y in range(MAP_ROW):
for x in range(MAP_COL):
coord = Coord(x,y)
if self.arena.get_cell_at_coord(coord).is_explored():
explored_bin_str += "1"
if self.arena.get_cell_at_coord(coord).is_obstacle():
obstacle_bin_str += "1"
else:
obstacle_bin_str += "0"
else:
explored_bin_str += "0"
explored_bin_str += "11"
if len(obstacle_bin_str) % 8 != 0:
num_pad_bits = 8 - len(obstacle_bin_str) % 8
obstacle_bin_str += "0" * num_pad_bits
explored_hex_str = f"{int(explored_bin_str, 2):X}"
print(explored_hex_str)
obstacle_hex_str = f"{int(obstacle_bin_str, 2):X}"
num_pad_bits = math.ceil(len(obstacle_bin_str) / 4) - len(obstacle_hex_str)
print("0" * num_pad_bits + obstacle_hex_str)
def convert_arena_to_mdf(self, arena):
explored_bin_str = "11"
obstacle_bin_str = ""
for y in range(MAP_ROW):
for x in range(MAP_COL):
coord = Coord(x, y)
if arena.get_cell_at_coord(coord).is_explored():
explored_bin_str += "1"
if arena.get_cell_at_coord(coord).is_obstacle():
obstacle_bin_str += "1"
else:
obstacle_bin_str += "0"
else:
explored_bin_str += "0"
explored_bin_str += "11"
if len(obstacle_bin_str) % 8 != 0:
num_pad_bits = 8 - len(obstacle_bin_str) % 8
obstacle_bin_str += "0" * num_pad_bits
explored_hex_str = f"{int(explored_bin_str, 2):X}" # explored bin string is never empty (only 0s)
if obstacle_bin_str:
obstacle_hex_str = f"{int(obstacle_bin_str, 2):X}"
num_pad_bits = math.ceil(
len(obstacle_bin_str) / 4) - len(obstacle_hex_str)
padded_obstacle_hex_str = "0" * num_pad_bits + obstacle_hex_str
else:
padded_obstacle_hex_str = ""
return explored_hex_str, padded_obstacle_hex_str
def get_eight_adjacent_in_arena(self, coord: Coord) -> list:
adj = []
for displacement in Arena.EIGHT_ADJACENCY:
adj_coord = coord.add(displacement)
if self.coord_is_valid(adj_coord):
adj.append(adj_coord)
return adj
def update_dangerous_cells(self) -> None:
for y in range(MAP_ROW):
for x in range(MAP_COL):
cur = Coord(x, y)
cell = self.get_cell_at_coord(cur)
if cell.is_obstacle():
cell.set_is_dangerous(True)
else:
cell.set_is_dangerous(False)
for displacement in Arena.EIGHT_ADJACENCY:
adj = cur.add(displacement)
if not self.coord_is_valid(adj):
continue
if self.get_cell_at_coord(adj).is_obstacle():
self.get_cell_at_coord(cur).set_is_dangerous(True)
break
self.mark_border_cells_dangerous()
def list_known_obstacles(self) -> list:
l = []
for y in range(MAP_ROW):
for x in range(MAP_COL):
coord = Coord(x,y)
if self.get_cell_at_coord(coord).is_obstacle():
l.append(coord)
return l
def list_unexplored_coords(self) -> list:
l = []
for y in range(MAP_ROW):
for x in range(MAP_COL):
coord = Coord(x,y)
if not self.get_cell_at_coord(coord).is_explored():
l.append(coord)
return l
def get_coverage_percentage(self) -> int:
explored = 0
for y in range(MAP_ROW):
for x in range(MAP_COL):
coord = Coord(x, y)
if self.get_cell_at_coord(coord).is_explored():
explored += 1
return explored / 300 * 100
def main(self):
i = 0
while True:
data = self.rx.recv_json()
# print(data)
print('qsize ', self.q_size)
if data['type'] == 'sensor':
if self.agent_task == AgentTask.EXPLORE or self.agent_task == AgentTask.IMAGEREC:
self.update_percepts(data)
if self.q_size == 0:
self.step()
print('got sensor data')
elif data['type'] == 'move_done':
self.q_size -= 1
print('got move_done')
elif data['type'] == 'start':
self.q_size = 0
if self.agent_task == AgentTask.FAST:
while not self.agent.get_robot_info().get_coord().is_equal(
Coord(13, 18)):
self.update_percepts(None)
self.step()
else:
self.step()
print('got start')
elif data['type'] == 'init':
self.init(data)
self.q_size = 1
print('got init')
elif data['type'] == 'waypoint':
self.waypoint = Coord(data['data']['x'], data['data']['y'])
print('got waypoint')
elif data['type'] == 'ping':
self.tx.send_json({'type': 'pong'})
elif data['type'] == 'terminate':
print('got terminate, quitting')
return
print('received message ', i)
i += 1
def get_coords_in_line(self, sensor_displacement: Coord,
displacement_per_step: Coord,
view_range: int) -> list:
all_possible_visible_coords = []
cur_coord = self.robot_info.get_coord()
for i in range(view_range):
coord = cur_coord.add(sensor_displacement).add(
displacement_per_step.multiply(i))
if self.arena.coord_is_valid(coord):
all_possible_visible_coords.append(coord)
else:
break
return all_possible_visible_coords
def parse_arena_string(string: str) -> Arena:
cl = list(string)
arena = Arena()
# set obstacles in arena
obstacle_list = []
i=0
for y in range(MAP_ROW-1, -1, -1): # counting from top to bottom
for x in range(MAP_COL):
coord = Coord(x, y)
if cl[i] == '\n':
i = i + 1
if cl[i] == '1':
arena.get_cell_at_coord(coord).increment_is_obstacle(delta=4)
obstacle_list.append(coord)
elif cl[i] == '0':
pass # not obstacle assumed
i = i+1
if y in [0,MAP_ROW-1] or x in [0,MAP_COL-1]:
# cells at edge of arena are too close to the walls
arena.get_cell_at_coord(coord).set_is_dangerous(True)
# set danger flag for cells too close to obstacles
for obs in obstacle_list:
displacements = [
Coord(-1, -1),
Coord(-1, 0),
Coord(-1, 1),
Coord(0, -1),
Coord(0, 1),
Coord(1, -1),
Coord(1, 0),
Coord(1, 1)
]
for d in displacements:
dangerous_coord = obs.add(d)
if 0 <= dangerous_coord.get_x() < 15 and 0 <= dangerous_coord.get_y() < 20:
arena.get_cell_at_coord(dangerous_coord).set_is_dangerous(True)
return arena
def mark_border_cells_dangerous(self) -> None:
for y in range(MAP_ROW):
for x in range(MAP_COL):
if y in [0,MAP_ROW-1] or x in [0,MAP_COL-1]:
# cells at edge of arena are too close to the walls
self.get_cell_at_coord(Coord(x,y)).set_is_dangerous(True)
class Arena:
ADJACENCY = [
Coord(0, 1),
Coord(1, 0),
Coord(0, -1),
Coord(-1, 0)
]
EIGHT_ADJACENCY = [
Coord(1, 1),
Coord(0, 1),
Coord(-1, 1),
Coord(-1, 0),
Coord(-1, -1),
Coord(0, -1),
Coord(1, -1),
Coord(1, 0)
]
def __init__(self):
self.cell_matrix = [[None for y in range(MAP_ROW)] for x in range(MAP_COL)] #initialise the matrix
for y in range(MAP_ROW): #create Cell objects for each cell in the matrix
for x in range(MAP_COL):
coord = Coord(x, y)
self.cell_matrix[coord.get_x()][coord.get_y()] = Cell(coord)
self.mark_border_cells_dangerous()
def coord_is_valid(self, coord: Coord) -> bool:
return coord.get_x() in range(MAP_COL) and coord.get_y() in range(MAP_ROW)
def get_cell_at_coord(self, coord):
return self.cell_matrix[coord.get_x()][coord.get_y()]
def get_four_adjacent_in_arena(self, coord: Coord) -> list:
adj = []
for displacement in Arena.ADJACENCY:
adj_coord = coord.add(displacement)
if self.coord_is_valid(adj_coord):
adj.append(adj_coord)
return adj
def get_eight_adjacent_in_arena(self, coord: Coord) -> list:
adj = []
for displacement in Arena.EIGHT_ADJACENCY:
adj_coord = coord.add(displacement)
if self.coord_is_valid(adj_coord):
adj.append(adj_coord)
return adj
def calculate_adjacent_lists(self, coord) -> dict:
adj_safe, adj_unb, adj_blocked = [], [], []
for adj_coord in self.get_four_adjacent_in_arena(coord):
cell = self.get_cell_at_coord(adj_coord)
if cell.is_obstacle():
adj_blocked.append(adj_coord)
else:
adj_unb.append(adj_coord)
if not cell.is_dangerous():
adj_safe.append(adj_coord)
return {
'safe': adj_safe,
'unblocked': adj_unb,
'blocked': adj_blocked
}
# def get_adjacent_blocked(self, coord) -> list:
# return self.calculate_adjacent_lists(coord)['blocked']
def get_adjacent_safe(self, coord) -> list:
return self.calculate_adjacent_lists(coord)['safe']
# def get_adjacent_unblocked(self, coord) -> list:
# return self.calculate_adjacent_lists(coord)['unblocked']
# def mark_dangerous_cells_around_obstacle(self, coord, is_dangerous):
# for displacement in Arena.EIGHT_ADJACENCY:
# adj_coord = coord.add(displacement)
# if self.coord_is_valid(adj_coord):
# self.get_cell_at_coord(adj_coord).set_is_dangerous(is_dangerous)
def update_dangerous_cells(self) -> None:
for y in range(MAP_ROW):
for x in range(MAP_COL):
cur = Coord(x, y)
cell = self.get_cell_at_coord(cur)
if cell.is_obstacle():
cell.set_is_dangerous(True)
else:
cell.set_is_dangerous(False)
for displacement in Arena.EIGHT_ADJACENCY:
adj = cur.add(displacement)
if not self.coord_is_valid(adj):
continue
if self.get_cell_at_coord(adj).is_obstacle():
self.get_cell_at_coord(cur).set_is_dangerous(True)
break
self.mark_border_cells_dangerous()
def get_coverage_percentage(self) -> int:
explored = 0
for y in range(MAP_ROW):
for x in range(MAP_COL):
coord = Coord(x, y)
if self.get_cell_at_coord(coord).is_explored():
explored += 1
return explored / 300 * 100
def list_unexplored_cells(self) -> list:
# returns CELLS not coords
l = []
for row in self.cell_matrix:
for cell in row:
if not cell.is_explored():
l.append(cell)
return l
def list_unexplored_coords(self) -> list:
l = []
for y in range(MAP_ROW):
for x in range(MAP_COL):
coord = Coord(x,y)
if not self.get_cell_at_coord(coord).is_explored():
l.append(coord)
return l
def set_all_explored(self) -> None:
for row in self.cell_matrix:
for cell in row:
cell.set_is_explored(True)
def mark_border_cells_dangerous(self) -> None:
for y in range(MAP_ROW):
for x in range(MAP_COL):
if y in [0,MAP_ROW-1] or x in [0,MAP_COL-1]:
# cells at edge of arena are too close to the walls
self.get_cell_at_coord(Coord(x,y)).set_is_dangerous(True)
# Used in Image Rec Algo:
def all_obstacles_seen(self) -> bool:
for coord in self.list_known_obstacles():
if not self.get_cell_at_coord(coord).is_seen():
return False
return True
def list_known_obstacles(self) -> list:
l = []
for y in range(MAP_ROW):
for x in range(MAP_COL):
coord = Coord(x,y)
if self.get_cell_at_coord(coord).is_obstacle():
l.append(coord)
return l
def get_nearest_obstacle_adj_coord(self, cur_coord: Coord) -> Coord: # if empty is true, return the adj coord, else return the obstacle itself
obstacle_coord_list = self.list_known_obstacles()
unseen_obstacles_list = []
target = None
for item in obstacle_coord_list:
if not self.get_cell_at_coord(item).is_seen():
if item.get_x() != 0 and item.get_x() != 14 and item.get_y() != 0 and item.get_y() != 19:
unseen_obstacles_list.append(item)
# store a list of all vantages, then take the nearest one
rwh_vantage_list = []
for obstacle in unseen_obstacles_list:
for rwh_vantage in self.calculate_rwh_vantage(obstacle): # iterate through the obstacle's vantage points
if not self.coord_is_valid(rwh_vantage):
continue
if self.get_cell_at_coord(rwh_vantage).is_obstacle():
continue
if self.get_cell_at_coord(rwh_vantage).is_dangerous():
continue
if not self.get_cell_at_coord(rwh_vantage).is_explored():
continue
distance = rwh_vantage.subtract(cur_coord).manhattan_distance()
rwh_vantage_list.append((distance, rwh_vantage, obstacle))
try:
target = sorted(rwh_vantage_list, key=lambda x: x[0])[0]
return target[1], target[2]
except IndexError:
print("NO MORE UNSEEN OBSTACLES")
return None, None
def calculate_rwh_vantage(self, ue: Coord) -> list:
# vantage points are cells where robot will stand next to an obstacle
rwh_vantage_points = []
for disp in [(-2,0), (0,2), (0,-2), (2,0)]:
coord = Coord(disp[0], disp[1])
rwh_vantage_points.append(ue.add(coord))
return rwh_vantage_points
def coord_is_valid(self, coord: Coord) -> bool:
return coord.get_x() in range(MAP_COL) and coord.get_y() in range(MAP_ROW)
def heuristic(cur: Coord, target: Coord) -> int:
return target.subtract(cur).manhattan_distance()
grey = [100, 100, 100]
red = [255, 0, 0]
green = [0, 255, 0]
yellow = [255, 255, 0]
purple = [138, 43, 226]
light_blue = [135, 206, 250]
# Defining map dimensions
MAP_ROW = 20
MAP_COL = 15
TILE_SIZE = 20
DIS_X = 300
DIS_Y = 400
# Defining start, end and waypoint coord
START_COORD = Coord(1, 1)
END_COORD = Coord(13, 18)
WAYPOINT = Coord(4, 4)
ALGO_RETURNS_FULL_PATH = False
class ArenaDisplayMode(Enum):
OBSERVED = 0
TRUE = 1
ARENA_DISPLAY_MODE = ArenaDisplayMode.OBSERVED
class Orientation(Enum):
