inteligentny-traktor/astar.py
2023-06-15 14:03:24 +02:00

196 lines
5.9 KiB
Python

from operator import itemgetter
import cart
import copy
class Istate:
def __init__(self, direction, x, y):
self.direction = direction
self.x = x
self.y = y
def get_direction(self):
return self.direction
def set_direction(self, direction):
self.direction = direction
def get_x(self):
return self.x
def set_x(self, x):
self.x = x
def get_y(self):
return self.y
def set_y(self, y):
self.y = y
class Node:
def __init__(self, action, direction, parent, x, y):
self.action = action
self.direction = direction
self.parent = parent
self.x = x
self.y = y
def get_action(self):
return self.action
def set_action(self, action):
self.action = action
def get_direction(self):
return self.direction
def set_direction(self, direction):
self.direction = direction
def get_parent(self):
return self.parent
def set_parent(self, parent):
self.parent = parent
def get_x(self):
return self.x
def set_x(self, x):
self.x = x
def get_y(self):
return self.y
def set_y(self, y):
self.y = y
def fieldCost(T, node):
c = 0
if T[node.x-1][node.y-1].plantType == 1:
c = 2
elif T[node.x-1][node.y-1].plantType == 2:
c = 5
elif T[node.x-1][node.y-1].plantType == 3:
c = 13
elif T[node.x-1][node.y-1].plantType == 4:
c = 100000
else:
c = 0
if T[node.x-1][node.y-1].isWet == 1:
c = c + 4
else:
c = c+1
return c
def cost(T, node):
cost = 0
while node.get_parent() is not None:
cost = cost + fieldCost(T, node)
node = node.get_parent()
return cost
def f(goaltest, map, node):
return cost(map, node) + heuristic(goaltest, node)
def goal_test(elem, goaltest):
if elem.get_x() == goaltest[0] and elem.get_y() == goaltest[1]:
return True
else:
return False
def graphsearch(explored, f, fringe, goaltest, istate, map, succ): # przeszukiwanie grafu wszerz
node = Node(None, istate.get_direction(), None, istate.get_x(), istate.get_y())
fringe.append((node, 0)) # wierzchołki do odwiedzenia z priorytetem
while True:
if not fringe:
return False
elem = fringe.pop(0) # zdejmujemy wierzchołek z kolejki fringe i rozpatrujemy go
temp = copy.copy(elem[0])
if goal_test(elem[0], goaltest) is True: # jeżeli osiągniemy cel w trakcie przeszukiwania grafu wszerz (wjedziemy na pole docelowe) : zwracamy listę ruchów, po których wykonaniu dotrzemy na miejsce
return print_moves(elem[0])
explored.append(elem) # dodajemy wierzchołek do listy wierzchołków odwiedzonych
for (action, state) in succ(temp): # iterujemy po wszystkich możliwych akcjach i stanach otrzymanych dla danego wierzchołka grafu
fringe_tuple = []
fringe_tuple_prio = []
explored_tuple = []
for (x, y) in fringe:
fringe_tuple.append((x.get_direction(), x.get_x(), x.get_y()))
fringe_tuple_prio.append(((x.get_direction(), x.get_x(), x.get_y()), y))
for (x, y) in explored:
explored_tuple.append((x.get_direction(), x.get_x(), x.get_y()))
x = Node(action, state[0], elem[0], state[1], state[2]) # stworzenie nowego wierzchołka, którego rodzicem jest elem
p = f(goaltest, map, x) # liczy priorytet
# print('Koszt =', p)
if state not in fringe_tuple and state not in explored_tuple: # jeżeli stan nie znajduje się na fringe oraz nie znajduje się w liście wierzchołków odwiedzonych
fringe.append((x, p)) # dodanie wierzchołka na fringe
fringe = sorted(fringe, key=itemgetter(1)) # sortowanie fringe'a według priorytetu
elif state in fringe_tuple:
i = 0
for (state_prio, r) in fringe_tuple_prio:
if str(state_prio) == str(state):
if r > p:
fringe.insert(i, (x, p)) # zamiana state, który należy do fringe z priorytetem r na state z priorytetem p (niższym)
fringe.pop(i + 1)
fringe = sorted(fringe, key=itemgetter(1)) # sortowanie fringe'a według priorytetu
break
i = i + 1
def heuristic(goaltest, node):
return abs(node.get_x() - goaltest[0]) + abs(node.get_y() - goaltest[1])
def print_moves(elem):
moves_list = []
while elem.get_parent() is not None:
moves_list.append(elem.get_action())
elem = elem.get_parent()
moves_list.reverse()
return moves_list
def succ(elem):
actions_list = []
temp = copy.copy(elem.get_direction())
if temp == 1:
temp = 4
else:
temp = temp - 1
actions_list.append(("rotate_right", (temp, elem.get_x(), elem.get_y())))
temp = copy.copy(elem.get_direction())
if temp == 4:
temp = 1
else:
temp = temp + 1
actions_list.append(("rotate_left", (temp, elem.get_x(), elem.get_y())))
temp_move_south = elem.get_y() - 1
temp_move_west = elem.get_x() - 1
temp_move_east = elem.get_x() + 1
temp_move_north = elem.get_y() + 1
if cart.Cart.is_move_allowed_succ(elem) == "x + 1":
actions_list.append(("move", (elem.get_direction(), temp_move_east, elem.get_y())))
elif cart.Cart.is_move_allowed_succ(elem) == "y + 1":
actions_list.append(("move", (elem.get_direction(), elem.get_x(), temp_move_north)))
elif cart.Cart.is_move_allowed_succ(elem) == "y - 1":
actions_list.append(("move", (elem.get_direction(), elem.get_x(), temp_move_south)))
elif cart.Cart.is_move_allowed_succ(elem) == "x - 1":
actions_list.append(("move", (elem.get_direction(), temp_move_west, elem.get_y())))
return actions_list