Merge pull request #227 from AtsushiSakai/fix-last-expansion-collision-check

fix last expansion collision detection

PathPlanning/RRT/rrt.py

@@ -33,7 +33,7 @@ class RRT:
             self.parent = None
 
     def __init__(self, start, goal, obstacle_list, rand_area,
-                 expand_dis=3.0, path_resolution=1.0, goal_sample_rate=5, max_iter=500):
+                 expand_dis=3.0, path_resolution=0.5, goal_sample_rate=5, max_iter=500):
         """
         Setting Parameter
 
@@ -76,8 +76,9 @@ class RRT:
                 self.draw_graph(rnd_node)
 
             if self.calc_dist_to_goal(self.node_list[-1].x, self.node_list[-1].y) <= self.expand_dis:
-                print("Goal!!")
-                return self.generate_final_course(len(self.node_list) - 1)
+                final_node = self.steer(self.node_list[-1], self.end, self.expand_dis)
+                if self.check_collision(final_node, self.obstacle_list):
+                    return self.generate_final_course(len(self.node_list) - 1)
 
             if animation and i % 5:
                 self.draw_graph(rnd_node)
@@ -141,7 +142,7 @@ class RRT:
             plt.plot(rnd.x, rnd.y, "^k")
         for node in self.node_list:
             if node.parent:
-                plt.plot(node.path_x, node.path_y, "-xg")
+                plt.plot(node.path_x, node.path_y, "-g")
 
         for (ox, oy, size) in self.obstacle_list:
             self.plot_circle(ox, oy, size)
@@ -200,7 +201,8 @@ def main(gx=6.0, gy=10.0):
         (3, 8, 2),
         (3, 10, 2),
         (7, 5, 2),
-        (9, 5, 2)
+        (9, 5, 2),
+        (8, 10, 1)
     ]  # [x, y, radius]
     # Set Initial parameters
     rrt = RRT(start=[0, 0],

PathPlanning/RRT/rrt_with_pathsmoothing.py

@@ -76,8 +76,6 @@ def line_collision_check(first, second, obstacleList):
         if d <= size:
             return False
 
-    #  print("OK")
-
     return True  # OK
 
 
@@ -127,7 +125,7 @@ def main():
         (7, 5, 2),
         (9, 5, 2)
     ]  # [x,y,size]
-    rrt = RRT(start=[0, 0], goal=[5, 10],
+    rrt = RRT(start=[0, 0], goal=[6, 10],
               rand_area=[-2, 15], obstacle_list=obstacleList)
     path = rrt.planning(animation=show_animation)
 
@@ -141,7 +139,7 @@ def main():
         plt.plot([x for (x, y) in path], [y for (x, y) in path], '-r')
 
         plt.plot([x for (x, y) in smoothedPath], [
-            y for (x, y) in smoothedPath], '-b')
+            y for (x, y) in smoothedPath], '-c')
 
         plt.grid(True)
         plt.pause(0.01)  # Need for Mac

PathPlanning/RRTStar/rrt_star.py

@@ -52,6 +52,7 @@ class RRTStar(RRT):
 
         """
         self.connect_circle_dist = connect_circle_dist
+        self.goal_node = self.Node(goal[0], goal[1])
 
     def planning(self, animation=True, search_until_max_iter=True):
         """
@@ -121,7 +122,14 @@ class RRTStar(RRT):
         dist_to_goal_list = [self.calc_dist_to_goal(n.x, n.y) for n in self.node_list]
         goal_inds = [dist_to_goal_list.index(i) for i in dist_to_goal_list if i <= self.expand_dis]
 
-        if not goal_inds:
+        # safe_goal_inds = goal_inds
+        safe_goal_inds = []
+        for goal_ind in goal_inds:
+            t_node = self.steer(self.node_list[goal_ind], self.goal_node)
+            if self.check_collision(t_node, self.obstacle_list):
+                safe_goal_inds.append(goal_ind)
+
+        if not safe_goal_inds:
             return None
 
         min_cost = min([self.node_list[i].cost for i in goal_inds])
@@ -177,12 +185,13 @@ def main():
         (3, 8, 2),
         (3, 10, 2),
         (7, 5, 2),
-        (9, 5, 2)
+        (9, 5, 2),
+        (8, 10, 1),
     ]  # [x,y,size(radius)]
 
     # Set Initial parameters
     rrt_star = RRTStar(start=[0, 0],
-                       goal=[10, 10],
+                       goal=[6, 10],
                        rand_area=[-2, 15],
                        obstacle_list=obstacle_list)
     path = rrt_star.planning(animation=show_animation)