easier path retrieving and shuffle children node

Author: guilyx

PathPlanning/BreadthFirstSearch/breadth_first_search.py

@@ -100,6 +100,8 @@ def planning(self, sx, sy, gx, gy):
             # Remove the item from the open set
             del open_set[c_id]
 
+            random.shuffle(self.motion)
+
             # expand_grid search grid based on motion model
             for i, _ in enumerate(self.motion):
                 node = self.Node(current.x + self.motion[i][0],
@@ -123,14 +125,11 @@ def calc_final_path(self, ngoal, closedset):
         rx, ry = [self.calc_grid_position(ngoal.x, self.minx)], [
             self.calc_grid_position(ngoal.y, self.miny)]
         n = closedset[ngoal.pind]
-        while n.parent is not None:
+        while n is not None:
             rx.append(self.calc_grid_position(n.x, self.minx))
             ry.append(self.calc_grid_position(n.y, self.miny))
             n = n.parent
 
-        rx.append(self.calc_grid_position(n.x, self.minx))
-        ry.append(self.calc_grid_position(n.y, self.miny))
-
         return rx, ry
 
     def calc_grid_position(self, index, minp):