minor fix in dijkstra.py

Author: yilunc2020

.gitignore

@@ -70,3 +70,4 @@ target/
 .ipynb_checkpoints
 
 matplotrecorder/*
+.vscode/settings.json

PathPlanning/Dijkstra/dijkstra.py

@@ -33,7 +33,7 @@ def __init__(self, x, y, cost, pind):
             self.x = x  # index of grid
             self.y = y  # index of grid
             self.cost = cost
-            self.pind = pind
+            self.pind = pind # index of previous Node
 
         def __str__(self):
             return str(self.x) + "," + str(self.y) + "," + str(self.cost) + "," + str(self.pind)
@@ -88,10 +88,10 @@ def planning(self, sx, sy, gx, gy):
             closedset[c_id] = current
 
             # expand search grid based on motion model
-            for i, _ in enumerate(self.motion):
-                node = self.Node(current.x + self.motion[i][0],
-                                 current.y + self.motion[i][1],
-                                 current.cost + self.motion[i][2], c_id)
+            for move_x, move_y, move_cost in self.motion:
+                node = self.Node(current.x + move_x,
+                                 current.y + move_y,
+                                 current.cost + move_cost, c_id)
                 n_id = self.calc_index(node)
 
                 if n_id in closedset:
@@ -124,11 +124,6 @@ def calc_final_path(self, ngoal, closedset):
 
         return rx, ry
 
-    def calc_heuristic(self, n1, n2):
-        w = 1.0  # weight of heuristic
-        d = w * math.hypot(n1.x - n2.x, n1.y - n2.y)
-        return d
-
     def calc_position(self, index, minp):
         pos = index*self.reso+minp
         return pos
@@ -242,6 +237,9 @@ def main():
     dijkstra = Dijkstra(ox, oy, grid_size, robot_radius)
     rx, ry = dijkstra.planning(sx, sy, gx, gy)
 
+    print(rx)
+    print(ry)
+
     if show_animation:  # pragma: no cover
         plt.plot(rx, ry, "-r")
         plt.show()