s464968/AIprojekt-wozek
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Merge branch 'min_heap'

Browse Source
This commit is contained in:
tonywesoly 2022-05-20 01:21:52 +02:00
commit 3b04067915
7 changed files with 225 additions and 96 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

148
Astar.py
View File

@ -1,63 +1,21 @@
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 = []
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)]
@ -66,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
@ -84,53 +43,98 @@ class Pathfinding:
neighbours.append(self.grid[neighbour_x][neighbour_y])
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 = []
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:
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]
fringe.remove(current_node)
while fringe.count() > 0:
# current_node = fringe[0]
current_node = fringe.extract()
#################################################### 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)
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
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:
fringe.append(neighbour)
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
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

18
Environment.py
View File

@ -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)
self.weekend = random.randint(0, 1)
@ -47,18 +47,6 @@ class Environment:
self.update_truck()
# time.sleep(0.5)
# def use_decision_tree(self):
# marking = self.package.type
# if marking == Package_types.fragile:
# marking = 0
# elif marking == Package_types.priority:
# marking = 1
# tree = DecisionTree(marking, self.weekend, self.package.company.popularity,
# self.package.company.payment_delay, self.package.payed_upfront,
# self.package.company.shipping_type)
# decision = tree.decision
# return decision
def use_astar(self):
start_state = State(1,self.truck.x,self.truck.y) # sprawić aby paczka i shelf były wyszukiwane raz
if self.truck.has_package:
@ -67,7 +55,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

5
Global_variables.py
View File

@ -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)

75
Min_heap.py Normal file
View File

@ -0,0 +1,75 @@
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) and self.items[l] < self.items[i]:
smallest = l
else:
smallest = i
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]
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[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
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]
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
temp_node.heap_index = len(self.items) - 1
self.items.append(temp_node)
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] is item
return in_range and contains

7
Moving_truck.py
View File

@ -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
@ -30,6 +31,7 @@ class Moving_truck:
self.truck.has_package = True
self.truck.package_type = package.type
self.truck.sector = package.sector
self.astar.reset_grid()
self.move_without_swapping(truck_x, truck_y, package_x, package_y)
@ -42,6 +44,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]

64
Node.py Normal file
View File

@ -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

4
Program.py
View File

@ -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()