bfs #19
7
agentOrientation.py
Normal file
7
agentOrientation.py
Normal file
@ -0,0 +1,7 @@
|
||||
from enum import Enum
|
||||
|
||||
class AgentOrientation (Enum):
|
||||
UP = 0
|
||||
RIGHT = 1
|
||||
DOWN = 2
|
||||
LEFT = 3
|
94
bfs.py
Normal file
94
bfs.py
Normal file
@ -0,0 +1,94 @@
|
||||
from agentState import AgentState
|
||||
from typing import Dict, Tuple
|
||||
from gridCellType import GridCellType
|
||||
from agentActionType import AgentActionType
|
||||
from agentOrientation import AgentOrientation
|
||||
from queue import Queue
|
||||
|
||||
class Succ:
|
||||
state: AgentState
|
||||
action: AgentActionType
|
||||
|
||||
def __init__(self, state: AgentState, action: AgentActionType) -> None:
|
||||
self.state = state
|
||||
self.action = action
|
||||
|
||||
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]] = []
|
||||
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)
|
||||
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]:
|
||||
already_visited = True
|
||||
break
|
||||
if already_visited:
|
||||
continue
|
||||
if is_state_valid(s.state, grid):
|
||||
new_list = currently_checked.copy()
|
||||
new_list.append(s)
|
||||
q.put(new_list)
|
||||
return []
|
||||
|
||||
|
||||
|
||||
def extract_actions(successors: list[Succ]) -> list[AgentActionType]:
|
||||
output: list[AgentActionType] = []
|
||||
for s in successors:
|
||||
if s.action != AgentActionType.UNKNOWN:
|
||||
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_right_orientation(state.orientation)), AgentActionType.TURN_RIGHT))
|
||||
state_succ = move_forward_succ(state)
|
||||
if state_succ != None:
|
||||
result.append(move_forward_succ(state))
|
||||
return result
|
||||
|
||||
def turn_left_orientation(orientation: AgentOrientation) -> AgentOrientation:
|
||||
return (orientation - 1) % 4
|
||||
|
||||
def turn_right_orientation(orientation: AgentOrientation) -> AgentOrientation:
|
||||
return (orientation + 1) % 4
|
||||
|
||||
def move_forward_succ(state: AgentState) -> Succ:
|
||||
position = get_next_cell(state)
|
||||
if position == None:
|
||||
return None
|
||||
return Succ(AgentState(position, state.orientation), AgentActionType.MOVE_FORWARD)
|
||||
|
||||
|
||||
def get_next_cell(state: AgentState) -> Tuple[int, int]:
|
||||
if state.orientation == AgentOrientation.UP:
|
||||
if state.position[1] - 1 < 1:
|
||||
return None
|
||||
return (state.position[0], state.position[1] - 1)
|
||||
if state.orientation == AgentOrientation.DOWN:
|
||||
if state.position[1] + 1 > 27:
|
||||
return None
|
||||
return (state.position[0], state.position[1] + 1)
|
||||
if state.orientation == AgentOrientation.LEFT:
|
||||
if state.position[0] - 1 < 1:
|
||||
return None
|
||||
return (state.position[0] - 1, state.position[1])
|
||||
if state.position[0] + 1 > 27:
|
||||
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)
|
||||
return grid[next_cell] == GridCellType.GARBAGE_CAN
|
||||
|
||||
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
|
Loading…
Reference in New Issue
Block a user