From 9f24b128ebe91275fd1082779d03ce14427aa80c Mon Sep 17 00:00:00 2001 From: Miron Date: Sun, 9 May 2021 20:19:18 +0200 Subject: [PATCH] =?UTF-8?q?Znowu=20nie=20dzia=C5=82a=20A*=20XD?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/main.py | 137 ++++++++++++++++++++++++++++++++++------------------ 1 file changed, 89 insertions(+), 48 deletions(-) diff --git a/src/main.py b/src/main.py index 7ec8fb1..f60bbbf 100644 --- a/src/main.py +++ b/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])