From b771ac0f00ea4b2bbf5edbb7c86159dc3555d23a Mon Sep 17 00:00:00 2001 From: tonywesoly Date: Thu, 12 May 2022 21:01:31 +0200 Subject: [PATCH 1/4] =?UTF-8?q?To=20jeszcze=20nie=20dzia=C5=82a.=20Testowy?= =?UTF-8?q?=20commit?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Astar.py | 81 ++++++++++++++--------------------------------------- Min_heap.py | 76 +++++++++++++++++++++++++++++++++++++++++++++++++ Node.py | 64 ++++++++++++++++++++++++++++++++++++++++++ 3 files changed, 161 insertions(+), 60 deletions(-) create mode 100644 Min_heap.py create mode 100644 Node.py diff --git a/Astar.py b/Astar.py index 57223cf..491b32f 100644 --- a/Astar.py +++ b/Astar.py @@ -1,60 +1,14 @@ import math +from operator import ne import pygame from Global_variables import Global_variables as G_var +from Min_heap import Min_heap +from Node import Node, State from Package import Package from Shelf import Shelf -class State: - - def __init__(self, direction, x, y): - self.direction = direction # kierunek w ktorym "patrzy wozek" - self.x = x - self.y = y - - def get_direction(self): - return self.direction - - def get_x(self): - return self.x - - def get_y(self): - return self.y - - def goal_test(self, goal): # sprawdza czy osiagnelismy cel - if self.x == goal[0] and self.y == goal[1]: - return True - else: - return False - - -class Node: - def __init__(self, state, walkable): - self.state = state - self.direction = state.direction - self.walkable = walkable - self.g_cost = 0 - self.h_cost = 0 - self.parent = None - - def get_action(self): - return self.action - - def get_direction(self): - return self.direction - - def get_parent(self): - return self.parent - - def f_cost(self): - if self.walkable: - return self.g_cost + self.h_cost - else: - # return 0 - return math.inf - - class Pathfinding: def __init__(self, enviroment_2d): # self.grid = [] @@ -89,39 +43,46 @@ class Pathfinding: start_node = self.grid[starting_state.x][starting_state.y] target_node = self.grid[target_state.x][target_state.y] - fringe = [] + # fringe = [] + fringe = Min_heap() explored = [] is_target_node_walkable = True if not target_node.walkable: target_node.walkable = True is_target_node_walkable = False - fringe.append(start_node) + # fringe.append(start_node) + fringe.insert(start_node) - while len(fringe) > 0: - current_node = fringe[0] - for i in range(1, len(fringe)): - if fringe[i].f_cost() < current_node.f_cost() or (fringe[i].f_cost() == current_node.f_cost() and fringe[i].h_cost < current_node.h_cost): - current_node = fringe[i] + # while len(fringe) > 0: + while fringe.count() > 0: + # current_node = fringe[0] + current_node = fringe.extract() + # for i in range(1, len(fringe)): + # if fringe[i].f_cost() < current_node.f_cost() or (fringe[i].f_cost() == current_node.f_cost() and fringe[i].h_cost < current_node.h_cost): + # current_node = fringe[i] - fringe.remove(current_node) + # fringe.remove(current_node) explored.append(current_node) if current_node.state == target_node.state: path = self.retrace_path(start_node,target_node) self.path = path + return for neighbour in self.succ(current_node): if not neighbour.walkable or neighbour in explored: # if neighbour in explored: continue new_movement_cost_to_neighbour = current_node.g_cost + self.get_distance(current_node,neighbour) - if new_movement_cost_to_neighbour < neighbour.g_cost or not neighbour in fringe: + # if new_movement_cost_to_neighbour < neighbour.g_cost or not neighbour in fringe: + if new_movement_cost_to_neighbour < neighbour.g_cost or not fringe.contains(neighbour): neighbour.g_cost = new_movement_cost_to_neighbour neighbour.h_cost = self.get_distance(neighbour,target_node) neighbour.parent = current_node - if not neighbour in fringe: - fringe.append(neighbour) + # if not neighbour in fringe: + if not fringe.contains(neighbour): + fringe.insert(neighbour) target_node.walkable = is_target_node_walkable diff --git a/Min_heap.py b/Min_heap.py new file mode 100644 index 0000000..633bc11 --- /dev/null +++ b/Min_heap.py @@ -0,0 +1,76 @@ +from cgitb import small +from heapq import heapify +import math +from multiprocessing.dummy import Array + +from Node import Node, State + + + +class Min_heap: + def __init__(self): + self.items = [] + + def parent(self,i): + return (i - 1) >> 1 + + def left(self,i): + return (i << 1) + 1 + + def right(self,i): + return (i << 1) + 2 + + def heapify(self, i): + l = self.left(i) + r = self.right(i) + if l < len(self.items) - 1 and self.items[l] < self.items[i]: + smallest = l + else: + smallest = i + if r < len(self.items) - 1 and self.items[r] < self.items[smallest]: + smallest = r + if smallest != i: + self.items[i], self.items[smallest] = self.items[smallest], self.items[i] + self.items[i].heap_index, self.items[smallest].heap_index = self.items[smallest].heap_index, self.items[i].heap_index + self.heapify(smallest) + + def extract(self): + if len(self.items) < 1: + print("STOS PUSTY!") + return + min = self.items[0] + # self.items[0] = self.items[last_item_index] + # self.items[0] = self.items.pop() + self.items[0] = self.items[len(self.items) - 1] + self.items.pop() + self.heapify(0) + return min + + def decrese_key(self, index, item): + if item > self.items[index]: + print("Nowy klucz wiekszy od klucza aktualnego!") + return + self.items[index] = item + while index > 0 and self.items[self.parent(index)] > self.items[index]: + self.items[index], self.items[self.parent( + index)] = self.items[self.parent(index)], self.items[index] + self.items[index].heap_index, self.items[self.parent( + index)].heap_index = self.items[self.parent(index)].heap_index, self.items[index].heap_index + index = self.parent(index) + + def insert(self, item): + temp_node = Node(State(0,0,0),False) + temp_node.h_cost = math.inf + self.items.append(temp_node) + item.heap_index = len(self.items) - 1 + self.decrese_key(len(self.items) - 1, item) + + def count(self): + return len(self.items) + + def contains(self, item): + in_range = len(self.items) > item.heap_index + contains = False + if in_range: + contains = self.items[item.heap_index] == item + return in_range and contains \ No newline at end of file diff --git a/Node.py b/Node.py new file mode 100644 index 0000000..f3e1cdf --- /dev/null +++ b/Node.py @@ -0,0 +1,64 @@ + +import math + + +class State: + + def __init__(self, direction, x, y): + self.direction = direction # kierunek w ktorym "patrzy wozek" + self.x = x + self.y = y + + def get_direction(self): + return self.direction + + def get_x(self): + return self.x + + def get_y(self): + return self.y + + def goal_test(self, goal): # sprawdza czy osiagnelismy cel + if self.x == goal[0] and self.y == goal[1]: + return True + else: + return False + + +class Node: + def __init__(self, state, walkable): + self.state = state + self.direction = state.direction + self.walkable = walkable + self.g_cost = 0 + self.h_cost = 0 + self.parent = None + self.heap_index = 0 + + def get_action(self): + return self.action + + def get_direction(self): + return self.direction + + def get_parent(self): + return self.parent + + def f_cost(self): + if self.walkable: + return self.g_cost + self.h_cost + else: + # return 0 + return math.inf + # if fringe[i].f_cost() < current_node.f_cost() or (fringe[i].f_cost() == current_node.f_cost() and fringe[i].h_cost < current_node.h_cost): + def __lt__(self, other): + if self.f_cost() == other.f_cost(): + return self.h_cost < other.h_cost + return self.f_cost() < other.f_cost() + def __gt__(self,other): + if self.f_cost() == other.f_cost(): + return self.h_cost > other.h_cost + return self.f_cost() > other.f_cost() + def __eq__(self,other): + return self.f_cost() == other.f_cost() and self.h_cost == other.h_cost + From 76f78b9e5853fb0d5f7f6493ee0aa4a411c691a8 Mon Sep 17 00:00:00 2001 From: tonywesoly Date: Thu, 12 May 2022 22:53:12 +0200 Subject: [PATCH 2/4] fixes --- Astar.py | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/Astar.py b/Astar.py index 491b32f..4b494cd 100644 --- a/Astar.py +++ b/Astar.py @@ -46,6 +46,7 @@ class Pathfinding: # fringe = [] fringe = Min_heap() explored = [] + # explored = set() is_target_node_walkable = True if not target_node.walkable: @@ -64,16 +65,20 @@ class Pathfinding: # fringe.remove(current_node) explored.append(current_node) + # explored.add(current_node) if current_node.state == target_node.state: path = self.retrace_path(start_node,target_node) self.path = path + target_node.walkable = is_target_node_walkable return for neighbour in self.succ(current_node): - if not neighbour.walkable or neighbour in explored: - # if neighbour in explored: + neighbour_in_explored = [e for e in explored if e.state == neighbour.state] + if not neighbour.walkable or len(neighbour_in_explored) > 0: continue + # if neighbour in explored: + # continue new_movement_cost_to_neighbour = current_node.g_cost + self.get_distance(current_node,neighbour) # if new_movement_cost_to_neighbour < neighbour.g_cost or not neighbour in fringe: if new_movement_cost_to_neighbour < neighbour.g_cost or not fringe.contains(neighbour): @@ -83,7 +88,6 @@ class Pathfinding: # if not neighbour in fringe: if not fringe.contains(neighbour): fringe.insert(neighbour) - target_node.walkable = is_target_node_walkable def get_distance(self, node_a, node_b): # funckja liczy dystans dla odległości między dwoma nodami From 2c258fb6d5de52deaea71fde417a874322a2efcc Mon Sep 17 00:00:00 2001 From: tonywesoly Date: Thu, 12 May 2022 22:53:19 +0200 Subject: [PATCH 3/4] fixes --- Environment.py | 2 +- Min_heap.py | 8 +++++--- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/Environment.py b/Environment.py index 0a957c3..1cd1c4d 100644 --- a/Environment.py +++ b/Environment.py @@ -38,7 +38,7 @@ class Environment: for field in row: field.draw() self.grid.draw_grid() - # self.astar.draw_path(self.window) + self.astar.draw_path(self.window) pygame.display.flip() def update_all_elements(self): diff --git a/Min_heap.py b/Min_heap.py index 633bc11..cb22778 100644 --- a/Min_heap.py +++ b/Min_heap.py @@ -23,11 +23,11 @@ class Min_heap: def heapify(self, i): l = self.left(i) r = self.right(i) - if l < len(self.items) - 1 and self.items[l] < self.items[i]: + if l < len(self.items) and self.items[l] < self.items[i]: smallest = l else: smallest = i - if r < len(self.items) - 1 and self.items[r] < self.items[smallest]: + if r < len(self.items) and self.items[r] < self.items[smallest]: smallest = r if smallest != i: self.items[i], self.items[smallest] = self.items[smallest], self.items[i] @@ -51,6 +51,7 @@ class Min_heap: print("Nowy klucz wiekszy od klucza aktualnego!") return self.items[index] = item + self.items[index].heap_index = index while index > 0 and self.items[self.parent(index)] > self.items[index]: self.items[index], self.items[self.parent( index)] = self.items[self.parent(index)], self.items[index] @@ -61,8 +62,9 @@ class Min_heap: def insert(self, item): temp_node = Node(State(0,0,0),False) temp_node.h_cost = math.inf + temp_node.heap_index = len(self.items) - 1 self.items.append(temp_node) - item.heap_index = len(self.items) - 1 + # item.heap_index = len(self.items) - 1 self.decrese_key(len(self.items) - 1, item) def count(self): From 71db6fb6b974b3822380ff180aaa2cd405b6a9e6 Mon Sep 17 00:00:00 2001 From: tonywesoly Date: Fri, 20 May 2022 01:13:40 +0200 Subject: [PATCH 4/4] Fixing astar and BIG performance improvments --- Astar.py | 83 +++++++++++++++++++++++++++++++++------------ Environment.py | 10 +++--- Global_variables.py | 5 --- Min_heap.py | 5 +-- Moving_truck.py | 7 ++-- Program.py | 4 +-- 6 files changed, 74 insertions(+), 40 deletions(-) diff --git a/Astar.py b/Astar.py index 4b494cd..9ed6b09 100644 --- a/Astar.py +++ b/Astar.py @@ -11,7 +11,11 @@ from Shelf import Shelf class Pathfinding: def __init__(self, enviroment_2d): - # self.grid = [] + self.enviroment_2d = enviroment_2d + self.reset_grid() + self.path = [] + + def reset_grid(self): self.grid = [[ # tworze pustej tablicy o wymiarach naszej kraty None for y in range(G_var().DIMENSION_Y)] @@ -20,12 +24,13 @@ class Pathfinding: for x in range(G_var().DIMENSION_X): # zapełnianie tablicy obiektami Node for y in range(G_var().DIMENSION_Y): is_walkable = True - if isinstance(enviroment_2d[x][y], Shelf): + to_check_type = self.enviroment_2d[x][y] + if isinstance(to_check_type, Shelf): + is_walkable = False + elif isinstance(to_check_type, Package) and to_check_type.is_placed: is_walkable = False self.grid[x][y] = Node(State(1, x, y), is_walkable) - self.path = [] - def succ(self,node): #funckja zwraca sąsiadów noda w argumencie node_x = node.state.x node_y = node.state.y @@ -39,33 +44,73 @@ class Pathfinding: return neighbours - def find_path(self, starting_state, target_state): # algorytm wyszukiwania trasy +################## TO REMOVE + def set_text(self, string, coordx, coordy, fontSize): #Function to set text + + font = pygame.font.Font('freesansbold.ttf', fontSize) + #(0, 0, 0) is black, to make black text + string = str(string) + text = font.render(string, True, (0, 0, 0)) + textRect = text.get_rect() + textRect.center = (coordx, coordy) + return (text, textRect) + + def draw_node(self,node, window, color): + node_x = node.state.x + node_y = node.state.y + #######################SET TEXT + f_cost_text = self.set_text(node.f_cost(), node_x * G_var().RECT_SIZE + (G_var().RECT_SIZE/2), node_y * + G_var().RECT_SIZE + (G_var().RECT_SIZE/2), 10) + g_cost_text = self.set_text(node.g_cost, node_x * G_var().RECT_SIZE + (G_var().RECT_SIZE/4), node_y * + G_var().RECT_SIZE + (G_var().RECT_SIZE/4), 10) + h_cost_text = self.set_text(node.h_cost, node_x * G_var().RECT_SIZE + (G_var().RECT_SIZE/4*3), node_y * + G_var().RECT_SIZE + (G_var().RECT_SIZE/4), 10) + + ############################### + node_x = node.state.x + node_y = node.state.y + block = pygame.Rect( + node_x * G_var().RECT_SIZE, node_y * + G_var().RECT_SIZE, G_var().RECT_SIZE, G_var().RECT_SIZE + ) + pygame.draw.rect(window, + color, + block) + window.blit(f_cost_text[0], f_cost_text[1]) + window.blit(g_cost_text[0], g_cost_text[1]) + window.blit(h_cost_text[0], h_cost_text[1]) + +############################### + + def find_path(self, starting_state, target_state, window): # algorytm wyszukiwania trasy start_node = self.grid[starting_state.x][starting_state.y] target_node = self.grid[target_state.x][target_state.y] - # fringe = [] fringe = Min_heap() explored = [] - # explored = set() is_target_node_walkable = True if not target_node.walkable: target_node.walkable = True is_target_node_walkable = False - # fringe.append(start_node) fringe.insert(start_node) - # while len(fringe) > 0: while fringe.count() > 0: # current_node = fringe[0] current_node = fringe.extract() - # for i in range(1, len(fringe)): - # if fringe[i].f_cost() < current_node.f_cost() or (fringe[i].f_cost() == current_node.f_cost() and fringe[i].h_cost < current_node.h_cost): - # current_node = fringe[i] - - # fringe.remove(current_node) + #################################################### TEST + # current_node_color = (213, 55, 221) + # to_check_color = (55, 213, 55) + # current_color = (233,55,55) + # # self.draw_node(current_node,window,current_node_color) + # for node_to_check in explored: + # self.draw_node(node_to_check,window, current_node_color) + # for node_to_check in fringe.items: + # self.draw_node(node_to_check,window, to_check_color) + # self.draw_node(current_node,window,current_color) + # pygame.display.flip() + ############################################################### explored.append(current_node) - # explored.add(current_node) if current_node.state == target_node.state: path = self.retrace_path(start_node,target_node) @@ -77,15 +122,11 @@ class Pathfinding: neighbour_in_explored = [e for e in explored if e.state == neighbour.state] if not neighbour.walkable or len(neighbour_in_explored) > 0: continue - # if neighbour in explored: - # continue new_movement_cost_to_neighbour = current_node.g_cost + self.get_distance(current_node,neighbour) - # if new_movement_cost_to_neighbour < neighbour.g_cost or not neighbour in fringe: if new_movement_cost_to_neighbour < neighbour.g_cost or not fringe.contains(neighbour): neighbour.g_cost = new_movement_cost_to_neighbour neighbour.h_cost = self.get_distance(neighbour,target_node) neighbour.parent = current_node - # if not neighbour in fringe: if not fringe.contains(neighbour): fringe.insert(neighbour) target_node.walkable = is_target_node_walkable @@ -94,9 +135,7 @@ class Pathfinding: dist_x = abs(node_a.state.x - node_b.state.x) dist_y = abs(node_a.state.y - node_b.state.y) - if dist_x > dist_y: - return 10 * (dist_x - dist_y) - return 10 * (dist_y - dist_x) + return (dist_x + dist_y) * 10 def retrace_path(self, start_node, end_node): # funkcja zwraca tablice która ma w sobie wartosci pola parent # od end_node do start_node diff --git a/Environment.py b/Environment.py index 1cd1c4d..9fbb584 100644 --- a/Environment.py +++ b/Environment.py @@ -28,9 +28,9 @@ class Environment: new_truck = Truck(window, 14, 7) self.enviroment_2d[14][7] = new_truck self.truck = new_truck - self.moving_truck = Moving_truck( - self.window, self.enviroment_2d, self.truck, self.package_spawner) self.astar = Pathfinding(self.enviroment_2d) + self.moving_truck = Moving_truck( + self.window, self.enviroment_2d, self.truck, self.package_spawner, self.astar) self.finding_fields = Finding_fields(self.enviroment_2d) def draw_all_elements(self): @@ -38,13 +38,13 @@ class Environment: for field in row: field.draw() self.grid.draw_grid() - self.astar.draw_path(self.window) + # self.astar.draw_path(self.window) pygame.display.flip() def update_all_elements(self): self.use_astar() # wywyoływanie za każdym razem astar jest bardzo zasobożerne. Lepiej raz na przejście self.update_truck() - time.sleep(0.5) + # time.sleep(0.5) def use_astar(self): @@ -54,7 +54,7 @@ class Environment: else: end_position = self.finding_fields.find_package() end_state = State(1,end_position.x, end_position.y) - self.astar.find_path(start_state,end_state) + self.astar.find_path(start_state,end_state,self.window) def update_truck(self): next_field_to_move = self.astar.path[0].state diff --git a/Global_variables.py b/Global_variables.py index 9129f8e..a27df53 100644 --- a/Global_variables.py +++ b/Global_variables.py @@ -10,11 +10,6 @@ class Global_variables(object): RECT_COLOR = (70, 77, 87) SHELF_COLOR = (143, 68, 33) - def __init__(self) -> None: - dim_x = 28 - dim_y = 15 - self.GRID = [["empty" for i in range(dim_x)] for j in range(dim_y)] - def __new__(cls): if cls._instance is None: cls._instance = super(Global_variables, cls).__new__(cls) diff --git a/Min_heap.py b/Min_heap.py index cb22778..b69bbbf 100644 --- a/Min_heap.py +++ b/Min_heap.py @@ -39,8 +39,6 @@ class Min_heap: print("STOS PUSTY!") return min = self.items[0] - # self.items[0] = self.items[last_item_index] - # self.items[0] = self.items.pop() self.items[0] = self.items[len(self.items) - 1] self.items.pop() self.heapify(0) @@ -64,7 +62,6 @@ class Min_heap: temp_node.h_cost = math.inf temp_node.heap_index = len(self.items) - 1 self.items.append(temp_node) - # item.heap_index = len(self.items) - 1 self.decrese_key(len(self.items) - 1, item) def count(self): @@ -74,5 +71,5 @@ class Min_heap: in_range = len(self.items) > item.heap_index contains = False if in_range: - contains = self.items[item.heap_index] == item + contains = self.items[item.heap_index] is item return in_range and contains \ No newline at end of file diff --git a/Moving_truck.py b/Moving_truck.py index 87c6172..d87d508 100644 --- a/Moving_truck.py +++ b/Moving_truck.py @@ -2,13 +2,14 @@ from Empty import Empty from Package import Package from Shelf import Shelf - +# TODO: DODAC OBSERWER ZAMIAST PRZEKAZYWANIE WSZYSTKICH BZDET class Moving_truck: - def __init__(self, window, enviroment_2d, truck, package_spawner): + def __init__(self, window, enviroment_2d, truck, package_spawner, a_star): self.enviroment_2d = enviroment_2d self.truck = truck self.window = window self.package_spawner = package_spawner + self.astar = a_star def move(self, x, y): truck_x = self.truck.x @@ -29,6 +30,7 @@ class Moving_truck: package = self.enviroment_2d[package_x][package_y] self.truck.has_package = True self.truck.package_type = package.type + self.astar.reset_grid() self.move_without_swapping(truck_x, truck_y, package_x, package_y) @@ -41,6 +43,7 @@ class Moving_truck: y].is_placed = True self.truck.has_package = False self.package_spawner.spawn_package() + self.astar.reset_grid() def swap_fields(self, x1, y1, x2, y2): self.enviroment_2d[x1][y1], self.enviroment_2d[x2][y2] = self.enviroment_2d[x2][y2], self.enviroment_2d[x1][y1] diff --git a/Program.py b/Program.py index 9ee68bc..2ef8674 100644 --- a/Program.py +++ b/Program.py @@ -17,5 +17,5 @@ class Program: for event in pygame.event.get(): # integrating with keyboard if event.type == QUIT: running = False - self.environment.update_all_elements() - self.environment.draw_all_elements() + self.environment.update_all_elements() + self.environment.draw_all_elements()