From 79463c2044e2b6f84cd6983ba73ac9af745db849 Mon Sep 17 00:00:00 2001 From: SaluSL Date: Tue, 18 May 2021 22:28:32 +0200 Subject: [PATCH] cleaning the code --- src/a_star.py | 131 +++++++++++++++++++++++++ src/bfs.py | 113 ++++++++++++++++++++++ src/main.py | 261 ++------------------------------------------------ 3 files changed, 252 insertions(+), 253 deletions(-) create mode 100644 src/a_star.py create mode 100644 src/bfs.py diff --git a/src/a_star.py b/src/a_star.py new file mode 100644 index 0000000..2f58549 --- /dev/null +++ b/src/a_star.py @@ -0,0 +1,131 @@ + +WAREHOUSE_MAP = [ + [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, 10, 0, 0, 0, 0, 0], + [10, 0, 0, 0, 10, 200, 200, 200, 200], + [0, 0, 10, 10, 10, 0, 0, 0, 0], + [0, 10, 0, 0, 0, 200, 200, 200, 200], +] + + +def a_star_search(start, end, agent_direction): + + open_nodes = [] + closed_nodes = [] + + start_node = NodeAStar(agent_direction, None, start) + start_node.g = start_node.h = start_node.f = 0 + end_node = NodeAStar(None, None, end) + end_node.g = end_node.h = end_node.f = 0 + + open_nodes.append(start_node) + + while len(open_nodes) != 0: + current_node = open_nodes[0] + current_node_index = 0 + for index, node in enumerate(open_nodes): + if node.f < current_node.f: + current_node = node + current_node_index = index + + open_nodes.pop(current_node_index) + closed_nodes.append(current_node) + + # Sprawdzam czy jesteśmy u celu jeżeli tak zwracamy ścieżkę + neighbour_nodes = [] + if current_node == end_node: + path = [] + current = current_node + while current is not None: + #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: # 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("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 + + 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 + neighbour.position == open_node.position and neighbour.g > open_node.g]) > 0: + continue + + open_nodes.append(neighbour) + + +class NodeAStar: + 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 + + def __eq__(self, other): + return self.position == other.position diff --git a/src/bfs.py b/src/bfs.py new file mode 100644 index 0000000..884de0f --- /dev/null +++ b/src/bfs.py @@ -0,0 +1,113 @@ + +# FRINGE - struktura danych przechowująca wierzchołki do odwiedzenia +# EXPLORED - lista odwiedzonych stanów +# istate - stan początkowy +# Succ - funkcja następnika +# Goaltest -test spełnienia celu + + +def breadth_first_search(istate, goal, agent_direction): # COORDINATES OF A START PLACE + fringe = [] + explored = [] + start = Node(istate, agent_direction) + fringe.append(start) + path = [] + + while True: + if not fringe: + return False + + elem = fringe.pop(0) + + if elem.state == goal: + # return ciag akcji zbudowany z wykorzystaniem pol parent i action + while elem.parent is not None: + if type(elem.action) == list: + elem.action.reverse() + for each_action in elem.action: + path.append(each_action) + else: + path.append(elem.action) + elem = elem.parent + + path.reverse() + return path + + explored.append(elem) + + for action, state, direction in elem.successor(): + fringe_states = [] + explored_states = [] + for node in fringe: + fringe_states.append(node.state) + for node2 in explored: + explored_states.append(node2.state) + + if state not in fringe_states and state not in explored_states: + x = Node(state, direction) + x.parent = elem + x.action = action + fringe.append(x) + + +class Node: + def __init__(self, state, agent_direction, action=None, parent=None): + self.state = state + self.action = action + self.parent = parent + self.agent_direction = agent_direction + + def successor(self): + neighbours = [] + x = self.state[0] + y = self.state[1] + + if x < 8: # RIGHT NEIGHBOUR + if self.agent_direction == "right": + actions = "move" + elif self.agent_direction == "left": + actions = ["rotate_right", "rotate_right", "move"] + elif self.agent_direction == "up": + actions = ["rotate_right", "move"] + elif self.agent_direction == "down": + actions = ["rotate_left", "move"] + + neighbours.append((actions, (x + 1, y), "right")) + + if x > 0: # LEFT NEIGHBOUR + if self.agent_direction == "right": + actions = ["rotate_left", "rotate_left", "move"] + elif self.agent_direction == "left": + actions = "move" + elif self.agent_direction == "up": + actions = ["rotate_left", "move"] + elif self.agent_direction == "down": + actions = ["rotate_right", "move"] + + neighbours.append((actions, (x - 1, y), "left")) + + if y > 0: # UP NEIGHBOUR + if self.agent_direction == "right": + actions = ["rotate_left", "move"] + elif self.agent_direction == "left": + actions = ["rotate_right", "move"] + elif self.agent_direction == "up": + actions = "move" + elif self.agent_direction == "down": + actions = ["rotate_left", "rotate_left", "move"] + + neighbours.append((actions, (x, y - 1), "up")) + + if y < 8: # DOWN NEIGHBOUR + if self.agent_direction == "right": + actions = ["rotate_right", "move"] + elif self.agent_direction == "left": + actions = ["rotate_left", "move"] + elif self.agent_direction == "up": + actions = ["rotate_left", "rotate_left", "move"] + elif self.agent_direction == "down": + actions = "move" + + neighbours.append((actions, (x, y + 1), "down")) + + return neighbours diff --git a/src/main.py b/src/main.py index 77123c0..d1b16fe 100644 --- a/src/main.py +++ b/src/main.py @@ -2,6 +2,9 @@ import pygame import random import time +from src.a_star import a_star_search, WAREHOUSE_MAP +from src.bfs import breadth_first_search + pygame.init() WIDTH = 675 @@ -23,19 +26,6 @@ def get_position_from_pix(pix_pos): print("ERROR: THERE IS NO POSITION LIKE THIS (get_position_from_pix)") -WAREHOUSE_MAP = [ - [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, 10, 0, 0, 0, 0, 0], - [10, 0, 0, 0, 10, 200, 200, 200, 200], - [0, 0, 10, 10, 10, 0, 0, 0, 0], - [0, 10, 0, 0, 0, 200, 200, 200, 200], -] - - def create_positions(): size_agent = 40 x_position_pix = size_agent @@ -57,239 +47,6 @@ def generate_package(a, b): return p1 -# FRINGE - struktura danych przechowująca wierzchołki do odwiedzenia -# EXPLORED - lista odwiedzonych stanów -# istate - stan początkowy -# Succ - funkcja następnika -# Goaltest -test spełnienia celu - - -def breadth_first_search(istate, agent_direction): # COORDINATES OF A START PLACE - fringe = [] - explored = [] - start = Node(istate, agent_direction) - fringe.append(start) - path = [] - - while True: - if not fringe: - return False - - elem = fringe.pop(0) - - if agent.goal_test(elem.state): - # return ciag akcji zbudowany z wykorzystaniem pol parent i action - while elem.parent is not None: - if type(elem.action) == list: - elem.action.reverse() - for each_action in elem.action: - path.append(each_action) - else: - path.append(elem.action) - elem = elem.parent - - path.reverse() - return path - - explored.append(elem) - - for action, state, direction in elem.successor(): - fringe_states = [] - explored_states = [] - for node in fringe: - fringe_states.append(node.state) - for node2 in explored: - explored_states.append(node2.state) - - if state not in fringe_states and state not in explored_states: - x = Node(state, direction) - x.parent = elem - x.action = action - fringe.append(x) - - -def a_star_search(start, end, agent_direction): - open_nodes = [] - closed_nodes = [] - - start_node = NodeAStar(agent_direction, None, start) - start_node.g = start_node.h = start_node.f = 0 - end_node = NodeAStar(None, None, end) - end_node.g = end_node.h = end_node.f = 0 - - open_nodes.append(start_node) - - while len(open_nodes) != 0: - current_node = open_nodes[0] - current_node_index = 0 - for index, node in enumerate(open_nodes): - if node.f < current_node.f: - current_node = node - current_node_index = index - - open_nodes.pop(current_node_index) - closed_nodes.append(current_node) - - # Sprawdzam czy jesteśmy u celu jeżeli tak zwracamy ścieżkę - neighbour_nodes = [] - if current_node == end_node: - path = [] - current = current_node - while current is not None: - #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: # 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("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 - - 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 - neighbour.position == open_node.position and neighbour.g > open_node.g]) > 0: - continue - - open_nodes.append(neighbour) - - -class NodeAStar: - 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 - - def __eq__(self, other): - return self.position == other.position - - - -class Node: - def __init__(self, state, agent_direction, action=None, parent=None): - self.state = state - self.action = action - self.parent = parent - self.agent_direction = agent_direction - - def successor(self): - neighbours = [] - x = self.state[0] - y = self.state[1] - - if x < 8: # RIGHT NEIGHBOUR - if self.agent_direction == "right": - actions = "move" - elif self.agent_direction == "left": - actions = ["rotate_right", "rotate_right", "move"] - elif self.agent_direction == "up": - actions = ["rotate_right", "move"] - elif self.agent_direction == "down": - actions = ["rotate_left", "move"] - - neighbours.append((actions, (x + 1, y), "right")) - - if x > 0: # LEFT NEIGHBOUR - if self.agent_direction == "right": - actions = ["rotate_left", "rotate_left", "move"] - elif self.agent_direction == "left": - actions = "move" - elif self.agent_direction == "up": - actions = ["rotate_left", "move"] - elif self.agent_direction == "down": - actions = ["rotate_right", "move"] - - neighbours.append((actions, (x - 1, y), "left")) - - if y > 0: # UP NEIGHBOUR - if self.agent_direction == "right": - actions = ["rotate_left", "move"] - elif self.agent_direction == "left": - actions = ["rotate_right", "move"] - elif self.agent_direction == "up": - actions = "move" - elif self.agent_direction == "down": - actions = ["rotate_left", "rotate_left", "move"] - - neighbours.append((actions, (x, y - 1), "up")) - - if y < 8: # DOWN NEIGHBOUR - if self.agent_direction == "right": - actions = ["rotate_right", "move"] - elif self.agent_direction == "left": - actions = ["rotate_left", "move"] - elif self.agent_direction == "up": - actions = ["rotate_left", "rotate_left", "move"] - elif self.agent_direction == "down": - actions = "move" - - neighbours.append((actions, (x, y + 1), "down")) - - return neighbours - - class Package: def __init__(self, pos, content, content_size, pack_image): self.pos = pos @@ -414,7 +171,7 @@ board = pygame.Surface((WIDTH, HEIGHT), pygame.SRCALPHA) # transparently surfac create_positions() -#Rysowanie lini +# 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) @@ -442,15 +199,15 @@ Package_list = [ print(a_star_search((6, 8), (0, 0), "down")) -#agent.path = breadth_first_search(agent.pos_coord, agent.agent_direction) +#agent.path = breadth_first_search(agent.pos_coord, agent.goal, agent.agent_direction) agent.path = a_star_search((agent.pos_coord[1], agent.pos_coord[0]), (0, 0), agent.agent_direction) print(agent.path) - -Stain_list = [] # Pętla służąca do tworzenia plam oleju na podstawie mapy magazynu +Stain_list = [] + for index_x in range(9): for index_y in range(9): if WAREHOUSE_MAP[index_x][index_y] == 10: @@ -466,8 +223,6 @@ while running: if event.type == pygame.KEYDOWN: pass - - if len(Package_list) < QTY_OF_PACKAGES: is_dock_empty = True @@ -491,7 +246,7 @@ 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.goal, agent.agent_direction) agent.path = a_star_search((agent.pos_coord[1], agent.pos_coord[0]), (agent.goal[1], agent.goal[0]), agent.agent_direction) for package in Package_list: