Dodanie funkcji h(start, goal)

agent.py

@@ -1,5 +1,8 @@
 from warehouse import Coordinates, Tile, Pack
 from attributes import PackStatus, TURN_LEFT_DIRECTIONS, TURN_RIGHT_DIRECTIONS
+from queue import PriorityQueue
+from pathfinder import Node
+from math import sqrt
 
 class Agent:
     def __init__(self, start_x, start_y, assigned_warehouse, radius=5):
@@ -94,3 +97,63 @@ class Agent:
         self.is_loaded = True
         pack.lays_on_field = None
         self.transported_package = pack
+
+    def find_route(self, goal):
+        explored = []
+        # tu trzeba dodać kolejkę priorytetową fringe
+        fringe = PriorityQueue()
+        current_tile = self.assigned_warehouse.tiles[self.x][self.y]
+        fringe.put((1, current_tile))
+
+        while True:
+            if len(fringe) == 0:
+                return False
+            # elem to węzeł z kolejki o najmniejszym priorytecie (najlepszy)
+            elem = fringe.get()
+
+            # jeśli zgadzają się współrzędne x i y wybranego węzłą oraz celu
+            if elem.x_position == goal.x_position and elem.y_position == goal.y_position:
+                # tu powinna zostać zwrócona ścieżka
+                print("doszedl")
+                return True
+
+             # przeniesienie wybranego węzłą z fringe do explored
+            explored.append(elem)
+
+             # dla każdego następnika węzła elem
+             # trzeba napisać funkcję succ wyznaczającą następników elem
+             # succesor[0] - akcja
+             # succesor[1] - stan czyli współrzędne i zwrot
+             # succesor[2] - rodzic
+            succ = self.get_succ(elem)
+
+            for action, state in succ:
+                new_node = Node(elem, elem.x_position, elem.y_position)
+                # x[1] = succesor[1]
+                # x[2] = elem
+                # x[0] = akcja
+                # trzeba napisać funkcję f(x) liczącą priorytet danego węzła
+                # p = f(x)
+
+                # if x[1] not in fringe and x[1] not in explored:
+                #     fringe.append(x[1])
+                # nie wiem jak wyznaczyć priorytet który x już ma w kolejce
+                # elif x[1] in fringe and priorytet który stan x już ma w kolejce > p:
+            # tu chodzi o to że zmieni się akcja i rodzić, ale stan nie
+            # zamień w fringe stary węzeł o stanie x[1] z węzłem x
+
+    def h(self, start, goal):
+        diff_x = pow(goal.x_position - start.x_position,2)
+        diff_y = pow(goal.y_position - start.y_position, 2)
+        return round(sqrt(diff_x+diff_y), 3)
+
+    def get_succ(self, elem):
+        move = self.move
+        turn_left = self.turn_left
+        turn_right = self.turn_right
+        successors = [
+            (move, self.get_next_move_coordinates()),
+            (turn_right, TURN_RIGHT_DIRECTIONS.get(self.direction, self.direction)),
+            (turn_left, TURN_LEFT_DIRECTIONS.get(self.direction, self.direction))
+        ]
+        return successors

main.py

@@ -25,8 +25,8 @@ class MainGameFrame:
         self.clock = pygame.time.Clock()
 
     def run(self):
-        demo_agent_step = 1
-        demo_agent_sign = 1
+        # demo_agent_step = 1
+        # demo_agent_sign = 1
         while True:
             for event in pygame.event.get():
                 if event.type == pygame.QUIT:
@@ -35,13 +35,13 @@ class MainGameFrame:
             self.draw_floor()
             self.draw_packages()
             self.draw_agent()
-            self.agent.demo_agent_move(demo_agent_step, 8) #linijka w celu zademonstrowania poruszania się agenta
+            # self.agent.demo_agent_move(demo_agent_step, 8) #linijka w celu zademonstrowania poruszania się agenta
             # self.agent.move() #oryginalna linijka
-            demo_agent_step += (1*demo_agent_sign)
-            if demo_agent_step >= 8:
-                demo_agent_sign = -1
-            if demo_agent_step == 0:
-                demo_agent_sign = 1
+            # demo_agent_step += (1*demo_agent_sign)
+            # if demo_agent_step >= 8:
+            #     demo_agent_sign = -1
+            # if demo_agent_step == 0:
+            #     demo_agent_sign = 1
             pygame.display.update()
             self.clock.tick(5)
 

warehouse.py

@@ -7,10 +7,11 @@ import itertools
 Coordinates = namedtuple("Coordinates",'x y')
 
 class CategoryData:
-    def __init__(self, name, passable=False, can_store=True, location='general', pack_size=PackSize.ALL):
+    def __init__(self, name, passable=False, can_store=True, location='general', pack_size=PackSize.ALL, cost=1):
         self.name = name
         self.passable = passable
         self.can_store = can_store
+        self.cost = cost
         self.location = location
         self.pack_size = pack_size
 
@@ -34,8 +35,8 @@ class Pack:
         return status
 
 CATEGORY = {
-    'floor': CategoryData('Floor', True, False),  #lava
-    'rack': CategoryData('Rack', False, True),
+    'floor': CategoryData('Floor', True, False, cost=1),  #lava
+    'rack': CategoryData('Rack', False, True, cost=100000),
     # 'freezer': CategoryData('Freezer', False, True, location='cold_room')
 }