diff --git a/agentOrientation.py b/agentOrientation.py new file mode 100644 index 0000000..e9dc88a --- /dev/null +++ b/agentOrientation.py @@ -0,0 +1,7 @@ +from enum import Enum + +class AgentOrientation (Enum): + UP = 0 + RIGHT = 1 + DOWN = 2 + LEFT = 3 \ No newline at end of file diff --git a/bfs.py b/bfs.py new file mode 100644 index 0000000..eaa99fa --- /dev/null +++ b/bfs.py @@ -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 \ No newline at end of file