From 15638862d363f98f5564c55130c56556551fe8d6 Mon Sep 17 00:00:00 2001 From: majkellll Date: Mon, 15 May 2023 11:49:58 +0200 Subject: [PATCH] A* working ok --- agentState.py | 2 +- bfs.py | 45 ++++++++++++++++++++++++++++++--------------- city.py | 2 +- main.py | 4 +--- movement.py | 3 ++- 5 files changed, 35 insertions(+), 21 deletions(-) diff --git a/agentState.py b/agentState.py index 1ac708d..ace580e 100644 --- a/agentState.py +++ b/agentState.py @@ -7,4 +7,4 @@ class AgentState: def __init__(self, position: Tuple[int, int], orientation: AgentOrientation) -> None: self.orientation = orientation - self.position = position \ No newline at end of file + self.position = position diff --git a/bfs.py b/bfs.py index 92556dd..02fd4a0 100644 --- a/bfs.py +++ b/bfs.py @@ -7,6 +7,7 @@ from agentOrientation import AgentOrientation from queue import Queue, PriorityQueue from turnCar import turn_left_orientation, turn_right_orientation + class Succ: state: AgentState action: AgentActionType @@ -19,6 +20,7 @@ class Succ: self.cost = cost self.predicted_cost = cost + class SuccList: succ_list: list[Succ] @@ -27,14 +29,17 @@ class SuccList: 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]: + +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: SuccList = SuccList([Succ(startState, AgentActionType.UNKNOWN, 0, _heuristics(startState.position, city))]) + startStates: SuccList = SuccList( + [Succ(startState, AgentActionType.UNKNOWN, 0, _heuristics(startState.position, city))]) q.put(startStates) while not q.empty(): currently_checked = q.get() @@ -45,7 +50,8 @@ def find_path_to_nearest_can(startState: AgentState, grid: Dict[Tuple[int, int], for s in successors: already_visited = False for v in visited: - if v.position[0] == s.state.position[0] and v.position[1] == s.state.position[1] and s.state.orientation == v.orientation: + if v.position[0] == s.state.position[0] and v.position[1] == s.state.position[ + 1] and s.state.orientation == v.orientation: already_visited = True break if already_visited: @@ -56,8 +62,7 @@ def find_path_to_nearest_can(startState: AgentState, grid: Dict[Tuple[int, int], q.put(SuccList(new_list)) return [] - - + def extract_actions(successors: SuccList) -> list[AgentActionType]: output: list[AgentActionType] = [] for s in successors.succ_list: @@ -65,24 +70,31 @@ def extract_actions(successors: SuccList) -> list[AgentActionType]: output.append(s.action) return output + def succ(succ: Succ, grid: Dict[Tuple[int, int], GridCellType], city: City) -> list[Succ]: result: list[Succ] = [] 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))) + 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))) + 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(state_succ) return result + 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 - + 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)) + return Succ(AgentState(position, succ.state.orientation), AgentActionType.MOVE_FORWARD, cost, + cost + _heuristics(position, city)) def get_next_cell(state: AgentState) -> Tuple[int, int]: @@ -102,6 +114,7 @@ def get_next_cell(state: AgentState) -> Tuple[int, int]: return None return (state.position[0] + 1, state.position[1]) + def is_state_success(state: AgentState, grid: Dict[Tuple[int, int], GridCellType]) -> bool: next_cell = get_next_cell(state) try: @@ -109,6 +122,7 @@ def is_state_success(state: AgentState, grid: Dict[Tuple[int, int], GridCellType except: return False + def get_cost_for_action(action: AgentActionType, cell_type: GridCellType) -> int: if action == AgentActionType.TURN_LEFT or action == AgentActionType.TURN_RIGHT: return 1 @@ -120,11 +134,13 @@ def get_cost_for_action(action: AgentActionType, cell_type: GridCellType) -> int def is_state_valid(state: AgentState, grid: Dict[Tuple[int, int], GridCellType]) -> bool: - try: - return grid[state.position] == GridCellType.STREET_HORIZONTAL or grid[state.position] == GridCellType.STREET_VERTICAL or grid[state.position] == GridCellType.SPEED_BUMP + try: + return grid[state.position] == GridCellType.STREET_HORIZONTAL or grid[ + state.position] == GridCellType.STREET_VERTICAL or grid[state.position] == GridCellType.SPEED_BUMP except: return False - + + def _heuristics(position: Tuple[int, int], city: City): min_distance: int = 300 found_nonvisited: bool = False @@ -137,5 +153,4 @@ def _heuristics(position: Tuple[int, int], city: City): min_distance = distance if found_nonvisited: return min_distance - return -1 - \ No newline at end of file + return -1 \ No newline at end of file diff --git a/city.py b/city.py index f53305c..68ae600 100644 --- a/city.py +++ b/city.py @@ -41,4 +41,4 @@ class City: def _render_bumps(self, game_context: GameContext) -> None: for bump in self.bumps: - bump.render(game_context) + bump.render(game_context) \ No newline at end of file diff --git a/main.py b/main.py index 721211c..deb76bc 100644 --- a/main.py +++ b/main.py @@ -20,10 +20,8 @@ game_context.dust_car_pil = dust_car_pil game_context.dust_car_pygame = pygame.image.frombuffer(dust_car_pil.tobytes(), dust_car_pil.size, 'RGB') game_context.canvas = canvas -city = City() - startup(game_context) -collect_garbage(game_context, city) +collect_garbage(game_context) exit = False diff --git a/movement.py b/movement.py index a9990e7..0060c0e 100644 --- a/movement.py +++ b/movement.py @@ -10,6 +10,7 @@ import pygame from bfs import find_path_to_nearest_can 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) @@ -22,6 +23,7 @@ def collect_garbage(game_context: GameContext) -> None: game_context.city.cans_dict[next_position].is_visited = True pass + def move_dust_car(actions: list[AgentActionType], game_context: GameContext) -> None: for action in actions: street_position = game_context.dust_car.position @@ -44,7 +46,6 @@ def move_dust_car(actions: list[AgentActionType], game_context: GameContext) -> pygame.display.update() time.sleep(0.15) - def calculate_next_position(car: GarbageTruck) -> Tuple[int, int]: if car.orientation == AgentOrientation.UP: