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