bfs.py

@@ -16,19 +16,19 @@ class Succ:
 
 def find_path_to_nearest_can(startState: AgentState, grid: Dict[Tuple[int, int], GridCellType]) -> list[AgentActionType]:
     q: Queue[list[Succ]] = Queue()
-    visited: list[Tuple[int, int]] = []
+    visited: list[AgentState] = []
     startStates: list[Succ] = [Succ(startState, AgentActionType.UNKNOWN)]
     q.put(startStates)
     while not q.empty():
         currently_checked = q.get()
-        visited.append(currently_checked[-1].state.position)
+        visited.append(currently_checked[-1].state)
         if is_state_success(currently_checked[-1].state, grid):
             return extract_actions(currently_checked)
         successors = succ(currently_checked[-1].state)
         for s in successors:
             already_visited = False
             for v in visited:
-                if v[0] == s.state.position[0] and v[1] == s.state.position[1]:
+                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: