astar for MIkołaj Gawor

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]:
+class SuccList:
-    q: Queue[list[Succ]] = Queue()
+    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)
+        visited.append(currently_checked.succ_list[-1].state)
-        if is_state_success(currently_checked[-1].state, grid):
+        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))
+    turn_left_cost = 1 + succ.cost
-    result.append(Succ(AgentState(state.position, turn_right_orientation(state.orientation)), AgentActionType.TURN_RIGHT))
+    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)))
-    state_succ = move_forward_succ(state)
+    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:
+def move_forward_succ(succ: Succ, city: City, grid: Dict[Tuple[int, int], GridCellType]) -> Succ:
-    position = get_next_cell(state)
+    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]:

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:

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)