This commit is contained in:
Pawel Felcyn 2023-04-22 18:06:50 +02:00
parent c5e6eadcae
commit c22e86e22b

6
bfs.py
View File

@ -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: