fix rrt_star_dubins

Author: Atsushi Sakai

PathPlanning/DubinsPath/dubins_path_planning.py

@@ -6,8 +6,9 @@
 
 """
 import math
-import numpy as np
+
 import matplotlib.pyplot as plt
+import numpy as np
 
 show_animation = True
 
@@ -136,8 +137,8 @@ def LRL(alpha, beta, d):
     return t, p, q, mode
 
 
-def dubins_path_planning_from_origin(ex, ey, eyaw, c):
-    # nomalize
+def dubins_path_planning_from_origin(ex, ey, eyaw, c, D_ANGLE):
+    # normalize
     dx = ex
     dy = ey
     D = math.sqrt(dx ** 2.0 + dy ** 2.0)
@@ -165,12 +166,12 @@ def dubins_path_planning_from_origin(ex, ey, eyaw, c):
             bcost = cost
 
     #  print(bmode)
-    px, py, pyaw = generate_course([bt, bp, bq], bmode, c)
+    px, py, pyaw = generate_course([bt, bp, bq], bmode, c, D_ANGLE)
 
     return px, py, pyaw, bmode, bcost
 
 
-def dubins_path_planning(sx, sy, syaw, ex, ey, eyaw, c):
+def dubins_path_planning(sx, sy, syaw, ex, ey, eyaw, c, D_ANGLE=np.deg2rad(10.0)):
     """
     Dubins path plannner
 
@@ -199,7 +200,7 @@ def dubins_path_planning(sx, sy, syaw, ex, ey, eyaw, c):
     leyaw = eyaw - syaw
 
     lpx, lpy, lpyaw, mode, clen = dubins_path_planning_from_origin(
-        lex, ley, leyaw, c)
+        lex, ley, leyaw, c, D_ANGLE)
 
     px = [math.cos(-syaw) * x + math.sin(-syaw)
           * y + sx for x, y in zip(lpx, lpy)]
@@ -210,7 +211,7 @@ def dubins_path_planning(sx, sy, syaw, ex, ey, eyaw, c):
     return px, py, pyaw, mode, clen
 
 
-def generate_course(length, mode, c):
+def generate_course(length, mode, c, D_ANGLE):
 
     px = [0.0]
     py = [0.0]
@@ -221,7 +222,7 @@ def generate_course(length, mode, c):
         if m == "S":
             d = 1.0 * c
         else:  # turning couse
-            d = np.deg2rad(3.0)
+            d = D_ANGLE
 
         while pd < abs(l - d):
             #  print(pd, l)

PathPlanning/RRT/rrt.py

@@ -19,7 +19,7 @@ class RRT:
     Class for RRT planning
     """
 
-    class Node():
+    class Node:
         """
         RRT Node
         """
@@ -104,6 +104,13 @@ def steer(self, from_node, to_node, extend_length=float("inf")):
             new_node.path_x.append(new_node.x)
             new_node.path_y.append(new_node.y)
 
+        d, _ = self.calc_distance_and_angle(new_node, to_node)
+        if d <= self.path_resolution:
+            new_node.x = to_node.x
+            new_node.y = to_node.y
+            new_node.path_x[-1] = to_node.x
+            new_node.path_y[-1] = to_node.y
+
         new_node.parent = from_node
 
         return new_node
@@ -137,9 +144,7 @@ def draw_graph(self, rnd=None):
             plt.plot(rnd.x, rnd.y, "^k")
         for node in self.node_list:
             if node.parent:
-                plt.plot([node.x, node.parent.x],
-                         [node.y, node.parent.y],
-                         "-g")
+                plt.plot(node.path_x, node.path_y, "-g")
 
         for (ox, oy, size) in self.obstacle_list:
             plt.plot(ox, oy, "ok", ms=30 * size)

PathPlanning/RRTStar/rrt_star.py

@@ -53,7 +53,7 @@ def __init__(self, start, goal, obstacle_list, rand_area,
         """
         self.connect_circle_dist = connect_circle_dist
 
-    def planning(self, animation=True, search_until_maxiter=True):
+    def planning(self, animation=True, search_until_max_iter=True):
         """
         rrt star path planning
 
@@ -63,7 +63,7 @@ def planning(self, animation=True, search_until_maxiter=True):
 
         self.node_list = [self.start]
         for i in range(self.max_iter):
-            print(i, len(self.node_list))
+            print("Iter:", i, ", number of nodes:", len(self.node_list))
             rnd = self.get_random_node()
             nearest_ind = self.get_nearest_list_index(self.node_list, rnd)
             new_node = self.steer(self.node_list[nearest_ind], rnd, self.expand_dis)
@@ -78,10 +78,10 @@ def planning(self, animation=True, search_until_maxiter=True):
             if animation and i % 5 == 0:
                 self.draw_graph(rnd)
 
-            if (not search_until_maxiter) and new_node:  # check reaching the goal
-                d, _ = self.calc_distance_and_angle(new_node, self.end)
-                if d <= self.expand_dis:
-                    return self.generate_final_course(len(self.node_list) - 1)
+            if (not search_until_max_iter) and new_node:  # check reaching the goal
+                last_index = self.search_best_goal_node()
+                if last_index:
+                    return self.generate_final_course(last_index)
 
         print("reached max iteration")
 
@@ -91,25 +91,6 @@ def planning(self, animation=True, search_until_maxiter=True):
 
         return None
 
-    def connect_nodes(self, from_node, to_node):
-        new_node = self.Node(from_node.x, from_node.y)
-        d, theta = self.calc_distance_and_angle(new_node, to_node)
-
-        new_node.path_x = [new_node.x]
-        new_node.path_y = [new_node.y]
-
-        n_expand = math.floor(d / self.path_resolution)
-
-        for _ in range(n_expand):
-            new_node.x += self.path_resolution * math.cos(theta)
-            new_node.y += self.path_resolution * math.sin(theta)
-            new_node.path_x.append(new_node.x)
-            new_node.path_y.append(new_node.y)
-
-        new_node.parent = from_node
-
-        return new_node
-
     def choose_parent(self, new_node, near_inds):
         if not near_inds:
             return None
@@ -130,8 +111,8 @@ def choose_parent(self, new_node, near_inds):
             return None
 
         min_ind = near_inds[costs.index(min_cost)]
+        new_node = self.steer(self.node_list[min_ind], new_node)
         new_node.parent = self.node_list[min_ind]
-        new_node = self.steer(new_node.parent, new_node)
         new_node.cost = min_cost
 
         return new_node
@@ -162,21 +143,22 @@ def rewire(self, new_node, near_inds):
         for i in near_inds:
             near_node = self.node_list[i]
             edge_node = self.steer(new_node, near_node)
-            edge_node.cost = self.calc_new_cost(near_node, new_node)
+            edge_node.cost = self.calc_new_cost(new_node, near_node)
 
             no_collision = self.check_collision(edge_node, self.obstacle_list)
             improved_cost = near_node.cost > edge_node.cost
 
             if no_collision and improved_cost:
+                near_node = edge_node
                 near_node.parent = new_node
-                near_node.cost = edge_node.cost
                 self.propagate_cost_to_leaves(new_node)
 
     def calc_new_cost(self, from_node, to_node):
         d, _ = self.calc_distance_and_angle(from_node, to_node)
         return from_node.cost + d
 
     def propagate_cost_to_leaves(self, parent_node):
+
         for node in self.node_list:
             if node.parent == parent_node:
                 node.cost = self.calc_new_cost(parent_node, node)