a* update, showing path to target
This commit is contained in:
Juundi
parent
302001e531
commit
80aa2995ff
@ -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
|
||||
Loading…
Reference in New Issue
Block a user