Przebudowa, zastosowanie PriorityQueue, jednolita zawartość pól warehouse.tiles

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):
@@ -12,6 +14,12 @@ class 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):
 		self.x = start_x
@@ -22,53 +30,53 @@ 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_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, 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_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 = []
@@ -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:

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)
              }

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,

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):