dodany A* - coś jeszcze nie działa

bfs.py

@@ -1,47 +1,56 @@
 from agentState import AgentState
-from typing import Dict, Tuple
+from typing import Dict, Tuple, List, Set
 from city import City
 from gridCellType import GridCellType
 from agentActionType import AgentActionType
 from agentOrientation import AgentOrientation
-from queue import Queue
+from queue import PriorityQueue
 from turnCar import turn_left_orientation, turn_right_orientation
+import heapq
+
 
 class Succ:
     state: AgentState
     action: AgentActionType
-    ##cost: int
+    cost: int
 
-    def __init__(self, state: AgentState, action: AgentActionType) -> None:
+    def __init__(self, state: AgentState, action: AgentActionType, cost: int) -> None:
         self.state = state
         self.action = action
-        ##self.cost = cost
+        self.cost = cost
 
-def find_path_to_nearest_can(startState: AgentState, grid: Dict[Tuple[int, int], GridCellType]) -> list[AgentActionType]:
+
-    q: Queue[list[Succ]] = Queue()
+def find_path_to_nearest_can(startState: AgentState, grid: Dict[Tuple[int, int], GridCellType], city: City) -> list[AgentActionType]:
-    visited: list[AgentState] = []
+    pq: PriorityQueue[Tuple[int, List[Succ]]] = PriorityQueue()
-    startStates: list[Succ] = [Succ(startState, AgentActionType.UNKNOWN)]
+    visited: set[AgentState] = set()
-    q.put(startStates)
+    startStates: list[Succ] = [Succ(startState, AgentActionType.UNKNOWN, 0)]
-    while not q.empty():
+    pq.put((0, startStates))
-        currently_checked = q.get()
+
-        visited.append(currently_checked[-1].state)
+    while not pq.empty():
-        if is_state_success(currently_checked[-1].state, grid):
+        _, currently_checked = pq.get()
-            return extract_actions(currently_checked)
+        last_state = currently_checked[-1].state
-        successors = succ(currently_checked[-1].state)
+        if last_state in visited:
-        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:
-                    already_visited = True
-                    break
-            if already_visited:
             continue
-            if is_state_valid(s.state, grid):
+        visited.add(last_state)
+
+        if is_state_success(last_state, grid):
+            return extract_actions(currently_checked)
+
+        successors = succ(last_state)
+        for s in successors:
+            if s.state in visited:
+                continue
+            if not is_state_valid(s.state, grid):
+                continue
+
+            g_cost = currently_checked[-1].cost + get_cost_for_action(s.action, grid.get(s.state.position, GridCellType.STREET_HORIZONTAL))
+            h_cost = _heuristics(s.state.position, city)
+            f_cost = g_cost + h_cost
             new_list = currently_checked.copy()
             new_list.append(s)
-                q.put(new_list)
+            pq.put((f_cost, new_list))
-    return []
 
+    return []
 
 
 def extract_actions(successors: list[Succ]) -> list[AgentActionType]:
@@ -51,20 +60,23 @@ def extract_actions(successors: list[Succ]) -> list[AgentActionType]:
             output.append(s.action)
     return output
 
+
 def succ(state: AgentState) -> 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_left_orientation(state.orientation)), AgentActionType.TURN_LEFT, 0))
-    result.append(Succ(AgentState(state.position, turn_right_orientation(state.orientation)), AgentActionType.TURN_RIGHT))
+    result.append(Succ(AgentState(state.position, turn_right_orientation(state.orientation)), AgentActionType.TURN_RIGHT, 0))
     state_succ = move_forward_succ(state)
-    if state_succ != None:
+    if state_succ is not None:
-        result.append(move_forward_succ(state))
+        result.append(Succ(state_succ.state, AgentActionType.MOVE_FORWARD, state_succ.cost))
     return result
 
+
 def move_forward_succ(state: AgentState) -> Succ:
     position = get_next_cell(state)
-    if position == None:
+    if position is None:
         return None
-    return Succ(AgentState(position, state.orientation), AgentActionType.MOVE_FORWARD)
+    return Succ(AgentState(position, state.orientation), AgentActionType.MOVE_FORWARD,
+                get_cost_for_action(AgentActionType.MOVE_FORWARD, GridCellType.STREET_HORIZONTAL))
 
 
 def get_next_cell(state: AgentState) -> Tuple[int, int]:
@@ -84,13 +96,15 @@ 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:
         return grid[next_cell] == GridCellType.GARBAGE_CAN
-    except:
+    except KeyError:
         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
@@ -103,11 +117,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
+        return grid[state.position] == GridCellType.STREET_HORIZONTAL or grid[
-    except:
+            state.position] == GridCellType.STREET_VERTICAL or grid[state.position] == GridCellType.SPEED_BUMP
+    except KeyError:
         return False
 
-def _heuristics(position: Tuple[int, int], city: City):
+
+def _heuristics(position: Tuple[int, int], city: City) -> int:
     min_distance: int = 300
     found_nonvisited: bool = False
     for can in city.cans:
@@ -121,3 +137,4 @@ def _heuristics(position: Tuple[int, int], city: City):
         return min_distance
     return -1
 
+

city.py

@@ -4,6 +4,7 @@ from speedBump import SpeedBump
 from street import Street
 from gameContext import GameContext
 
+
 class City:
     cans: List[GarbageCan]
     bumps: List[SpeedBump]

main.py

@@ -1,4 +1,6 @@
 import pygame
+
+from city import City
 from gameEventHandler import handle_game_event 
 from gameContext import GameContext
 from startup import startup
@@ -17,8 +19,11 @@ game_context = GameContext()
 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)
+collect_garbage(game_context, city)
 
 exit = False
 

movement.py

@@ -9,19 +9,21 @@ from agentOrientation import AgentOrientation
 import pygame
 from bfs import find_path_to_nearest_can
 from agentState import AgentState
+from city import City
 
-def collect_garbage(game_context: GameContext) -> None:
+
+def collect_garbage(game_context: GameContext, city: City) -> 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, city)
-        if path == None or len(path) == 0:
+        if path is None or len(path) == 0:
             break
         move_dust_car(path, game_context)
         next_position = calculate_next_position(game_context.dust_car)
         game_context.grid[next_position] = GridCellType.VISITED_GARBAGE_CAN
-        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
@@ -39,11 +41,8 @@ def move_dust_car(actions: list[AgentActionType], game_context: GameContext) ->
                 game_context.render_in_cell(street_position, "imgs/street_horizontal.png")
             elif game_context.grid[street_position] == GridCellType.STREET_VERTICAL:
                 game_context.render_in_cell(street_position, "imgs/street_vertical.png")
-            elif game_context.grid[street_position] == GridCellType.SPEED_BUMP:
-                game_context.render_in_cell(street_position, "imgs/speed_bump.png")
         pygame.display.update()
-        time.sleep(0.15)
+        time.sleep(0.5)
-
 
 
 def calculate_next_position(car: GarbageTruck) -> Tuple[int, int]: