Znowu nie działa A* XD
This commit is contained in:
Miron
parent
7288e4901c
commit
9f24b128eb
137
src/main.py
137
src/main.py
@ -24,15 +24,15 @@ def get_position_from_pix(pix_pos):
|
||||
|
||||
|
||||
WAREHOUSE_MAP = [
|
||||
[1, 1, 1, 1, 1, 1, 1, 1, 1],
|
||||
[20, 20, 20, 20, 20, 20, 20, 20, 1],
|
||||
[1, 1, 1, 1, 1, 1, 1, 1, 1],
|
||||
[1, 199, 199, 199, 199, 199, 199, 199, 199],
|
||||
[1, 1, 1, 1, 1, 1, 1, 1, 1],
|
||||
[1, 1, 1, 1, 1, 1, 1, 1, 1],
|
||||
[1, 1, 1, 1, 1, 1, 1, 1, 1],
|
||||
[1, 1, 1, 1, 1, 1, 1, 1, 1],
|
||||
[1, 1, 1, 1, 1, 1, 1, 1, 1],
|
||||
[0, 0, 0, 10, 10, 0, 0, 0, 0],
|
||||
[0, 10, 0, 10, 0, 10, 0, 0, 0],
|
||||
[0, 10, 0, 0, 0, 200, 200, 200, 200],
|
||||
[0, 0, 10, 0, 0, 0, 0, 0, 0],
|
||||
[0, 0, 10, 0, 0, 200, 200, 200, 200],
|
||||
[0, 0, 10, 0, 0, 0, 0, 0, 0],
|
||||
[10, 0, 0, 0, 0, 200, 200, 200, 200],
|
||||
[0, 0, 0, 10, 0, 0, 0, 0, 0],
|
||||
[0, 10, 0, 0, 0, 200, 200, 200, 200],
|
||||
]
|
||||
|
||||
|
||||
@ -108,13 +108,13 @@ def breadth_first_search(istate, agent_direction): # COORDINATES OF A START PLA
|
||||
fringe.append(x)
|
||||
|
||||
|
||||
def a_star_search(start, end):
|
||||
def a_star_search(start, end, agent_direction):
|
||||
open_nodes = []
|
||||
closed_nodes = []
|
||||
|
||||
start_node = NodeAStar(None, start)
|
||||
start_node = NodeAStar(agent_direction, None, start)
|
||||
start_node.g = start_node.h = start_node.f = 0
|
||||
end_node = NodeAStar(None, end)
|
||||
end_node = NodeAStar(None, None, end)
|
||||
end_node.g = end_node.h = end_node.f = 0
|
||||
|
||||
open_nodes.append(start_node)
|
||||
@ -136,38 +136,73 @@ def a_star_search(start, end):
|
||||
path = []
|
||||
current = current_node
|
||||
while current is not None:
|
||||
path.append(current.position)
|
||||
#path.append(current.position)
|
||||
if current.action is not None:
|
||||
current.action.reverse()
|
||||
for each_action in current.action:
|
||||
path.append(each_action)
|
||||
|
||||
current = current.parent
|
||||
#path = path.pop()
|
||||
return path[::-1]
|
||||
#
|
||||
|
||||
x = current_node.position[0]
|
||||
y = current_node.position[1]
|
||||
if x < 8: # RIGHT NEIGHBOUR
|
||||
if x < 8: # DOWN NEIGHBOUR
|
||||
if current_node.agent_direction == "right":
|
||||
actions = ["rotate_right", "move"]
|
||||
elif current_node.agent_direction == "left":
|
||||
actions = ["rotate_left", "move"]
|
||||
elif current_node.agent_direction == "up":
|
||||
actions = ["rotate_right", "rotate_right", "move"]
|
||||
elif current_node.agent_direction == "down":
|
||||
actions = ["move"]
|
||||
neighbour_nodes.append(
|
||||
NodeAStar(current_node,
|
||||
(x + 1, y)
|
||||
))
|
||||
if x > 0: # LEFT NEIGHBOUR
|
||||
neighbour_nodes.append(
|
||||
NodeAStar(current_node,
|
||||
(x - 1, y)
|
||||
))
|
||||
if y > 0: # UP NEIGHBOUR
|
||||
neighbour_nodes.append(
|
||||
NodeAStar(current_node,
|
||||
(x, y - 1)
|
||||
))
|
||||
if y < 8: # DOWN NEIGHBOUR
|
||||
neighbour_nodes.append(
|
||||
NodeAStar(current_node,
|
||||
(x, y + 1)
|
||||
))
|
||||
NodeAStar("down", current_node, (x + 1, y), actions))
|
||||
|
||||
if x > 0: # UP NEIGHBOUR
|
||||
if current_node.agent_direction == "right":
|
||||
actions = ["rotate_left", "move"]
|
||||
elif current_node.agent_direction == "left":
|
||||
actions = ["rotate_right", "move"]
|
||||
elif current_node.agent_direction == "up":
|
||||
actions = ["move"]
|
||||
elif current_node.agent_direction == "down":
|
||||
actions = ["rotate_right", "rotate_right", "move"]
|
||||
neighbour_nodes.append(NodeAStar("up", current_node, (x - 1, y), actions))
|
||||
|
||||
if y > 0: # LEFT NEIGHBOUR
|
||||
if current_node.agent_direction == "right":
|
||||
actions = ["rotate_left", "rotate_left", "move"]
|
||||
elif current_node.agent_direction == "left":
|
||||
actions = ["move"]
|
||||
elif current_node.agent_direction == "up":
|
||||
actions = ["rotate_left", "move"]
|
||||
elif current_node.agent_direction == "down":
|
||||
actions = ["rotate_right", "move"]
|
||||
neighbour_nodes.append(NodeAStar("left", current_node, (x, y - 1), actions))
|
||||
|
||||
if y < 8: # RIGHT NEIGHBOUR
|
||||
if current_node.agent_direction == "right":
|
||||
actions = ["move"]
|
||||
elif current_node.agent_direction == "left":
|
||||
actions = ["rotate_left", "rotate_left", "move"]
|
||||
elif current_node.agent_direction == "up":
|
||||
actions = ["rotate_right", "move"]
|
||||
elif current_node.agent_direction == "down":
|
||||
actions = ["rotate_left", "move"]
|
||||
neighbour_nodes.append(NodeAStar("right", current_node, (x, y + 1), actions))
|
||||
for neighbour in neighbour_nodes:
|
||||
if len([closed_neighbour for closed_neighbour in closed_nodes if closed_neighbour == neighbour]) > 0:
|
||||
continue
|
||||
|
||||
neighbour.g = current_node.g + WAREHOUSE_MAP[neighbour.position[0]][neighbour.position[1]]
|
||||
neighbour.h = abs(neighbour.position[0] - end_node.position[0]) + abs(neighbour.position[1] - end_node.position[1])
|
||||
action_len = 0
|
||||
if current_node.action is not None:
|
||||
action_len = len(current_node.action)
|
||||
neighbour.g = current_node.g + WAREHOUSE_MAP[neighbour.position[0]][neighbour.position[1]] + action_len
|
||||
neighbour.h = abs(neighbour.position[0] - end_node.position[0]) + abs(
|
||||
neighbour.position[1] - end_node.position[1])
|
||||
neighbour.f = neighbour.g + neighbour.h
|
||||
|
||||
if len([open_node for open_node in open_nodes if
|
||||
@ -178,10 +213,11 @@ def a_star_search(start, end):
|
||||
|
||||
|
||||
class NodeAStar:
|
||||
def __init__(self, parent=None, position=None):
|
||||
def __init__(self, agent_direction, parent=None, position=None, action=None):
|
||||
self.agent_direction = agent_direction
|
||||
self.parent = parent
|
||||
self.position = position
|
||||
|
||||
self.action = action
|
||||
self.g = 0
|
||||
self.h = 0
|
||||
self.f = 0
|
||||
@ -190,10 +226,7 @@ class NodeAStar:
|
||||
return self.position == other.position
|
||||
|
||||
|
||||
print(a_star_search((0, 0), (3, 0)))
|
||||
#print(a_star_search((2, 2), (7, 7)))
|
||||
print(WAREHOUSE_MAP[5][0])
|
||||
print(5, 0)
|
||||
|
||||
class Node:
|
||||
def __init__(self, state, agent_direction, action=None, parent=None):
|
||||
self.state = state
|
||||
@ -279,14 +312,14 @@ class Shelf:
|
||||
class Stain:
|
||||
def __init__(self, pos):
|
||||
self.pos_pix = get_pix_from_position[pos]
|
||||
self.image = STAIN = pygame.transform.scale(pygame.image.load("img/stain.png"), (60, 60))
|
||||
self.image = pygame.transform.scale(pygame.image.load("img/stain.png"), (60, 60))
|
||||
self.rect = self.image.get_rect(center=self.pos_pix)
|
||||
|
||||
|
||||
class Agent:
|
||||
def __init__(self, pos, agent_direction="right"):
|
||||
self.pos = pos
|
||||
self.pos_coord = get_position_from_pix(pos)
|
||||
self.pos_coord = (get_position_from_pix(pos)[1], get_position_from_pix(pos)[0])
|
||||
self.image = IMAGE
|
||||
self.rect = self.image.get_rect(center=pos)
|
||||
self.goal = (0, 0)
|
||||
@ -377,11 +410,11 @@ class Agent:
|
||||
|
||||
board = pygame.Surface((WIDTH, HEIGHT), pygame.SRCALPHA) # transparently surface
|
||||
|
||||
#
|
||||
|
||||
create_positions()
|
||||
|
||||
#
|
||||
|
||||
#Rysowanie lini
|
||||
for x in range(9):
|
||||
for y in range(9):
|
||||
pygame.draw.rect(board, (0, 0, 0), (x * size, y * size, size, size), 3)
|
||||
@ -406,13 +439,21 @@ Package_list = [
|
||||
generate_package(40, 40)
|
||||
]
|
||||
|
||||
agent.path = breadth_first_search(agent.pos_coord, agent.agent_direction)
|
||||
|
||||
print(a_star_search((6, 8), (0, 0), "down"))
|
||||
|
||||
#agent.path = breadth_first_search(agent.pos_coord, agent.agent_direction)
|
||||
agent.path = a_star_search(agent.pos_coord, (0, 0), agent.agent_direction)
|
||||
|
||||
|
||||
print(agent.path)
|
||||
|
||||
|
||||
Stain_list = []
|
||||
# Pętla służąca do tworzenia plam oleju na podstawie mapy magazynu
|
||||
for index_x in range(9):
|
||||
for index_y in range(9):
|
||||
if WAREHOUSE_MAP[index_x][index_y] == 5:
|
||||
if WAREHOUSE_MAP[index_x][index_y] == 10:
|
||||
Stain_list.append(Stain((index_y, index_x)))
|
||||
running = True
|
||||
|
||||
@ -448,7 +489,6 @@ while running:
|
||||
|
||||
agent.move_bfs()
|
||||
|
||||
|
||||
else:
|
||||
time.sleep(.25)
|
||||
|
||||
@ -457,7 +497,8 @@ while running:
|
||||
agent.goal = coord_goals.pop(0)
|
||||
agent.goal_achieved = False
|
||||
|
||||
agent.path = breadth_first_search(agent.pos_coord, agent.agent_direction)
|
||||
#agent.path = breadth_first_search(agent.pos_coord, agent.agent_direction)
|
||||
agent.path = a_star_search(agent.pos_coord, agent.goal, agent.agent_direction)
|
||||
|
||||
# DRAWING
|
||||
screen.blit(BACKGROUND, [0, 0])
|
||||
|
||||
Loading…
Reference in New Issue
Block a user