141 lines
4.8 KiB
Python
141 lines
4.8 KiB
Python
from agentState import AgentState
|
|
from typing import Dict, Tuple, List, Set
|
|
from city import City
|
|
from gridCellType import GridCellType
|
|
from agentActionType import AgentActionType
|
|
from agentOrientation import AgentOrientation
|
|
from queue import PriorityQueue
|
|
from turnCar import turn_left_orientation, turn_right_orientation
|
|
import heapq
|
|
|
|
|
|
class Succ:
|
|
state: AgentState
|
|
action: AgentActionType
|
|
cost: int
|
|
|
|
def __init__(self, state: AgentState, action: AgentActionType, cost: int) -> None:
|
|
self.state = state
|
|
self.action = action
|
|
self.cost = cost
|
|
|
|
|
|
def find_path_to_nearest_can(startState: AgentState, grid: Dict[Tuple[int, int], GridCellType], city: City) -> list[AgentActionType]:
|
|
pq: PriorityQueue[Tuple[int, List[Succ]]] = PriorityQueue()
|
|
visited: set[AgentState] = set()
|
|
startStates: list[Succ] = [Succ(startState, AgentActionType.UNKNOWN, 0)]
|
|
pq.put((0, startStates))
|
|
|
|
while not pq.empty():
|
|
_, currently_checked = pq.get()
|
|
last_state = currently_checked[-1].state
|
|
if last_state in visited:
|
|
continue
|
|
visited.add(last_state)
|
|
|
|
if is_state_success(last_state, grid):
|
|
return extract_actions(currently_checked)
|
|
|
|
successors = succ(last_state)
|
|
for s in successors:
|
|
if s.state in visited:
|
|
continue
|
|
if not is_state_valid(s.state, grid):
|
|
continue
|
|
|
|
g_cost = currently_checked[-1].cost + get_cost_for_action(s.action, grid.get(s.state.position, GridCellType.STREET_HORIZONTAL))
|
|
h_cost = _heuristics(s.state.position, city)
|
|
f_cost = g_cost + h_cost
|
|
new_list = currently_checked.copy()
|
|
new_list.append(s)
|
|
pq.put((f_cost, 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, 0))
|
|
result.append(Succ(AgentState(state.position, turn_right_orientation(state.orientation)), AgentActionType.TURN_RIGHT, 0))
|
|
state_succ = move_forward_succ(state)
|
|
if state_succ is not None:
|
|
result.append(Succ(state_succ.state, AgentActionType.MOVE_FORWARD, state_succ.cost))
|
|
return result
|
|
|
|
|
|
def move_forward_succ(state: AgentState) -> Succ:
|
|
position = get_next_cell(state)
|
|
if position is None:
|
|
return None
|
|
return Succ(AgentState(position, state.orientation), AgentActionType.MOVE_FORWARD,
|
|
get_cost_for_action(AgentActionType.MOVE_FORWARD, GridCellType.STREET_HORIZONTAL))
|
|
|
|
|
|
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)
|
|
try:
|
|
return grid[next_cell] == GridCellType.GARBAGE_CAN
|
|
except KeyError:
|
|
return False
|
|
|
|
|
|
def get_cost_for_action(action: AgentActionType, cell_type: GridCellType) -> int:
|
|
if action == AgentActionType.TURN_LEFT or action == AgentActionType.TURN_RIGHT:
|
|
return 1
|
|
if cell_type == GridCellType.SPEED_BUMP:
|
|
if action == AgentActionType.MOVE_FORWARD:
|
|
return 10
|
|
if action == AgentActionType.MOVE_FORWARD:
|
|
return 3
|
|
|
|
|
|
def is_state_valid(state: AgentState, grid: Dict[Tuple[int, int], GridCellType]) -> bool:
|
|
try:
|
|
return grid[state.position] == GridCellType.STREET_HORIZONTAL or grid[
|
|
state.position] == GridCellType.STREET_VERTICAL or grid[state.position] == GridCellType.SPEED_BUMP
|
|
except KeyError:
|
|
return False
|
|
|
|
|
|
def _heuristics(position: Tuple[int, int], city: City) -> int:
|
|
min_distance: int = 300
|
|
found_nonvisited: bool = False
|
|
for can in city.cans:
|
|
if can.is_visited:
|
|
continue
|
|
found_nonvisited = True
|
|
distance = 3 * (abs(position[0] - can.position[0]) + abs(position[1] - can.position[1]))
|
|
if distance < min_distance:
|
|
min_distance = distance
|
|
if found_nonvisited:
|
|
return min_distance
|
|
return -1
|
|
|
|
|