not adding parent if node in closed set

Author: guilyx

PathPlanning/BreadthFirstSearch/breadth_first_search.py

@@ -106,14 +106,15 @@ def planning(self, sx, sy, gx, gy):
             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+1, current)
+                                 current.cost + self.motion[i][2], c_id+1, None)
                 n_id = self.calc_grid_index(node)
 
                 # If the node is not safe, do nothing
                 if not self.verify_node(node):
                     continue
 
                 if n_id not in closed_set:
+                    node.parent = current
                     closed_set[n_id] = node
                     open_set[n_id] = node