changed A*

classes/Garbagetruck.py

@@ -280,6 +280,7 @@ class Garbagetruck:
         temp = self.getPosition()[:]
         temp.append(self.getOrientation())
         initial = Node(temp)
+        initial.setCost(0)
         fringe.append((0, initial))  # (priority, node)
 
         while True:
@@ -309,7 +310,8 @@ class Garbagetruck:
                     x = Node(wynik["result"])
                     x.setParent(elem)
                     x.setAction(wynik["action"])
-                    priority = cost(x.getState()) + heuristic(x.getState())
+                    x.setCost(elem.getCost() + cost(x.getState()))
+                    priority = x.getCost() + heuristic(x.getState())
                     heapq.heappush(fringe, (priority, x))
 
     def executeMovement(self):

classes/Node.py

@@ -3,6 +3,7 @@ class Node:
         self.state = state
         self.action = ""
         self.parent: Node = None
+        self.cost = 0
 
     def getState(self):
         return self.state
@@ -21,5 +22,11 @@ class Node:
         self.parent = parent
         return self
 
+    def getCost(self):
+        return self.cost
+
+    def setCost(self, cost):
+        self.cost = cost
+
     def __lt__(self, other):
         return self.getState() < other.getState()