A*

classes/Garbagetruck.py

@@ -1,5 +1,6 @@
 import math
 import random
+import heapq
 
 import pygame
 from classes.Household import *
@@ -213,6 +214,14 @@ class Garbagetruck:
         self.modPosiotion(stepX, stepY)
 
     def graphsearch(self):
+        house_positions = [(15, 5), (17, 5), (19, 5), (21, 5), (15, 8), (17, 8), (19, 8), (21, 8)]
+        trash_cans = [
+            {'position': (1, 0), 'type': 'paper'},
+            {'position': (1, 1), 'type': 'metals_and_plastics'},
+            {'position': (1, 2), 'type': 'mixed'},
+            {'position': (1, 3), 'type': 'bio_waste'},
+            {'position': (1, 4), 'type': 'glass'},
+        ]
         def succ(elem):
             def virtRotateLeft(state):
                 ort = (state[-1] + 1) % 4
@@ -243,6 +252,7 @@ class Garbagetruck:
                 result = [x, y, ort]
                 return result
 
+
             op = elem.getState()
             forward = {"result": virtMoveForward(op), "action": "F"}
             left = {"result": virtRotateLeft(op), "action": "L"}
@@ -250,23 +260,34 @@ class Garbagetruck:
             # print("got children")
             return [forward, left, right]
 
+        def heuristic(state):
+            x, y, _ = state
+            target_x, target_y = self.target.getPosition()
+            return abs(target_x - x) + abs(target_y - y)
+
+        def cost(state):
+            x, y, _ = state
+            if (x, y) in house_positions:
+                return 10
+            elif any(can['position'] == (x, y) for can in trash_cans):
+                return 5
+            else:
+                return 1
+
         fringe = []
         explored = []
         target = self.target.getPosition()
         temp = self.getPosition()[:]
         temp.append(self.getOrientation())
         initial = Node(temp)
-        fringe.append(initial)
+        fringe.append((0, initial))  # (priority, node)
 
         while True:
-            fringPos = [item.getState() for item in fringe]
-            explPos = [item.getState() for item in explored]
             if not fringe:
                 return False
-
-            elem = fringe.pop(0)
+            priority, elem = heapq.heappop(fringe)
             virtPos = elem.getState()[:-1]
-            dist = abs(virtPos[0]-target[0]) + abs(virtPos[1]-target[1])
+            dist = abs(virtPos[0] - target[0]) + abs(virtPos[1] - target[1])
             if dist == 0:
                 def findWay(node):
                     temp = node
@@ -284,11 +305,12 @@ class Garbagetruck:
             explored.append(elem)
             suc = succ(elem)
             for wynik in suc:
-                if wynik['result'] not in fringPos and wynik['result'] not in explPos:
+                if wynik['result'] not in [item[1].getState() for item in fringe] and wynik['result'] not in [item.getState() for item in explored]:
                     x = Node(wynik["result"])
                     x.setParent(elem)
                     x.setAction(wynik["action"])
-                    fringe.append(x)
+                    priority = cost(x.getState()) + heuristic(x.getState())
+                    heapq.heappush(fringe, (priority, x))
 
     def executeMovement(self):
         element = self.movesequence.pop(0) if self.movesequence else ""

classes/Node.py

@@ -20,3 +20,6 @@ class Node:
     def setParent(self, parent):
         self.parent = parent
         return self
+
+    def __lt__(self, other):
+        return self.getState() < other.getState()

main.py

@@ -9,7 +9,7 @@ MULT = 50
 SIZE = (MULT*W, MULT*H)
 pygame.init()
 screen = pygame.display.set_mode(SIZE)
-tilemap = Tilemap(Tileset("sprites/Tiles/1 Tiles/FieldsTile_38.png", mult=MULT), mult=MULT, size=(W, H))
+tilemap = Tilemap(Tileset("sprites/TIles/1Tiles/FieldsTile_38.png", mult=MULT), mult=MULT, size=(W, H))
 targimage = pygame.image.load("sprites/X.png").convert_alpha()
 targimage = pygame.transform.scale(targimage, (MULT, MULT))
 
@@ -52,4 +52,4 @@ while running:
     time.sleep(0.5)
 
 
-pygame.quit()
+pygame.quit()