Fix path not connecting to start node

Author: guilyx

PathPlanning/BreadthFirstSearch/breadth_first_search.py

@@ -128,6 +128,9 @@ def calc_final_path(self, ngoal, closedset):
             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):