Male_zoo_Projekt_SI/state_space_search.py

100 lines
3.3 KiB
Python
Raw Normal View History

2024-04-26 15:46:02 +02:00
from queue import PriorityQueue
DEFAULT_COST_VALUE = 1
def is_border(x, y, max_x, max_y):
return 0 <= x < max_x and 0 <= y < max_y
def is_obstacle(x, y, obstacles):
return (x, y) in obstacles
def succ(current_state, max_x, max_y, obstacles):
successors = []
x, y, direction = current_state
# Akcja: Do przodu
direction_x, direction_y = {'N': (0, -1), 'E': (1, 0), 'S': (0, 1), 'W': (-1, 0)}[direction] # Słownik przesunięć w zależności od kierunku
new_x, new_y = x + direction_x, y + direction_y
if is_border(new_x, new_y, max_x, max_y) and not(is_obstacle(new_x, new_y, obstacles)):
successors.append(((new_x, new_y, direction), 'Go Forward'))
# Akcja: Obrót w lewo
left_turns = {'N': 'W', 'W': 'S', 'S': 'E', 'E': 'N'} # Słownik kierunków po obrocie w lewo
successors.append(((x, y, left_turns[direction]), 'Turn Left'))
# Akcja: Obrót w prawo
right_turns = {'N': 'E', 'E': 'S', 'S': 'W', 'W': 'N'} # Słownik kierunków po obrocie w prawo
successors.append(((x, y, right_turns[direction]), 'Turn Right'))
return successors
2024-04-26 15:46:02 +02:00
def graphsearch(istate, goal, max_x, max_y, obstacles, cost_map):
fringe = PriorityQueue()
explored = set()
2024-04-26 15:46:02 +02:00
fringe.put((0, (istate, None , None)))
while not fringe.empty():
_, node = fringe.get()
state, _, _ = node
if goaltest(state, goal):
2024-04-26 15:46:02 +02:00
return build_action_sequence(node)
explored.add(state)
successors = succ(state, max_x, max_y, obstacles)
for new_state, action in successors:
2024-04-26 15:46:02 +02:00
new_node = (new_state, node, action)
p_new_state = current_cost(node, cost_map) + heuristic(state, goal)
if not is_state_in_queue(new_state, fringe) and new_state not in explored:
fringe.put((p_new_state, new_node))
elif is_state_in_queue(new_state, fringe):
for i, (p_existing_state, (existing_state, _, _)) in enumerate(fringe.queue):
if existing_state == new_state and p_existing_state > p_new_state:
fringe.queue[i] = (p_new_state, new_node)
else:
break
return False
2024-04-26 15:46:02 +02:00
def is_state_in_queue(state, queue):
for _, (s, _, _) in queue.queue:
if s == state:
return True
return False
def build_action_sequence(node):
actions = []
2024-04-26 15:46:02 +02:00
while node[1] is not None: # Dopóki nie dojdziemy do korzenia
_, parent, action = node
actions.append(action)
node = parent
actions.reverse()
return actions
def goaltest(state, goal):
x, y, _ = state
goal_x, goal_y = goal
2024-04-26 15:46:02 +02:00
return (x,y) == (goal_x, goal_y)
def current_cost(node, cost_map):
cost = 0
while node[1] is not None: # Dopóki nie dojdziemy do korzenia
_, parent, action = node
# Dodaj koszt pola z mapy kosztów tylko jeśli akcja to "Forward"
if action == 'Go Forward':
state, _, _ = node
cost += cost_map.get(state[:2], DEFAULT_COST_VALUE) # Pobiera koszt przejścia przez dane pole, a jeśli koszt nie jest zdefiniowany to bierze wartość domyślną
node = parent # Przejdź do rodzica
return cost
def heuristic(state, goal):
x, y, _ = state
goal_x, goal_y = goal
return abs(x - goal_x) + abs(y - goal_y) # Odległość Manhattana do celu