87 lines
2.4 KiB
Python
87 lines
2.4 KiB
Python
from __future__ import annotations
|
|
from typing import List, Set
|
|
|
|
from project_constants import Direction, Action
|
|
|
|
# temporary goal for testing
|
|
GOAL = (10, 10)
|
|
|
|
|
|
class State:
|
|
def __init__(self, row, column, direction: Direction):
|
|
self.row = row
|
|
self.column = column
|
|
self.direction = direction
|
|
|
|
|
|
class Node:
|
|
def __init__(self, state: State, parent: Node = None, action: Action = None):
|
|
self.state = state
|
|
self.parent = parent
|
|
self.action = action
|
|
|
|
|
|
def goal_test(state: State):
|
|
if (state.row, state.column) == GOAL:
|
|
return True
|
|
return False
|
|
|
|
|
|
# TODO: depends od Agent Class (rotate/go implementation)
|
|
def get_successors(state: State):
|
|
successors = list()
|
|
|
|
# state_left = get Agent State after rotate_left()
|
|
# successors.add("rotate_left", state_left)
|
|
|
|
# state_right = get Agent State after rotate_right()
|
|
# successors.add("rotate_right", state_right)
|
|
|
|
# target = get Agent position after go()
|
|
# if is_valid_move(target):
|
|
# state_go = Agent State after go()
|
|
# successors.add("go", state_go)
|
|
|
|
return successors
|
|
|
|
|
|
def graphsearch(fringe: List[Node], explored: Set[Node], initial_state: State):
|
|
|
|
fringe.append(Node(initial_state))
|
|
|
|
while True:
|
|
# fringe empty -> solution not found
|
|
if not len(fringe):
|
|
return False
|
|
|
|
element = fringe.pop(0)
|
|
|
|
# solution found, prepare and return actions sequence
|
|
if goal_test(element.state):
|
|
actions_sequence = list()
|
|
parent = element.parent
|
|
|
|
while parent is not None:
|
|
# root's action will be None, don't add it
|
|
if parent.action is not None:
|
|
actions_sequence.append(parent.action)
|
|
parent = parent.parent
|
|
|
|
return actions_sequence.reverse()
|
|
|
|
explored.add(element)
|
|
|
|
for successor in get_successors(element.state):
|
|
# either me or the pseudocode is dumb
|
|
# somebody has to verify it
|
|
fringe_states = [el.state for el in fringe]
|
|
explored_states = [el.state for el in explored]
|
|
|
|
if successor.state not in fringe_states and \
|
|
successor.state not in explored_states:
|
|
|
|
new_node = Node(state=successor.state,
|
|
parent=element,
|
|
action=successor.action)
|
|
fringe.append(new_node)
|