Dodanie szukania najbliższej paczki, szukania najbliższego regału pasującego do paczki, i ładowania i odkładania paczki na miejsce

agent.py

@@ -1,13 +1,16 @@
 from warehouse import Coordinates, Tile, Pack
 from queue import PriorityQueue
 from math import sqrt
-from attributes import TURN_LEFT_DIRECTIONS, TURN_RIGHT_DIRECTIONS
+from attributes import TURN_LEFT_DIRECTIONS, TURN_RIGHT_DIRECTIONS, PackStatus, PackSize
+import pdb
 
 class Node:
-	def __init__(self, coord_x, coord_y):
+	def __init__(self, coord_x, coord_y, package=None, is_rack=False):
 		self.x = coord_x
 		self.y = coord_y
 		self.parent = None
+		self.package = package
+		self.is_rack = is_rack
 		self.g_cost = 0
 		self.h_cost = 0
 	def __eq__(self, other):
@@ -38,12 +41,20 @@ class Agent:
 
 	def find_path(self):
 		self.closed = []
+		self.path = []
 		self.open = PriorityQueue()
+		if self.is_loaded:
+			rack = self.find_nearest_rack_for(self.transported_package)
+			self.dest = Node(rack.x_position, rack.y_position, is_rack=True)
+		else:
+			package = self.find_nearest_package()
+			self.dest = Node(package.lays_on_field.x_position, package.lays_on_field.y_position, package=package)
+
 		start_node = Node(self.x, self.y)
+
 		self.open.put((0, start_node))
 		while self.open:
 			_, 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:
 				while current_node.x != start_node.x or current_node.y != start_node.y:
@@ -98,13 +109,21 @@ class Agent:
 		return neighbours
     	
 	def move(self):
+		dest_coords = (self.dest.x, self.dest.y)
 		if not self.path:
+			print("chuj")
 			if not self.find_path():
 				return
 		else:
 			next = self.path.pop()
+			if (next.x, next.y) == dest_coords:
+				if self.dest.package:
+					self.pick_up_package(self.dest.package)
+					return
+				elif self.dest.is_rack:
+					self.unload_package(self.dest)
+					return
 			star_dir = self.direction
-			print(next.x, next.y)
 			if self.x > next.x and not self.direction == 'left':
 				if self.direction == 'down':
 					self.turn_right()
@@ -133,17 +152,70 @@ class Agent:
 				self.path.append(next)
 		self.closed = []
 		
-	def check_if_can_move(self, next_coords: Coordinates):
+	def check_if_can_move(self, next_coords: Coordinates, rack_searching=False):
 		tile_on_map = 0 <= next_coords.x < self.warehouse.width and 0 <= next_coords.y < self.warehouse.height
+		tile_passable = True
 		if not tile_on_map:
 			return False
 		next_tile = self.warehouse.tiles[next_coords.x][next_coords.y]
+		if not self.is_loaded:
 			tile_passable = isinstance(next_tile, Tile) and next_tile.category.passable
 		return tile_passable
 
+	def find_nearest_package(self):
+		packages_costs = []
+		start_node = Node(self.x, self.y)
+		if not self.warehouse.packages:
+			return
+		for package in self.warehouse.packages:
+			if package.status == PackStatus.STORED:
+				continue
+			new_node = Node(package.lays_on_field.x_position, package.lays_on_field.y_position)
+			cost = self.heuristic(start_node, new_node)
+			if cost > 0:
+				packages_costs.append((package, cost))
+		package = min(packages_costs, key=lambda l: l[1])[0]
+		return package
+
+	def find_nearest_rack_for(self, package, expand_box=0):
+		weight = package.size
+		accepted_weights = [weight, PackSize.ALL]
+		start_node = Node(self.x, self.y)
+		quarter_x = int(self.warehouse.width/4) + expand_box
+		quarter_y = int(self.warehouse.height/4) + expand_box
+		start_quarter_x = self.x - quarter_x if self.x - quarter_x > 0 else 0
+		end_quarter_x = self.x + quarter_x if self.x + quarter_x < self.warehouse.width else self.warehouse.width - 1
+		start_quarter_y = self.y - quarter_y if self.y - quarter_y > 0 else 0
+		end_quarter_y = self.y + quarter_y if self.y + quarter_y < self.warehouse.height else self.warehouse.height - 1
+		quarter = [row[start_quarter_y:end_quarter_y] for row in self.warehouse.tiles[start_quarter_x:end_quarter_x]]
+		quarter_racks = [[t for t in row if t.category.name == "Rack" and not t.occupied and t.category.pack_size in accepted_weights] for row in quarter]
+		quarter_racks = [t for row in quarter_racks for t in row]
+		racks_costs = []
+		if not quarter_racks:
+			self.find_nearest_rack_for(package, expand_box+1)
+		for rack in quarter_racks:
+			new_node = Node(rack.x_position, rack.y_position)
+			cost = self.heuristic(start_node, new_node)
+			if cost > 0:
+				racks_costs.append((rack, cost))
+		rack = min(racks_costs, key=lambda l: l[1])[0]
+		return rack
+
+
 	def pick_up_package(self, pack):
-		tile = pack.lays_on_field
-		self.assigned_warehouse.tiles[tile.x_position][tile.y_position] = tile
+		self.warehouse.packages.remove(pack)
 		self.is_loaded = True
+		self.warehouse.tiles[pack.lays_on_field.x_position][pack.lays_on_field.y_position].occupied = False
+		self.dest.package = None
 		pack.lays_on_field = None
 		self.transported_package = pack
+
+	def unload_package(self, rack):
+		pack = self.transported_package
+		tile = self.warehouse.tiles[rack.x][rack.y]
+		self.transported_package = None
+		self.is_loaded = False
+		pack.lays_on_field = tile
+		self.warehouse.tiles[rack.x][rack.y].occupied = True
+		pack.status = PackStatus.STORED
+		self.warehouse.packages.append(pack)

main.py

@@ -21,7 +21,7 @@ class MainGameFrame:
         self.display = pygame.display.set_mode(WINDOW_SIZE)
         agent_radius = int(TILE_WIDTH/2)
         self.agent_tex = pygame.image.load('forklift.png')
-        self.warehouse_map = warehouse.Warehouse(20, 20, 150, 20)
+        self.warehouse_map = warehouse.Warehouse(20, 20, 150, 25)
         starting_x, starting_y = self.set_starting_agent_position()
         self.agent = agent.Agent(starting_x, starting_y, self.warehouse_map, agent_radius)
         self.clock = pygame.time.Clock()

warehouse.py

@@ -161,6 +161,7 @@ class Warehouse:
         for i in range(no_of_packages):
             pack_x, pack_y = self._set_package_position()
             package_field = self.tiles[pack_x][pack_y]
+            self.tiles[pack_x][pack_y].occupied = True
             new_package = Pack(lays_on_field=package_field)
             packages.append(new_package)
         return packages
@@ -206,6 +207,7 @@ class Tile:
         self.category = CATEGORY.get(category, CATEGORY['floor'])
         self.x_position = x_position
         self.y_position = y_position
+        self.occupied = False
 
 
     def __str__(self):