From 311a2d075793bb5750f5e6e7e3aeb97eac3163a0 Mon Sep 17 00:00:00 2001 From: Pawel Felcyn Date: Mon, 15 May 2023 10:59:30 +0200 Subject: [PATCH] =?UTF-8?q?astar=20for=20MIko=C5=82aj=20Gawor?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- bfs.py | 62 ++++++++++++++++++++++++++++++++++------------------- city.py | 6 +++--- movement.py | 2 +- 3 files changed, 44 insertions(+), 26 deletions(-) diff --git a/bfs.py b/bfs.py index a559592..92556dd 100644 --- a/bfs.py +++ b/bfs.py @@ -4,30 +4,44 @@ from city import City from gridCellType import GridCellType from agentActionType import AgentActionType from agentOrientation import AgentOrientation -from queue import Queue +from queue import Queue, PriorityQueue from turnCar import turn_left_orientation, turn_right_orientation class Succ: state: AgentState action: AgentActionType - ##cost: int + cost: int + predicted_cost: int - def __init__(self, state: AgentState, action: AgentActionType) -> None: + def __init__(self, state: AgentState, action: AgentActionType, cost: int, predicted_cost: int) -> None: self.state = state self.action = action - ##self.cost = cost + self.cost = cost + self.predicted_cost = cost -def find_path_to_nearest_can(startState: AgentState, grid: Dict[Tuple[int, int], GridCellType]) -> list[AgentActionType]: - q: Queue[list[Succ]] = Queue() +class SuccList: + succ_list: list[Succ] + + def __init__(self, succ_list: list[Succ]) -> None: + self.succ_list = succ_list + + def __lt__(self, other): + return self.succ_list[-1].predicted_cost < other.succ_list[-1].predicted_cost + + def __gt__(self, other): + return self.succ_list[-1].predicted_cost > other.succ_list[-1].predicted_cost + +def find_path_to_nearest_can(startState: AgentState, grid: Dict[Tuple[int, int], GridCellType], city: City) -> list[AgentActionType]: + q: PriorityQueue[SuccList] = PriorityQueue() visited: list[AgentState] = [] - startStates: list[Succ] = [Succ(startState, AgentActionType.UNKNOWN)] + startStates: SuccList = SuccList([Succ(startState, AgentActionType.UNKNOWN, 0, _heuristics(startState.position, city))]) q.put(startStates) while not q.empty(): currently_checked = q.get() - visited.append(currently_checked[-1].state) - if is_state_success(currently_checked[-1].state, grid): + visited.append(currently_checked.succ_list[-1].state) + if is_state_success(currently_checked.succ_list[-1].state, grid): return extract_actions(currently_checked) - successors = succ(currently_checked[-1].state) + successors = succ(currently_checked.succ_list[-1], grid, city) for s in successors: already_visited = False for v in visited: @@ -37,34 +51,38 @@ def find_path_to_nearest_can(startState: AgentState, grid: Dict[Tuple[int, int], if already_visited: continue if is_state_valid(s.state, grid): - new_list = currently_checked.copy() + new_list = currently_checked.succ_list.copy() new_list.append(s) - q.put(new_list) + q.put(SuccList(new_list)) return [] -def extract_actions(successors: list[Succ]) -> list[AgentActionType]: +def extract_actions(successors: SuccList) -> list[AgentActionType]: output: list[AgentActionType] = [] - for s in successors: + for s in successors.succ_list: if s.action != AgentActionType.UNKNOWN: output.append(s.action) return output -def succ(state: AgentState) -> list[Succ]: +def succ(succ: Succ, grid: Dict[Tuple[int, int], GridCellType], city: City) -> list[Succ]: result: list[Succ] = [] - result.append(Succ(AgentState(state.position, turn_left_orientation(state.orientation)), AgentActionType.TURN_LEFT)) - result.append(Succ(AgentState(state.position, turn_right_orientation(state.orientation)), AgentActionType.TURN_RIGHT)) - state_succ = move_forward_succ(state) + turn_left_cost = 1 + succ.cost + result.append(Succ(AgentState(succ.state.position, turn_left_orientation(succ.state.orientation)), AgentActionType.TURN_LEFT, turn_left_cost, turn_left_cost + _heuristics(succ.state.position, city))) + turn_right_cost = 1 + succ.cost + result.append(Succ(AgentState(succ.state.position, turn_right_orientation(succ.state.orientation)), AgentActionType.TURN_RIGHT, turn_right_cost, turn_right_cost + _heuristics(succ.state.position, city))) + state_succ = move_forward_succ(succ, city, grid) if state_succ != None: - result.append(move_forward_succ(state)) + result.append(state_succ) return result -def move_forward_succ(state: AgentState) -> Succ: - position = get_next_cell(state) +def move_forward_succ(succ: Succ, city: City, grid: Dict[Tuple[int, int], GridCellType]) -> Succ: + position = get_next_cell(succ.state) if position == None: return None - return Succ(AgentState(position, state.orientation), AgentActionType.MOVE_FORWARD) + + cost = get_cost_for_action(AgentActionType.MOVE_FORWARD, grid[position]) + succ.cost + return Succ(AgentState(position, succ.state.orientation), AgentActionType.MOVE_FORWARD, cost, cost + _heuristics(position, city)) def get_next_cell(state: AgentState) -> Tuple[int, int]: diff --git a/city.py b/city.py index 431f582..004e987 100644 --- a/city.py +++ b/city.py @@ -11,12 +11,12 @@ class City: cans_dict: Dict[Tuple[int, int], GarbageCan] = {} def __init__(self) -> None: - self.nodes = [] + self.cans = [] self.streets = [] self.bumps = [] def add_can(self, can: GarbageCan) -> None: - self.nodes.append(can) + self.cans.append(can) self.cans_dict[can.position] = can def add_street(self, street: Street) -> None: @@ -35,7 +35,7 @@ class City: street.render(game_context) def _render_nodes(self, game_context: GameContext) -> None: - for node in self.nodes: + for node in self.cans: node.render(game_context) def _render_bumps(self, game_context: GameContext) -> None: diff --git a/movement.py b/movement.py index 308c64b..a9990e7 100644 --- a/movement.py +++ b/movement.py @@ -13,7 +13,7 @@ from agentState import AgentState def collect_garbage(game_context: GameContext) -> None: while True: start_agent_state = AgentState(game_context.dust_car.position, game_context.dust_car.orientation) - path = find_path_to_nearest_can(start_agent_state, game_context.grid) + path = find_path_to_nearest_can(start_agent_state, game_context.grid, game_context.city) if path == None or len(path) == 0: break move_dust_car(path, game_context)