a* update, showing path to target

This commit is contained in:
Juundi 2022-04-29 04:34:12 +02:00
parent 302001e531
commit 80aa2995ff

View File

@ -1,13 +1,25 @@
import sys
from queue import PriorityQueue
import numpy as np
import astar
import pygame
screen = []
objectArray = []
collisionsMap = []
weightsMap = ([1, 2, 1, 4, 5, 2, 7, 8, 5, 4, 15, 3, 4, 5, 8],
[1, 2, 1, 4, 5, 2, 7, 8, 1, 4, 1, 3, 4, 5, 1],
[1, 2, 1, 4, 5, 2, 7, 8, 1, 4, 1, 3, 4, 5, 3],
[1, 2, 1, 4, 5, 2, 7, 8, 1, 4, 3, 3, 8, 5, 4],
[1, 2, 1, 4, 5, 2, 7, 8, 1, 4, 1, 3, 9, 5, 2],
[1, 2, 1, 4, 5, 2, 7, 8, 1, 4, 1, 12, 4, 5, 6],
[1, 2, 1, 4, 5, 2, 7, 8, 1, 4, 7, 3, 4, 5, 7],
[5, 2, 1, 4, 5, 2, 7, 8, 1, 4, 17, 14, 4, 5, 1],
[1, 2, 1, 4, 5, 2, 7, 8, 1, 4, 1, 3, 4, 14, 3],
[1, 2, 1, 4, 5, 2, 7, 8, 1, 4, 1, 3, 4, 14, 2],
[5, 2, 1, 4, 5, 2, 7, 8, 1, 4, 1, 3, 4, 14, 6],
[1, 2, 1, 4, 5, 2, 7, 8, 1, 4, 1, 13, 14, 15, 7],
[1, 2, 1, 4, 5, 2, 7, 8, 1, 4, 1, 14, 4, 14, 1],
[1, 2, 1, 4, 5, 2, 7, 8, 1, 4, 1, 3, 4, 15, 2],
[1, 2, 1, 4, 5, 2, 7, 8, 1, 4, 1, 3, 4, 15, 2])
class Position:
def __init__(self, x, y):
@ -53,18 +65,9 @@ class Agent(Object):
truck = pygame.transform.scale(truck, (square, square))
screen.blit(truck, (circleX, circleY))
def move(self, event, gridLength):
if event.key == pygame.K_LEFT:
newPos = self.pos.get_moved(-1, 0)
self.move_if_possible(newPos, gridLength)
if event.key == pygame.K_RIGHT:
newPos = self.pos.get_moved(1, 0)
self.move_if_possible(newPos, gridLength)
if event.key == pygame.K_UP:
newPos = self.pos.get_moved(0, -1)
self.move_if_possible(newPos, gridLength)
if event.key == pygame.K_DOWN:
newPos = self.pos.get_moved(0, 1)
def move(self, gridLength, path):
for (x, y) in path:
newPos = self.pos.get_moved(x, y)
self.move_if_possible(newPos, gridLength)
def move_if_possible(self, newPos, gridLength):
@ -149,105 +152,13 @@ def draw(square_num, objectArr):
(a + grid_size, b + grid_size), 2)
def kb_listen(objectArray, gridLength):
def kb_listen(objectArray, gridLength, path):
agent = objectArray[0]
for event in pygame.event.get():
if event.type == pygame.KEYDOWN:
agent.move(event, gridLength)
if event.type == pygame.QUIT:
sys.exit()
#moje
def manhattan(node1, node2):
x1, y1 = node1.state[0], node1.state[1]
x2, y2 = node2.state[0], node2.state[1]
distance = abs(x1 - x2) + abs(y1 - y2)
return distance
def f(state):#tablica z losowymi wagami(kosztami) pól, w astar trzeba zsumować wagę pola z heurystyką - f + manhattan
weights = np.array([
[1, 2, 1, 4, 5, 2, 7, 8, 1, 4, 1, 3, 4, 5, 8],
[1, 2, 1, 4, 5, 2, 7, 8, 1, 4, 1, 3, 4, 5, 1],
[1, 2, 1, 4, 5, 2, 7, 8, 1, 4, 1, 3, 4, 5, 3],
[1, 2, 1, 4, 5, 2, 7, 8, 1, 4, 1, 3, 4, 5, 4],
[1, 2, 1, 4, 5, 2, 7, 8, 1, 4, 1, 3, 4, 5, 2],
[1, 2, 1, 4, 5, 2, 7, 8, 1, 4, 1, 3, 4, 5, 6],
[1, 2, 1, 4, 5, 2, 7, 8, 1, 4, 1, 3, 4, 5, 7],
[1, 2, 1, 4, 5, 2, 7, 8, 1, 4, 1, 3, 4, 5, 1],
[1, 2, 1, 4, 5, 2, 7, 8, 1, 4, 1, 3, 4, 5, 3],
[1, 2, 1, 4, 5, 2, 7, 8, 1, 4, 1, 3, 4, 5, 2],
[1, 2, 1, 4, 5, 2, 7, 8, 1, 4, 1, 3, 4, 5, 6],
[1, 2, 1, 4, 5, 2, 7, 8, 1, 4, 1, 3, 4, 5, 7],
[1, 2, 1, 4, 5, 2, 7, 8, 1, 4, 1, 3, 4, 5, 1],
[1, 2, 1, 4, 5, 2, 7, 8, 1, 4, 1, 3, 4, 5, 2],
[1, 2, 1, 4, 5, 2, 7, 8, 1, 4, 1, 3, 4, 5, 2]
])
pos_x = state[0]
pos_y = state[1]
#print(weights[pos_x][pos_y])
return weights[pos_x][pos_y]
class Node: #prawie jak Field w bfs
def __init__(self, state, parent, action):
self.state = state #position - (x, y, direction)
self.parent = parent
self.action = action
def succ1(state):
successors = []#-90 obrót w lewo, +90 obrót w prawo
print(state[0])#operujemy na 0, 90, 180, 270
print(state[1])
right = state[2] + 90
successors.append((("turn", "right"), (state[0], state[1], right)))
left = state[2] - 90
successors.append((("turn", "left"), (state[0], state[1], left)))
if state[2] == 360:
state[2] = 0
if state[2] == -90:
state[2] = 270
if (state[0], state[1]) not in black_list:#działa
if state[2] == 0 and state[0] < 14:
new_x = state[0]+1
successors.append((("move forward"), (new_x, state[1], state[2])))
elif state[2] == 90 and state[1] > 0:
new_y = state[1]-1
successors.append((("move forward"), (state[0], new_y, state[2])))
elif state[2] == 180 and state[0] > 0:
new_x = state[0]-1
successors.append((("move forward"), (new_x, state[1], state[2])))
elif state[2] == 270 and state[1] < 14:
new_y = state[1]+1
successors.append((("move forward"), (state[0], new_y, state[2])))
return successors
def algorithm():
opened = PriorityQueue()#może być też lista
closed = []#już odwiedzone, odrzucone wierzchołki
first_state = (0, 0, "Right")#x, y, kierunek
final_state = (14, 14, "Right")
starting_point = Node(first_state, False, False)
ending_point = Node(final_state, False, False)
pos1 = (starting_point.state[0], starting_point.state[1])
pos2 = (ending_point.state[0], ending_point.state[1])
opened.put((1, starting_point))
a = final_state[0]
b = final_state[1]
#hole = Hole("astar", Position(a, b))#narysowana dziura w miejscu mety(celu), by sprawdzić, czy działa
#objectArray.append(hole)
agent.move(gridLength, path)
if __name__ == '__main__':
pygame.init() # inicjalizacja modułów, na razie niepotrzebna
gridSize = 15
astar()
# Tworzymy nowego playera, czy tam agenta
agent = Agent("smieciarka", Position(0, 0))
@ -267,16 +178,18 @@ if __name__ == '__main__':
for object in objectArray[1:]:
collisionsMap[object.pos.x][object.pos.y] = True
black_list = [(10, 10), (7, 4), (3, 10), (8, 10), (4, 5), (1, 2), (10, 4), (13, 14), (6, 9), (4, 9), (5, 11), (11, 7), (13, 8)]
#lista obiektów potrzebna do succ1 - na te pole śmieciarka nie wchodzi, więc nie ma ich brać pod uwagę
width = 610
height = 530
screen = pygame.display.set_mode((width, height)) # ustalanie rozmiarów okna
while 1:
startPos = (0, 0)
finalPos = (14, 14)
astarPath = astar.aStar(weightsMap, startPos, finalPos)
print(astarPath)
while True:
c = (255, 255, 255) # tymczasowy kolor tła - do usunięcia, jak już będzie zdjęcie
screen.fill(c)
draw(gridSize, objectArray)
kb_listen(objectArray, gridSize)
kb_listen(objectArray, gridSize, astarPath)
pygame.display.update() # by krata pojawiła się w okienku - update powierzc