diff --git a/src/main.py b/src/main.py index 1a48026..6137488 100644 --- a/src/main.py +++ b/src/main.py @@ -1,44 +1,31 @@ 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 HEIGHT = 675 size = 75 QTY_OF_PACKAGES = 8 RECT_SIZE = 9 - + IMAGE = pygame.image.load('img/wozek2.png') BACKGROUND = pygame.transform.scale(pygame.image.load('img/background.png'), (WIDTH, HEIGHT)) DOCK = pygame.transform.scale(pygame.image.load('img/dock_left.png'), (75, 75)) get_pix_from_position = {} - - + + def get_position_from_pix(pix_pos): for key, value in get_pix_from_position.items(): if pix_pos == value: return key print("ERROR: THERE IS NO POSITION LIKE THIS (get_position_from_pix)") - - -WAREHOUSE_MAP = [ - [0, 0, 0, 10, 10, 0, 0, 0, 0], - [10, 10, 0, 10, 0, 10, 0, 0, 0], - [10, 10, 0, 0, 0, 200, 200, 200, 200], - [10, 0, 10, 0, 0, 0, 0, 0, 0], - [10, 10, 10, 0, 0, 200, 200, 200, 200], - [10, 10, 10, 10, 0, 0, 0, 0, 0], - [10, 10, 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 @@ -49,8 +36,8 @@ def create_positions(): y_position_pix += size y_position_pix = size_agent x_position_pix += size - - + + def generate_package(a, b): rand_y = random.randint(1, 4) rand_x = random.randint(10, 150) @@ -58,240 +45,8 @@ def generate_package(a, b): image_list = ['img/package_grocery.png', 'img/package_explosive.png', 'img/package.png', 'img/package_builders.png'] p1 = Package((a, b), rand_x, name_list[rand_y - 1], image_list[rand_y - 1]) 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 @@ -300,8 +55,8 @@ class Package: self.image = pygame.transform.scale(pygame.image.load(pack_image), (50, 50)) self.rect = self.image.get_rect(center=pos) self.is_package_up = False - - + + class Shelf: def __init__(self, pos, content, content_size): self.pos_pix = get_pix_from_position[pos] @@ -309,15 +64,15 @@ class Shelf: self.content_size = content_size self.image = pygame.transform.scale(pygame.image.load('img/shelf.png'), (60, 60)) self.rect = self.image.get_rect(center=self.pos_pix) - - + + class Stain: def __init__(self, pos): self.pos_pix = get_pix_from_position[pos] 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 @@ -328,99 +83,101 @@ class Agent: self.path = None self.goal_achieved = False self.agent_direction = agent_direction - + self.image_left = pygame.transform.flip(IMAGE, True, False) self.image_right = IMAGE self.image_down = pygame.transform.rotate(self.image_right, -90) self.image_up = pygame.transform.rotate(self.image_left, -90) - + def move_bfs(self): if self.path is None or not self.path: self.goal_achieved = True - + else: move = self.path.pop(0) - + if move == "rotate_left": if self.agent_direction == "right": self.agent_direction = "up" self.image = self.image_up - + elif self.agent_direction == "left": self.agent_direction = "down" self.image = self.image_down - + elif self.agent_direction == "up": self.agent_direction = "left" self.image = self.image_left - + elif self.agent_direction == "down": self.agent_direction = "right" self.image = self.image_right - + elif move == "rotate_right": if self.agent_direction == "right": self.agent_direction = "down" self.image = self.image_down - + elif self.agent_direction == "left": self.agent_direction = "up" self.image = self.image_up - + elif self.agent_direction == "up": self.agent_direction = "right" self.image = self.image_right - + elif self.agent_direction == "down": self.agent_direction = "left" self.image = self.image_left - + elif move == "move": if self.agent_direction == "right": self.rect.move_ip(size, 0) self.pos = (self.pos[0] + size, self.pos[1]) self.pos_coord = get_position_from_pix(self.pos) - + elif self.agent_direction == "left": self.rect.move_ip(-size, 0) self.pos = (self.pos[0] - size, self.pos[1]) self.pos_coord = get_position_from_pix(self.pos) - + elif self.agent_direction == "up": self.rect.move_ip(0, -size) self.pos = (self.pos[0], self.pos[1] - size) self.pos_coord = get_position_from_pix(self.pos) - + elif self.agent_direction == "down": self.rect.move_ip(0, size) self.pos = (self.pos[0], self.pos[1] + size) self.pos_coord = get_position_from_pix(self.pos) - + def lift_package_bfs(self): for package in Package_list: if package.is_package_up: package.is_package_up = False elif package.pos == agent.pos: package.is_package_up = True - + def goal_test(self, state): if state == self.goal: return True else: return False - - + + 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) - + agent = Agent((40, 640)) - + Shelf_list = [ Shelf((5, 2), 'Explosive', 5), Shelf((6, 2), 'Explosive', 5), Shelf((7, 2), 'Explosive', 5), Shelf((8, 2), 'Explosive', 5), @@ -429,77 +186,77 @@ Shelf_list = [ Shelf((8, 6), 'Builders', 5), Shelf((5, 8), 'Electronic', 5), Shelf((6, 8), 'Electronic', 5), Shelf((7, 8), 'Electronic', 5), Shelf((8, 8), 'Electronic', 5)] - + coord_goals = [(8, 0), (0, 0), (5, 8), (0, 0), (8, 0), (0, 0), (8, 8), (0, 0), (5, 6), (0, 0), (6, 6), (0, 0), (7, 6), (0, 0), (8, 6), (0, 8)] - + screen = pygame.display.set_mode([WIDTH, HEIGHT]) - + Package_list = [ generate_package(40, 40) ] - + + print(a_star_search((6, 8), (0, 0), "down")) - + #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) - + # 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: Stain_list.append(Stain((index_y, index_x))) - - running = True + while running: - + for event in pygame.event.get(): if event.type == pygame.QUIT: running = False - + if event.type == pygame.KEYDOWN: pass - + if len(Package_list) < QTY_OF_PACKAGES: - + is_dock_empty = True - + for package in Package_list: if package.pos == (40, 40): is_dock_empty = False elif package.pos == get_pix_from_position[(8, 0)] and not package.is_package_up: print("Paczka wyrzucona") Package_list.remove(package) - + if is_dock_empty: Package_list.append(generate_package(40, 40)) - + # PATHING if not agent.goal_achieved: # print(agent.path) agent.move_bfs() - + else: - + agent.lift_package_bfs() if coord_goals: agent.goal = coord_goals.pop(0) agent.goal_achieved = False - + #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: if package.is_package_up: package.rect.move_ip(agent.pos[0] - package.pos[0], agent.pos[1] - package.pos[1]) package.pos = agent.pos - + # DRAWING screen.blit(BACKGROUND, [0, 0]) screen.blit(DOCK, [0, 0]) @@ -512,9 +269,9 @@ while running: screen.blit(agent.image, agent.rect) for package in Package_list: screen.blit(package.image, package.rect) - + time.sleep(0.15) - + pygame.display.update() - -pygame.quit() + +pygame.quit() \ No newline at end of file