visit all garbage cans with bfs

bfs.py

@@ -83,7 +83,13 @@ def get_next_cell(state: AgentState) -> Tuple[int, int]:
 
 def is_state_success(state: AgentState, grid: Dict[Tuple[int, int], GridCellType]) -> bool:
     next_cell = get_next_cell(state)
-    return grid[next_cell] == GridCellType.GARBAGE_CAN
+    try:
+        return grid[next_cell] == GridCellType.GARBAGE_CAN
+    except:
+        return False
 
 def is_state_valid(state: AgentState, grid: Dict[Tuple[int, int], GridCellType]) -> bool:
-    return grid[state.position] == GridCellType.STREET_HORIZONTAL or grid[state.position] == GridCellType.STREET_VERTICAL
+    try: 
+        return grid[state.position] == GridCellType.STREET_HORIZONTAL or grid[state.position] == GridCellType.STREET_VERTICAL
+    except:
+        return False

main.py

@@ -4,7 +4,7 @@ from gameContext import GameContext
 from startup import startup
 from PIL import Image
 from agentActionType import AgentActionType
-from movement import move_dust_car
+from movement import collect_garbage
 
 pygame.init()
 
@@ -18,7 +18,7 @@ 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
 startup(game_context)
-
+collect_garbage(game_context)
 # test = [AgentActionType.MOVE_FORWARD, AgentActionType.MOVE_FORWARD, AgentActionType.MOVE_FORWARD,
 #         AgentActionType.MOVE_FORWARD, AgentActionType.MOVE_FORWARD, AgentActionType.MOVE_FORWARD,
 #         AgentActionType.MOVE_FORWARD, AgentActionType.TURN_RIGHT, AgentActionType.MOVE_FORWARD,

movement.py

@@ -7,6 +7,19 @@ from gridCellType import GridCellType
 from gameContext import GameContext
 from agentOrientation import AgentOrientation
 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)
+        path = find_path_to_nearest_can(start_agent_state, game_context.grid)
+        if path == 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
+    pass
 
 def move_dust_car(actions: list[AgentActionType], game_context: GameContext) -> None:
     for action in actions: