diff --git a/agent.py b/agent.py index b00b18a..3fd9e6f 100644 --- a/agent.py +++ b/agent.py @@ -1,4 +1,6 @@ from warehouse import Coordinates, Tile, Pack +from queue import PriorityQueue +from math import sqrt class Node: def __init__(self, coord_x, coord_y): @@ -11,6 +13,12 @@ class Node: if isinstance(other, Node): return self.x == other.x and self.y == self.y return False + + def __lt__(self, other): + return isinstance(other, Node) and self.g_cost < other.g_cost + + def __repr__(self): + return "Node:{}x{}".format(self.x, self.y) class Agent: def __init__(self, start_x, start_y, assigned_warehouse, radius=5): @@ -22,54 +30,54 @@ class Agent: self.transported_package = None self.dest = Node(1, 1) self.closed = list() - self.open = list() + self.open = PriorityQueue() self.path = list() - + + def find_path(self): + self.closed = [] + self.open = PriorityQueue() + import pdb start_node = Node(self.x, self.y) - self.open.append(start_node) + self.open.put((0, start_node)) while self.open: - current_node = self.open.pop() + _, current_node = self.open.get() print(current_node.x, current_node.y) self.closed.append(current_node) if current_node.x == self.dest.x and current_node.y == self.dest.y: + # pdb.set_trace() while current_node.x != start_node.x and current_node.y != start_node.y: self.path.append(current_node) current_node = current_node.parent return True neighbour_list = self.get_neighbours(current_node) for neighbour in neighbour_list: - if not self.check_if_closed(neighbour): - cost = current_node.g_cost + self.heur_cost_est(current_node, neighbour) - if (cost < neighbour.g_cost) or not self.check_if_open(neighbour): + cost = current_node.g_cost + self.heuristic(current_node, neighbour) + if self.check_if_closed(neighbour): + continue + if self.check_if_open(neighbour): + if neighbour.g_cost > cost: neighbour.g_cost = cost - neighbour.h_cost = self.heur_cost_est(neighbour, self.dest) neighbour.parent = current_node - if not self.check_if_open(neighbour): - self.open.append(neighbour); + else: + neighbour.g_cost = cost + neighbour.h_cost = self.heuristic(neighbour, self.dest) + neighbour.parent = current_node + self.open.put((neighbour.g_cost, neighbour)) + # pdb.set_trace() return False - def heur_cost_est(self, nodeA: Node, nodeB: Node): - deltaX = abs(nodeA.x - nodeB.x) - deltaY = abs(nodeA.y - nodeB.y) - if deltaX > deltaY: - return (14 * deltaY) + (10 * (deltaX - deltaY)) - return (14 * deltaX) + (10 *(deltaY - deltaX)) + def heuristic(self, nodeA: Node, nodeB: Node): + diff_x = pow(nodeB.x - nodeA.x, 2) + diff_y = pow(nodeB.y - nodeA.y, 2) + return round(sqrt(diff_x + diff_y), 3) - def check_if_open(self, nodeA: Node): - for node in self.open: - if node.x == nodeA.x and node.y == nodeA.y: - print("open") - return True - return False - - def check_if_closed(self, nodeA: Node): - for node in self.closed: - if node.x == nodeA.x and node.y == nodeA.y: - print("closed") - return True - return False - + def check_if_open(self, node: Node): + return (node.x, node.y) in [(n.x, n.y) for (_,n) in self.open.queue] + + def check_if_closed(self, node: Node): + return (node.x, node.y) in [(n.x, n.y) for n in self.closed] + def get_neighbours(self, node: Node): neighbours = [] print(neighbours) @@ -91,7 +99,7 @@ class Agent: next = self.path.pop() self.x = next.x self.y = next.y - + self.closed = [] def check_if_can_move(self, next_coords: Coordinates): tile_on_map = 0 <= next_coords.x < self.warehouse.width and 0 <= next_coords.y < self.warehouse.height if not tile_on_map: diff --git a/attributes.py b/attributes.py index 5976863..36ba969 100644 --- a/attributes.py +++ b/attributes.py @@ -25,5 +25,6 @@ COLORS = { 'yellow': (235, 235, 0), 'lightgreen': (70, 238, 70), 'red': (255, 0, 0), - 'lightblue': (135, 206, 250) + 'lightblue': (135, 206, 250), + 'orange': (255, 165, 0) } diff --git a/main.py b/main.py index 2915c35..7608b35 100644 --- a/main.py +++ b/main.py @@ -4,12 +4,13 @@ import agent import random import sys from attributes import PackSize, PackStatus, COLORS - +import pdb WINDOW_SIZE = (600, 600) COLOR_OF_FIELD = { 'Floor': 'gray', 'Rack': 'white', - 'Pack': 'yellow' + 'Pack': 'yellow', + 'path': 'orange' } TILE_WIDTH = 30 TILE_HEIGHT = 30 @@ -33,8 +34,10 @@ class MainGameFrame: self.draw_floor() self.draw_packages() self.draw_agent() + # pdb.set_trace() self.agent.move() pygame.display.update() + # pdb.set_trace() self.clock.tick(2) def draw_floor(self): @@ -50,10 +53,12 @@ class MainGameFrame: def draw_field(self, x, y): current_tile = self.warehouse_map.tiles[x][y] - if not isinstance(current_tile, warehouse.Tile): - current_tile = current_tile.lays_on_field if isinstance(current_tile, warehouse.Pack) else None + # if not isinstance(current_tile, warehouse.Tile): + # current_tile = current_tile.lays_on_field if isinstance(current_tile, warehouse.Pack) else None color = COLOR_OF_FIELD.get(current_tile.category.name, 'white') color = COLORS[color] + if (current_tile.x_position,current_tile.y_position) in [(a.x, a.y) for a in self.agent.path]: + color = COLORS.get('orange') pygame.draw.rect(self.display, COLORS['black'], (x * TILE_WIDTH, y * TILE_HEIGHT, TILE_WIDTH, TILE_HEIGHT)) pygame.draw.rect(self.display, color, diff --git a/warehouse.py b/warehouse.py index 319873e..ab1f5ec 100644 --- a/warehouse.py +++ b/warehouse.py @@ -162,7 +162,6 @@ class Warehouse: pack_x, pack_y = self._set_package_position() package_field = self.tiles[pack_x][pack_y] new_package = Pack(lays_on_field=package_field) - self.tiles[pack_x][pack_y] = new_package packages.append(new_package) return packages def _set_package_position(self):