From 4d91d86aef67e8c219407901d44c129ef5d84527 Mon Sep 17 00:00:00 2001 From: Yurii Date: Thu, 28 Apr 2022 09:32:14 +0200 Subject: [PATCH] a star, robot uwzglednia koszt --- classes/ai.py | 10 +- classes/bfs.py | 265 ++++++++++++++++++----------------------- classes/minesweeper.py | 2 +- classes/node.py | 3 + 4 files changed, 129 insertions(+), 151 deletions(-) diff --git a/classes/ai.py b/classes/ai.py index 5737878..461cfe0 100644 --- a/classes/ai.py +++ b/classes/ai.py @@ -89,11 +89,17 @@ class AI: def bfs(self): dists = [] # how long way is to all mines from start([0, 0]) for mine in self.current_map.mines: - dists.append(sqrt((self.saper.position_x-mine.position_x)**2 + (self.saper.position_y-mine.position_y)**2)) + # dists.append(sqrt((self.saper.position_x-mine.position_x)**2 + (self.saper.position_y-mine.position_y)**2)) + dists.append(abs(self.saper.position_x - mine.position_x) + abs(self.saper.position_y - mine.position_y)) print(dists) ind = dists.index(min(dists)) # choose nearest mine goal_state = [self.current_map.mines[ind].position_x, self.current_map.mines[ind].position_y] print(f'We go to {goal_state}') find_path = bfs.BFS(self.saper, self.window) - self.the_way = find_path.graphsearch([], [], bfs.BFS.successor, goal_state) \ No newline at end of file + tmp = find_path.graphsearch([], [], bfs.BFS.successor, goal_state) + print(f'tmp = {tmp}') + if tmp is None: + raise Exception("Error, path does not exist") + else: + self.the_way = tmp \ No newline at end of file diff --git a/classes/bfs.py b/classes/bfs.py index a2400af..ee92622 100644 --- a/classes/bfs.py +++ b/classes/bfs.py @@ -1,6 +1,5 @@ import heapq # dla utrzymania fringe from classes import node, minesweeper, system -import time class BFS: @@ -15,180 +14,122 @@ class BFS: 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]) - #print(neighbours_list) - # 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']: if neighbours_list[0][1] == 'grass': - cost = 5 + cost = 10 elif neighbours_list[0][1] == 'sand': - cost = 1 + cost = 2 elif neighbours_list[0][1] == 'mine': cost = 0 tmp = ('forward', [current_position[0], current_position[1] - 1, 180], cost) new_nodes.append(tmp) if neighbours_list[1][0] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']: - if neighbours_list[1][0] == 'grass': - cost = 5 - elif neighbours_list[1][0] == 'sand': - cost = 1 - elif neighbours_list[1][0] == 'mine': - cost = 0 - tmp = ('left', [current_position[0],current_position[1], 270], cost) + # if neighbours_list[1][0] == 'grass': + # cost = 10 + # elif neighbours_list[1][0] == 'sand': + # cost = 2 + # elif neighbours_list[1][0] == 'mine': + # cost = 0 + tmp = ('left', [current_position[0],current_position[1], 270], 1) new_nodes.append(tmp) if neighbours_list[1][2] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']: - if neighbours_list[1][2] == 'grass': - cost = 5 - elif neighbours_list[1][2] == 'sand': - cost = 1 - elif neighbours_list[1][2] == 'mine': - cost = 0 - tmp = ('right', [current_position[0], current_position[1], 90], cost) + # if neighbours_list[1][2] == 'grass': + # cost = 10 + # elif neighbours_list[1][2] == 'sand': + # cost = 2 + # elif neighbours_list[1][2] == 'mine': + # cost = 0 + tmp = ('right', [current_position[0], current_position[1], 90], 1) 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']: if neighbours_list[1][2] == 'grass': - cost = 5 + cost = 10 elif neighbours_list[1][2] == 'sand': - cost = 1 + cost = 2 elif neighbours_list[1][2] == 'mine': cost = 0 tmp = ('forward', [current_position[0] + 1, current_position[1], 90], cost) new_nodes.append(tmp) if neighbours_list[0][1] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']: - if neighbours_list[0][1] == 'grass': - cost = 5 - elif neighbours_list[0][1] == 'sand': - cost = 1 - elif neighbours_list[0][1] == 'mine': - cost = 0 - tmp = ('left', [current_position[0], current_position[1], 180], cost) + # if neighbours_list[0][1] == 'grass': + # cost = 10 + # elif neighbours_list[0][1] == 'sand': + # cost = 2 + # elif neighbours_list[0][1] == 'mine': + # cost = 0 + tmp = ('left', [current_position[0], current_position[1], 180], 1) new_nodes.append(tmp) if neighbours_list[2][1] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']: - if neighbours_list[2][1] == 'grass': - cost = 5 - elif neighbours_list[2][1] == 'sand': - cost = 1 - elif neighbours_list[2][1] == 'mine': - cost = 0 - tmp = ('right', [current_position[0], current_position[1], 0], cost) + # if neighbours_list[2][1] == 'grass': + # cost = 10 + # elif neighbours_list[2][1] == 'sand': + # cost = 2 + # elif neighbours_list[2][1] == 'mine': + # cost = 0 + tmp = ('right', [current_position[0], current_position[1], 0], 1) 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']: if neighbours_list[2][1] == 'grass': - cost = 5 + cost = 10 elif neighbours_list[2][1] == 'sand': - cost = 1 + cost = 2 elif neighbours_list[2][1] == 'mine': cost = 0 tmp = ('forward', [current_position[0], current_position[1] + 1, 0], cost) new_nodes.append(tmp) if neighbours_list[1][2] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']: - if neighbours_list[1][2] == 'grass': - cost = 5 - elif neighbours_list[1][2] == 'sand': - cost = 1 - elif neighbours_list[1][2] == 'mine': - cost = 0 - tmp = ('left', [current_position[0], current_position[1], 90], cost) + # if neighbours_list[1][2] == 'grass': + # cost = 10 + # elif neighbours_list[1][2] == 'sand': + # cost = 2 + # elif neighbours_list[1][2] == 'mine': + # cost = 0 + tmp = ('left', [current_position[0], current_position[1], 90], 1) new_nodes.append(tmp) if neighbours_list[1][0] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']: - if neighbours_list[1][0] == 'grass': - cost = 5 - elif neighbours_list[1][0] == 'sand': - cost = 1 - elif neighbours_list[1][0] == 'mine': - cost = 0 - tmp = ('right', [current_position[0],current_position[1], 270], cost) + # if neighbours_list[1][0] == 'grass': + # cost = 10 + # elif neighbours_list[1][0] == 'sand': + # cost = 2 + # elif neighbours_list[1][0] == 'mine': + # cost = 0 + tmp = ('right', [current_position[0],current_position[1], 270], 1) 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']: if neighbours_list[1][0] == 'grass': - cost = 5 + cost = 10 elif neighbours_list[1][0] == 'sand': - cost = 1 + cost = 2 elif neighbours_list[1][0] == 'mine': cost = 0 tmp = ('forward', [current_position[0] - 1,current_position[1], 270], cost) new_nodes.append(tmp) if neighbours_list[2][1] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']: - if neighbours_list[2][1] == 'grass': - cost = 5 - elif neighbours_list[2][1] == 'sand': - cost = 1 - elif neighbours_list[2][1] == 'mine': - cost = 0 - tmp = ('left', [current_position[0], current_position[1], 0], cost) + # if neighbours_list[2][1] == 'grass': + # cost = 10 + # elif neighbours_list[2][1] == 'sand': + # cost = 2 + # elif neighbours_list[2][1] == 'mine': + # cost = 0 + tmp = ('left', [current_position[0], current_position[1], 0], 1) new_nodes.append(tmp) if neighbours_list[0][1] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']: - if neighbours_list[0][1] == 'grass': - cost = 5 - elif neighbours_list[0][1] == 'sand': - cost = 1 - elif neighbours_list[0][1] == 'mine': - cost = 0 - tmp = ('right', [current_position[0], current_position[1], 180], cost) + # if neighbours_list[0][1] == 'grass': + # cost = 10 + # elif neighbours_list[0][1] == 'sand': + # cost = 2 + # elif neighbours_list[0][1] == 'mine': + # cost = 0 + tmp = ('right', [current_position[0], current_position[1], 180], 1) new_nodes.append(tmp) return new_nodes @@ -208,29 +149,25 @@ class BFS: 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}') + position_at_beginning = [self.agent.position_x, self.agent.position_y, self.agent.rotation_degrees] # x, y, gdzie_patczy final_action_list = [] # lista co ma robic zeby dojechac do miny - root = node.Node(None, None, positiont_at_beginning, 0) # parent, action, position - counter = 0 - heapq.heappush(fringe, (counter, root)) - #visited_position = [] + root = node.Node(None, None, position_at_beginning, 0) # parent, action, position, cost + + heapq.heappush(fringe, (0, root)) # add first node to fringe - while len(fringe) != 0: - flag = True + while len(fringe) != 0: # poki sa wezly do odwiedzenia(na fringe) if len(fringe) == 0: return False - tmp_node = heapq.heappop(fringe) # node + # get node from fringe + tmp_node = heapq.heappop(fringe) # tuple(priority, 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) + # jesli tmp node to goaltest if tmp_node_position[:2] == goaltest: print('Find') @@ -238,28 +175,60 @@ class BFS: final_action_list.append(tmp_node[1].get_action()) tmp_node = tmp_node[1].get_parent() final_action_list.reverse() - print(final_action_list) + #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) + explored.append(tmp_node[1]) # add node to array of visited nodes + 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], node_[2]) # action - heapq.heappush(fringe, (counter, x)) - self.window.draw_search([self.agent.position_x, self.agent.position_y], [node_[1][0], node_[1][1]], self.agent.current_map.tile_size, self.agent.current_map, self.agent) - p = manhattan(node_[1], goaltest) + node_[2] - # print(f'Mancgeten = {p}') + notInFringe = True # false if node in fringe + notInExplored = True # false if node in explored + + # if node_[1] is None: + # continue + + p = manhattan(node_[1], goaltest) + node_[2] + # wyznacza jaki jest priorytet nastempnika + # manchaten from node_ + cost of way from tmp_ode to node_ + + priority_in_fringe = 0 + counter = 0 + tmp_position_in_fringe = 0 # zero if node not in fringe + + for fringeNode in fringe: # isc po wszystkich wezlach ktore juz sa w fringe + # jesli nasz wezel juz jest w fringe + if fringeNode[1].get_position()[0] == node_[1][0] and fringeNode[1].get_position()[1] == node_[1][1] and fringeNode[1].get_position()[2] == node_[1][2]: + notInFringe = False + priority_in_fringe = fringeNode[0] + # number of element in fringe + tmp_position_in_fringe = counter + counter = counter + 1 + + + for exploredNode in explored: # isc po wszystkich wezlach z listy explored + # jesli nasz wezel juz jest w explored + if exploredNode.get_position()[0] == node_[1][0] and exploredNode.get_position()[1] == node_[1][1] and exploredNode.get_position()[2] == node_[1][2]: + notInExplored = False + + + # if node not in fringe and not in explored + if notInFringe and notInExplored: + x = node.Node(tmp_node, node_[0], node_[1], node_[2]) # parent, action, state_array, cost + heapq.heappush(fringe, (p, x)) + # if node not in fringe + elif notInFringe is False and (priority_in_fringe > p): + x = node.Node(tmp_node, node_[0], node_[1], node_[2]) # parent, action, state_array, cost + tmp = list(fringe[tmp_position_in_fringe]) + tmp[0] = p + tmp[1] = x + fringe[tmp_position_in_fringe] = tuple(tmp) + #self.window.draw_search([self.agent.position_x, self.agent.position_y], [node_[1][0], node_[1][1]],self.agent.current_map.tile_size, self.agent.current_map, self.agent) diff --git a/classes/minesweeper.py b/classes/minesweeper.py index efa26b4..35c41df 100644 --- a/classes/minesweeper.py +++ b/classes/minesweeper.py @@ -113,7 +113,7 @@ class Map: if ok and rng==0 and not (i<2 and j<3): matrix[i].append(10) #kamień - elif ok and rng>8 and not (i<2 and j<3): + elif ok and rng>4 and not (i<2 and j<3): matrix[i].append(5) #trawa elif ok and rng<2 and not (i<2 and j<3): matrix[i].append(0) #piasek diff --git a/classes/node.py b/classes/node.py index 6b1a38f..7d87f3f 100644 --- a/classes/node.py +++ b/classes/node.py @@ -20,3 +20,6 @@ class Node: def get_cost(self): return self.cost + def __lt__(self, other): + return self.cost < other.get_cost() +