def get_next_step(self, arena: Arena, robot_info: RobotInfo) -> Coord:
self.arena = arena
self.cur_coord = robot_info.get_coord()
self.cur_direction = robot_info.get_orientation()
# check for looping in LWH
if len(self.lwh_last_four) == 4 and self.lwh_last_four[0].is_equal(
self.cur_coord) and not self.lwh_last_four[1].is_equal(
self.lwh_last_four[3]):
self.lwh_looped = True
if self.lwh_looped:
next_step = self.move_back()
self.lwh_looped = False
self.lwh_last_four = []
else:
next_step = self.left_wall_hug()
if len(self.lwh_last_four) >= 4:
self.lwh_last_four.pop(0)
self.lwh_last_four.append(self.cur_coord)
return next_step
def init(self, agent_task: AgentTask, arena_string: str, waypoint: Coord):
self.robot_info = RobotInfo(START_COORD, Orientation.NORTH)
self.sim_display = SimulationDisplay(self.robot_info)
self.arena = ArenaStringParser.parse_arena_string(arena_string)
# send copies of arena and robot info so that simulator and agent do not modify a mutual copy
empty_arena_string = open(
"./algorithms/src/simulator/empty_arena_string.txt", "r").read()
self.agent = Agent(empty_arena_string, self.robot_info.copy(),
agent_task, END_COORD, waypoint)
def init(self, agent_task: AgentTask, arena_string: str, waypoint: Coord):
self.robot_info = RobotInfo(START_COORD, Orientation.NORTH)
self.sim_display = SimulationDisplay(self.robot_info)
self.arena = ArenaStringParser.parse_arena_string(arena_string) #used in line 34 and 100
# self.update_display()
# arena and robot info are separate so that simulator and agent do not modify a mutual copy
if agent_task == AgentTask.FAST:
self.agent = Agent(arena_string, self.robot_info.copy(), agent_task, END_COORD, waypoint)
else:
empty_arena_string = open("./algorithms/src/simulator/empty_arena_string.txt", "r").read()
self.agent = Agent(empty_arena_string, self.robot_info.copy(), agent_task, END_COORD, waypoint)
def get_next_step(self, arena: Arena, robot_info: RobotInfo) -> Coord:
# implement the flood fill algorithm using a stack
self.arena = arena
self.cur_coord = robot_info.get_coord()
next_targets = self.get_nearest_unexplored(self.cur_coord)
if not next_targets:
# list is empty, all have been explored
return None
fp_algo = FastestPathAlgo()
for next_target in next_targets:
next_step = fp_algo.get_next_step(arena,
robot_info,
end=next_target,
waypoint=None)
if next_step:
break
if not next_step:
# unexplored cells are inaccessible, probably an obstacle is falsely set
raise Exception(
'ExplorationAlgo: unexplored cell(s) are inacessible')
return next_step
def process_sensor_data(self, percept_data):
# feed agent percepts
print('\n\nprocessing percepts')
if self.q_size == 0:
robot_info = self.agent.get_expected_robot_info()
elif self.q_size == 1:
ri_old = self.agent.get_robot_info()
ri_new = self.agent.get_expected_robot_info()
robot_info = RobotInfo(
ri_old.get_coord(),
ri_new.get_orientation()
if ri_new else ri_old.get_orientation())
elif self.q_size < 0:
raise Exception(
f'move q-size: {self.q_size}\nreceived extra move done before sensor data!'
)
else:
raise Exception(
f'move q-size: {self.q_size}\n2 last moves were not executed yet! \
Is there desync between bot and algo?')
if percept_data:
obstacle_coord_list, no_obs_coord_list = SensorParser.main_sensor_parser(
percept_data, robot_info)
else:
obstacle_coord_list, no_obs_coord_list = [], []
print('obstacle list: ')
obstacle_str = ''
for coord, distance in obstacle_coord_list:
obstacle_str += f'({coord.get_x()}, {coord.get_y()}), {distance}, '
print(obstacle_str)
return obstacle_coord_list, no_obs_coord_list
def parse_init_data(self, init_data):
assert (init_data['type'] == 'init')
# arena string
mdf_string = init_data['data']['arena'][
'P2'] # p1 string ignored since always FFFF..
arena_string = self.convert_mdf_to_binary(
mdf_string) if init_data['data']['task'] == 'FP' else None
# robot info
robot_info = RobotInfo(START_COORD, START_ORIENTATION)
# agent task
if init_data['data']['task'] == 'FP':
agent_task = AgentTask.FAST
elif init_data['data']['task'] == 'EX':
agent_task = AgentTask.EXPLORE
elif init_data['data']['task'] == 'IR':
agent_task = AgentTask.IMAGEREC
else:
raise Exception('Invalid agent task: ' + init_data['data']['task'])
# end_coord
end_coord = END_COORD
return arena_string, robot_info, agent_task, end_coord
def calculate_move(self, current_coord, target_coord) -> MoveCommand:
displacement = target_coord.subtract(current_coord)
target_orientation = OT.displacement_to_orientation(displacement)
turn_angle = OT.calc_degree_of_turn(self.robot_info.get_orientation(),
target_orientation)
# update expected robot info
self.expected_robot_info = RobotInfo(target_coord, target_orientation)
return MoveCommand(turn_angle, displacement.manhattan_distance())
def align_right_wall(self, arena: Arena, robot_info: RobotInfo,
target_obstacle: Coord) -> Coord:
self.arena = arena
self.cur_coord = robot_info.get_coord()
self.cur_direction = robot_info.get_orientation()
# check which side the obstacle is on with respect to the robot's current orientation,
# then rotate the robot to have its right side facing the obstacle
if self.check_left_free(True).is_equal(target_obstacle):
target = self.move_back()
elif self.check_front_free(True).is_equal(target_obstacle):
if self.check_left_free():
target = self.move_left()
else:
target = self.move_back()
elif self.check_right_free(True).is_equal(target_obstacle):
if self.check_front_free():
target = self.move_forward()
elif self.check_left_free():
target = self.move_left()
else:
target = self.move_back()
return target
def get_agent_output_list(self) -> list:
# Calculate fastest path of coords
coords_list = []
cl_to_waypoint = self.algo.get_next_step(self.arena,
self.robot_info,
self.end_coord,
self.waypoint_coord,
full_path=True)
last_displacement = cl_to_waypoint[-1].subtract(cl_to_waypoint[-2])
last_orientation = OT.displacement_to_orientation(last_displacement)
robot_info_at_waypoint = RobotInfo(cl_to_waypoint[-1],
last_orientation)
cl_to_end = self.algo.get_next_step(self.arena,
robot_info_at_waypoint,
self.end_coord,
None,
full_path=True)
coords_list.extend(cl_to_waypoint)
coords_list.extend(cl_to_end)
return self.calc_agent_output_full_path(coords_list)
class Simulator:
def __init__(self):
# display
pygame.init()
self.sim_display = None
pygame.display.set_caption('arena simulator')
# data
self.robot_info = None
self.agent = None
self.arena = None
self.full_path_step_count = 0
def init(self, agent_task: AgentTask, arena_string: str, waypoint: Coord):
self.robot_info = RobotInfo(START_COORD, Orientation.NORTH)
self.sim_display = SimulationDisplay(self.robot_info)
self.arena = ArenaStringParser.parse_arena_string(arena_string) #used in line 34 and 100
# self.update_display()
# arena and robot info are separate so that simulator and agent do not modify a mutual copy
if agent_task == AgentTask.FAST:
self.agent = Agent(arena_string, self.robot_info.copy(), agent_task, END_COORD, waypoint)
else:
empty_arena_string = open("./algorithms/src/simulator/empty_arena_string.txt", "r").read()
self.agent = Agent(empty_arena_string, self.robot_info.copy(), agent_task, END_COORD, waypoint)
def step(self):
# calculate agent percepts
sis = SensorInputSimulation(self.robot_info, self.arena)
obstacle_coord_list, no_obs_coord_list = sis.calculate_percepts()
for coord, _ in obstacle_coord_list: # distance is not used, hence renamed to _
self.arena.get_cell_at_coord(coord).set_is_explored(True)
for coord, _ in no_obs_coord_list:
self.arena.get_cell_at_coord(coord).set_is_explored(True)
self.arena.get_cell_at_coord(self.robot_info.get_coord()).set_is_visited(True)
if ALGO_RETURNS_FULL_PATH:
if self.full_path_step_count >= len(self.agent_output_full_path):
move_command = None
else:
agent_output = self.agent_output_full_path[self.full_path_step_count]
if agent_output.get_message():
print(agent_output.get_message())
move_command = agent_output.get_move_command()
self.full_path_step_count += 1
else:
# get agent next move
self.agent.calc_percepts(obstacle_coord_list, no_obs_coord_list)
agent_output = self.agent.step()
print(agent_output.get_message())
move_command = agent_output.get_move_command()
if move_command is None:
self.quit()
if self.coverage <= self.arena.get_coverage_percentage():
print("Coverage limit reached!")
if self.robot_info.get_coord().is_equal(START_COORD):
self.quit() # when exploring we need to go back to start point, so don't quit before
self.update_robot_info(move_command)
if self.time_ran_out:
print("Time's up!")
self.quit()
# update pygame display
self.update_display()
def timeout(self):
self.time_ran_out = True
def run(self):
self.speed = 1 / float(input("Enter robot speed (steps per second) (-1 for default): "))
self.coverage = int(input("Enter coverage limit (%) (-1 for default): "))
self.timelimit = float(input("Enter time limit (sec) (-1 for default): "))
if self.coverage < 0:
self.coverage = 1000
if self.speed < 0:
self.speed = 0.5
if self.timelimit < 0:
self.timelimit = 600 #default: 10 mins
t = Timer(self.timelimit, self.timeout)
self.time_ran_out = False
t.start()
if ALGO_RETURNS_FULL_PATH:
self.agent_output_full_path = self.agent.get_agent_output_list()
i = 0
while i<200:
self.events()
self.step()
time.sleep(self.speed)
i+=1
print(f'Run timed-out after {i+1} steps')
def update_display(self):
if ARENA_DISPLAY_MODE == ArenaDisplayMode.OBSERVED:
seen_arena = self.agent.get_arena()
self.sim_display.draw(seen_arena, self.robot_info)
else:
self.sim_display.draw(self.arena, self.robot_info)
pygame.display.update()
def update_robot_info(self, move_command):
# update internal representation of robot
new_orientation = OrientationTransform.calc_orientation_after_turn(self.robot_info.get_orientation(), move_command.get_turn_angle())
if new_orientation != self.robot_info.get_orientation():
if abs(new_orientation.value - self.robot_info.get_orientation().value) == 1 or abs(new_orientation.value - self.robot_info.get_orientation().value) == 3:
self.robot_info.set_orientation(new_orientation)
self.robot_info.set_coord(self.robot_info.get_coord())
self.update_display()
elif abs(new_orientation.value - self.robot_info.get_orientation().value) == 2:
self.robot_info.set_orientation(Orientation((new_orientation.value-1)%3))
self.robot_info.set_coord(self.robot_info.get_coord())
self.update_display()
self.robot_info.set_orientation(new_orientation)
self.robot_info.set_coord(self.robot_info.get_coord())
self.update_display()
unit_move = OrientationTransform.orientation_to_unit_displacement(new_orientation)
move = unit_move.multiply(move_command.get_cells_to_advance()) # enable this if robot can move multiple squares
self.robot_info.set_coord(self.robot_info.get_coord().add(move))
def quit(self):
self.arena = self.agent.get_arena() # cheeky patch to let our agent fill in unexplored cells as obstacles
print('debug number of unexplored cells: ', len(self.arena.list_unexplored_cells()))
print('debug number of explored cells: ', 300-len(self.arena.list_unexplored_cells()))
self.print_mdf()
self.update_display()
self.update_display()
print('Quitting...')
time.sleep(5)
pygame.quit()
quit()
def events(self):
for event in pygame.event.get():
# Close the screen by pressing the x
if event.type==pygame.QUIT:
self.quit()
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 get_fastest_path(self, arena: Arena, robot_info: RobotInfo, end: Coord, waypoint:Coord=None):
start = robot_info.get_coord()
start_orientation = robot_info.get_orientation()
if not arena.coord_is_valid(end):
raise Exception(f'FastestPath: end coord out of arena: {end.get_x()}, {end.get_y()}')
end_cell = arena.get_cell_at_coord(end)
if end_cell.is_obstacle():
raise Exception(f'FastestPath: cannot move to obstacle cell: {end.get_x()}, {end.get_y()}')
if waypoint:
# we haven't passed through it yet
target = waypoint
else:
target = end
root = DecisionNode(start, start_orientation, exact_cost=0, parent=None)
fringe_nodes = PriorityQueue()
queue_tie_breaker = 0
fringe_nodes.put((
FastestPathAlgo.heuristic(root.get_coord(), target),
queue_tie_breaker,
root
))
examined = [[False for y in range(MAP_ROW)] for x in range(MAP_COL)]
cur = fringe_nodes.get()[-1]
while True:
# two conditions to exit loop, one may or may not be enabled
cur_coord = cur.get_coord()
cur_orientation = cur.get_orientation()
if not end_cell.is_dangerous():
# normally exit when target is found
if cur_coord.is_equal(target):
# found target so break and find first step
break
else:
# if cell is dangerous, move to any adjacent cell to it \
# (some cells, surrounded by danger, will be left out, but they are likely to be obstacles)
if cur_coord.subtract(target).manhattan_distance() <= 1 \
and cur_coord.subtract(start).manhattan_distance() > 0:
# target not found so return None
return None
if fringe_nodes.qsize() >= 3000:
# if we've got such a long queue, there's probably no way to the target
return None
examined[cur_coord.get_x()][cur_coord.get_y()] = True
adjacent_coords = arena.get_adjacent_safe(cur_coord)
for coord in adjacent_coords:
if examined[coord.get_x()][coord.get_y()]:
# assume heuristic is admissible, otherwise need to assess total_cost \
# and remove node from examined set if g+h < existing g
continue
# get costs
target_orientation = OrientationTransform.displacement_to_orientation(coord.subtract(cur_coord))
degree_of_turn = OrientationTransform.calc_degree_of_turn(cur_orientation, target_orientation)
turning_cost = int(degree_of_turn/90)*TimeCosts.QUARTER_TURN
displacement_cost = TimeCosts.MOVE_ONE_UNIT
cost = turning_cost + displacement_cost
exact_cost = cur.get_exact_cost() + cost
estimated_cost = FastestPathAlgo.heuristic(cur_coord, target)
total_cost = exact_cost + estimated_cost
fringe_node = DecisionNode(coord, target_orientation, exact_cost=exact_cost, parent=cur)
queue_tie_breaker += 1
fringe_nodes.put((total_cost, queue_tie_breaker, fringe_node))
if fringe_nodes.empty():
return None
cur = fringe_nodes.get()[-1]
cur_list = []
while cur.get_parent() and not cur.get_parent().get_coord().is_equal(start):
cur_list.append(cur)
cur = cur.get_parent()
cur_list.append(cur)
cur_list.reverse()
return cur_list
class Simulator:
def __init__(self):
# display
pygame.init()
self.sim_display = None
pygame.display.set_caption('arena simulator')
# data
self.robot_info = None
self.robot_sprite = None
self.agent = None
self.arena = None
def init(self, agent_task: AgentTask, arena_string: str, waypoint: Coord):
self.robot_info = RobotInfo(START_COORD, Orientation.NORTH)
self.sim_display = SimulationDisplay(self.robot_info)
self.arena = ArenaStringParser.parse_arena_string(arena_string)
# send copies of arena and robot info so that simulator and agent do not modify a mutual copy
empty_arena_string = open(
"./algorithms/src/simulator/empty_arena_string.txt", "r").read()
self.agent = Agent(empty_arena_string, self.robot_info.copy(),
agent_task, END_COORD, waypoint)
def step(self):
# calculate agent percepts
sis = SensorInputSimulation(self.robot_info, self.arena)
obstacle_coord_list, no_obs_coord_list = sis.calculate_percepts()
for coord in obstacle_coord_list:
self.arena.get_cell_at_coord(coord).set_is_explored(True)
for coord in no_obs_coord_list:
self.arena.get_cell_at_coord(coord).set_is_explored(True)
self.arena.get_cell_at_coord(
self.robot_info.get_coord()).set_is_visited(True)
# get agent next move
agent_output = self.agent.step(obstacle_coord_list, no_obs_coord_list,
self.robot_info)
print(agent_output.get_message())
move_command = agent_output.get_move_command()
if move_command == None:
self.quit()
# update internal representation of robot
new_orientation = OrientationTransform.calc_orientation_after_turn(
self.robot_info.get_orientation(), move_command.get_turn_angle())
if new_orientation != self.robot_info.get_orientation():
if abs(new_orientation.value -
self.robot_info.get_orientation().value) == 1 or abs(
new_orientation.value -
self.robot_info.get_orientation().value) == 3:
self.robot_info.set_orientation(new_orientation)
self.robot_info.set_coord(self.robot_info.get_coord())
self.update_display()
time.sleep(self.speed)
elif abs(new_orientation.value -
self.robot_info.get_orientation().value) == 2:
self.robot_info.set_orientation(
Orientation((new_orientation.value - 1) % 3))
self.robot_info.set_coord(self.robot_info.get_coord())
self.update_display()
time.sleep(self.speed)
self.robot_info.set_orientation(new_orientation)
self.robot_info.set_coord(self.robot_info.get_coord())
self.update_display()
time.sleep(self.speed)
unit_move = OrientationTransform.orientation_to_unit_displacement(
new_orientation)
# move = unit_move.multiply(move_command.get_cells_to_advance()) # enable this if robot can move multiple squares
move = unit_move # enable this if robot can move one square at a time
self.robot_info.set_coord(self.robot_info.get_coord().add(move))
# update pygame display
self.update_display()
def run(self):
self.speed = 1 / float(
input("Enter robot speed (steps per second) (-1 for default): "))
if self.speed < 0:
self.speed = 0.5
i = 0
while i < 99:
self.events()
self.step()
time.sleep(self.speed)
i += 1
print(f'Run timed-out after {i+1} steps')
def update_display(self):
self.sim_display.draw(self.arena, self.robot_info)
pygame.display.update()
def quit(self):
pygame.quit()
quit()
def events(self):
for event in pygame.event.get():
# Close the screen by pressing the x
if event.type == pygame.QUIT:
self.quit()