Handle goal node at index zero in RRT Dubins

PathPlanning/RRTDubins/rrt_dubins.py

@@ -89,13 +89,13 @@ def planning(self, animation=True, search_until_max_iter=True):
 
             if (not search_until_max_iter) and new_node:  # check reaching the goal
                 last_index = self.search_best_goal_node()
-                if last_index:
+                if last_index is not None:
                     return self.generate_final_course(last_index)
 
         print("reached max iteration")
 
         last_index = self.search_best_goal_node()
-        if last_index:
+        if last_index is not None:
             return self.generate_final_course(last_index)
         else:
             print("Cannot find path")

tests/test_rrt_dubins.py

@@ -7,5 +7,21 @@ def test1():
     m.main()
 
 
+def test_start_already_satisfies_goal():
+    planner = m.RRTDubins(
+        start=[0.0, 0.0, 0.0],
+        goal=[0.0, 0.0, 0.0],
+        obstacle_list=[],
+        rand_area=[-1.0, 1.0],
+        max_iter=0,
+    )
+
+    path = planner.planning(animation=False)
+
+    assert path is not None
+    assert path[0] == [0.0, 0.0]
+    assert path[-1] == [0.0, 0.0]
+
+
 if __name__ == '__main__':
     conftest.run_this_test(__file__)