183 lines
9.8 KiB
Python
183 lines
9.8 KiB
Python
import heapq # dla utrzymania fringe
|
|
from classes import node, minesweeper, system
|
|
import time
|
|
|
|
|
|
class BFS:
|
|
window: system.Window
|
|
agent: minesweeper.Minesweeper
|
|
node: node.Node
|
|
|
|
def __init__(self, agent, window):
|
|
self.agent = agent
|
|
self.window = window
|
|
|
|
def successor(self, current_position):
|
|
new_nodes = []
|
|
neighbours_list = self.agent.sensor(current_position[0], current_position[1])
|
|
#print(f'Current position {current_position}')
|
|
#print(neighbours_list[0])
|
|
#print(neighbours_list[1])
|
|
#print(neighbours_list[2])
|
|
|
|
|
|
# if neighbours_list[1][0] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
|
|
# new_nodes.append([current_position[0] - 1, current_position[1]])
|
|
# if neighbours_list[2][1] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
|
|
# new_nodes.append([current_position[0], current_position[1] + 1])
|
|
# if neighbours_list[1][2] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
|
|
# new_nodes.append([current_position[0] + 1, current_position[1]])
|
|
# if neighbours_list[0][1] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
|
|
# new_nodes.append([current_position[0],current_position[1] - 1])
|
|
|
|
# if current_position[2] == 0: # jesli patrzy na polnoc
|
|
# if neighbours_list[1][0] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
|
|
# tmp = ('forward', [current_position[0] - 1, current_position[1], 0])
|
|
# new_nodes.append(tmp)
|
|
# if neighbours_list[0][1] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
|
|
# tmp = ('left', [current_position[0],current_position[1], 270])
|
|
# new_nodes.append(tmp)
|
|
# if neighbours_list[2][1] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
|
|
# tmp = ('right', [current_position[0], current_position[1], 90])
|
|
# new_nodes.append(tmp)
|
|
#
|
|
# if current_position[2] == 90: # jesli patrzy na wschod
|
|
# if neighbours_list[2][1] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
|
|
# tmp = ('forward', [current_position[0], current_position[1] + 1, 90])
|
|
# new_nodes.append(tmp)
|
|
# if neighbours_list[1][0] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
|
|
# tmp = ('left', [current_position[0], current_position[1], 0])
|
|
# new_nodes.append(tmp)
|
|
# if neighbours_list[1][2] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
|
|
# tmp = ('right', [current_position[0], current_position[1], 180])
|
|
# new_nodes.append(tmp)
|
|
#
|
|
# if current_position[2] == 180: # jesli patczy na poludzie
|
|
# if neighbours_list[1][2] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
|
|
# tmp = ('forward', [current_position[0] + 1, current_position[1], 180])
|
|
# new_nodes.append(tmp)
|
|
# if neighbours_list[2][1] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
|
|
# tmp = ('left', [current_position[0], current_position[1], 90])
|
|
# new_nodes.append(tmp)
|
|
# if neighbours_list[0][1] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
|
|
# tmp = ('right', [current_position[0],current_position[1], 270])
|
|
# new_nodes.append(tmp)
|
|
#
|
|
# if current_position[2] == 270: # jesli patczy na wschod
|
|
# if neighbours_list[0][1] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
|
|
# tmp = ('forward', [current_position[0],current_position[1] - 1, 270])
|
|
# new_nodes.append(tmp)
|
|
# if neighbours_list[1][2] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
|
|
# tmp = ('left', [current_position[0], current_position[1], 180])
|
|
# new_nodes.append(tmp)
|
|
# if neighbours_list[1][0] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
|
|
# tmp = ('right', [current_position[0], current_position[1], 0])
|
|
# new_nodes.append(tmp)
|
|
|
|
if current_position[2] == 180: # jesli patrzy na polnoc
|
|
if neighbours_list[0][1] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
|
|
tmp = ('forward', [current_position[0], current_position[1] - 1, 180])
|
|
new_nodes.append(tmp)
|
|
if neighbours_list[1][0] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
|
|
tmp = ('left', [current_position[0],current_position[1], 270])
|
|
new_nodes.append(tmp)
|
|
if neighbours_list[1][2] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
|
|
tmp = ('right', [current_position[0], current_position[1], 90])
|
|
new_nodes.append(tmp)
|
|
|
|
if current_position[2] == 90: # jesli patrzy na wschod
|
|
if neighbours_list[1][2] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
|
|
tmp = ('forward', [current_position[0] + 1, current_position[1], 90])
|
|
new_nodes.append(tmp)
|
|
if neighbours_list[0][1] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
|
|
tmp = ('left', [current_position[0], current_position[1], 180])
|
|
new_nodes.append(tmp)
|
|
if neighbours_list[2][1] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
|
|
tmp = ('right', [current_position[0], current_position[1], 0])
|
|
new_nodes.append(tmp)
|
|
|
|
if current_position[2] == 0: # jesli patczy na poludzie
|
|
if neighbours_list[2][1] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
|
|
tmp = ('forward', [current_position[0], current_position[1] + 1, 0])
|
|
new_nodes.append(tmp)
|
|
if neighbours_list[1][2] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
|
|
tmp = ('left', [current_position[0], current_position[1], 90])
|
|
new_nodes.append(tmp)
|
|
if neighbours_list[1][0] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
|
|
tmp = ('right', [current_position[0],current_position[1], 270])
|
|
new_nodes.append(tmp)
|
|
|
|
if current_position[2] == 270: # jesli patczy na wschod
|
|
if neighbours_list[1][0] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
|
|
tmp = ('forward', [current_position[0] - 1,current_position[1], 270])
|
|
new_nodes.append(tmp)
|
|
if neighbours_list[2][1] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
|
|
tmp = ('left', [current_position[0], current_position[1], 0])
|
|
new_nodes.append(tmp)
|
|
if neighbours_list[0][1] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
|
|
tmp = ('right', [current_position[0], current_position[1], 180])
|
|
new_nodes.append(tmp)
|
|
|
|
return new_nodes
|
|
|
|
|
|
|
|
# fringe = struktura danych przeszowyjąca wierchowki do odwiedzenia
|
|
# explored = lista odwiedzonych stanow
|
|
# position_at_beginning = stan poczatkowy
|
|
# succ = funkcja nastempnika
|
|
# goaltest = test spewnienia celu
|
|
|
|
def graphsearch(self, fringe, explored, succ, goaltest):
|
|
self.window.pause(True)
|
|
|
|
positiont_at_beginning = [self.agent.position_x, self.agent.position_y, self.agent.rotation_degrees] # x, y, gdzie_patczy
|
|
print(f'Position {positiont_at_beginning}')
|
|
final_action_list = [] # lista co ma robic zeby dojechac do miny
|
|
root = node.Node(None, None, positiont_at_beginning) # parent, action, position
|
|
counter = 0
|
|
heapq.heappush(fringe, (counter, root))
|
|
#visited_position = []
|
|
|
|
|
|
while len(fringe) != 0:
|
|
flag = True
|
|
|
|
|
|
if len(fringe) == 0:
|
|
return False
|
|
|
|
tmp_node = heapq.heappop(fringe) # node
|
|
tmp_node_position = tmp_node[1].get_position() # x, y , gdzie patczy
|
|
#print(f'Position before succ {tmp_node_position}')
|
|
|
|
#visited_position.append(tmp_node_position)
|
|
explored.append(tmp_node_position)
|
|
|
|
if tmp_node_position[:2] == goaltest:
|
|
print('Find')
|
|
|
|
while tmp_node[1].get_parent() is not None:
|
|
final_action_list.append(tmp_node[1].get_action())
|
|
tmp_node = tmp_node[1].get_parent()
|
|
final_action_list.reverse()
|
|
print(final_action_list)
|
|
self.window.pause(False)
|
|
return final_action_list
|
|
|
|
# nie wiem na razie po co to
|
|
# chyba visited_position lepszy odpowiednik
|
|
# explored.append(tmp_node)
|
|
|
|
|
|
neighbours_list_of_our_node = self.successor(tmp_node_position) # lista możliwych akcij
|
|
#print(neighbours_list_of_our_node)
|
|
|
|
for node_ in neighbours_list_of_our_node:
|
|
# jesli pozucja wezla nie jest w fringe i nie jest w visited
|
|
if [node_[1][0], node_[1][1], node_[1][2]] not in explored and node_[1][0] >= 0 and node_[1][1] >=0:
|
|
counter += 1
|
|
x = node.Node(tmp_node, node_[0], node_[1]) # action
|
|
heapq.heappush(fringe, (counter, x))
|
|
self.window.draw_search([self.agent.position_x, self.agent.position_y], [node_[1][0], node_[1][1]])
|