Bug RRT* fix, issues AtsushiSakai#382 and AtsushiSakai#383 (AtsushiSakai#401)

Author: rafaelrojasmiliani

PathPlanning/RRT/rrt.py

@@ -32,8 +32,15 @@ def __init__(self, x, y):
             self.path_y = []
             self.parent = None
 
-    def __init__(self, start, goal, obstacle_list, rand_area,
-                 expand_dis=3.0, path_resolution=0.5, goal_sample_rate=5, max_iter=500):
+    def __init__(self,
+                 start,
+                 goal,
+                 obstacle_list,
+                 rand_area,
+                 expand_dis=3.0,
+                 path_resolution=0.5,
+                 goal_sample_rate=5,
+                 max_iter=500):
         """
         Setting Parameter
 
@@ -75,8 +82,10 @@ def planning(self, animation=True):
             if animation and i % 5 == 0:
                 self.draw_graph(rnd_node)
 
-            if self.calc_dist_to_goal(self.node_list[-1].x, self.node_list[-1].y) <= self.expand_dis:
-                final_node = self.steer(self.node_list[-1], self.end, self.expand_dis)
+            if self.calc_dist_to_goal(self.node_list[-1].x,
+                                      self.node_list[-1].y) <= self.expand_dis:
+                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)
 
@@ -108,6 +117,8 @@ def steer(self, from_node, to_node, extend_length=float("inf")):
         if d <= self.path_resolution:
             new_node.path_x.append(to_node.x)
             new_node.path_y.append(to_node.y)
+            new_node.x = to_node.x
+            new_node.y = to_node.y
 
         new_node.parent = from_node
 
@@ -130,17 +141,19 @@ def calc_dist_to_goal(self, x, y):
 
     def get_random_node(self):
         if random.randint(0, 100) > self.goal_sample_rate:
-            rnd = self.Node(random.uniform(self.min_rand, self.max_rand),
-                            random.uniform(self.min_rand, self.max_rand))
+            rnd = self.Node(
+                random.uniform(self.min_rand, self.max_rand),
+                random.uniform(self.min_rand, self.max_rand))
         else:  # goal point sampling
             rnd = self.Node(self.end.x, self.end.y)
         return rnd
 
     def draw_graph(self, rnd=None):
         plt.clf()
         # for stopping simulation with the esc key.
-        plt.gcf().canvas.mpl_connect('key_release_event',
-                                     lambda event: [exit(0) if event.key == 'escape' else None])
+        plt.gcf().canvas.mpl_connect(
+            'key_release_event',
+            lambda event: [exit(0) if event.key == 'escape' else None])
         if rnd is not None:
             plt.plot(rnd.x, rnd.y, "^k")
         for node in self.node_list:
@@ -167,8 +180,8 @@ def plot_circle(x, y, size, color="-b"):  # pragma: no cover
 
     @staticmethod
     def get_nearest_node_index(node_list, rnd_node):
-        dlist = [(node.x - rnd_node.x) ** 2 + (node.y - rnd_node.y)
-                 ** 2 for node in node_list]
+        dlist = [(node.x - rnd_node.x)**2 + (node.y - rnd_node.y)**2
+                 for node in node_list]
         minind = dlist.index(min(dlist))
 
         return minind
@@ -184,7 +197,7 @@ def check_collision(node, obstacleList):
             dy_list = [oy - y for y in node.path_y]
             d_list = [dx * dx + dy * dy for (dx, dy) in zip(dx_list, dy_list)]
 
-            if min(d_list) <= size ** 2:
+            if min(d_list) <= size**2:
                 return False  # collision
 
         return True  # safe
@@ -202,20 +215,14 @@ def main(gx=6.0, gy=10.0):
     print("start " + __file__)
 
     # ====Search Path with RRT====
-    obstacleList = [
-        (5, 5, 1),
-        (3, 6, 2),
-        (3, 8, 2),
-        (3, 10, 2),
-        (7, 5, 2),
-        (9, 5, 2),
-        (8, 10, 1)
-    ]  # [x, y, radius]
+    obstacleList = [(5, 5, 1), (3, 6, 2), (3, 8, 2), (3, 10, 2), (7, 5, 2),
+                    (9, 5, 2), (8, 10, 1)]  # [x, y, radius]
     # Set Initial parameters
-    rrt = RRT(start=[0, 0],
-              goal=[gx, gy],
-              rand_area=[-2, 15],
-              obstacle_list=obstacleList)
+    rrt = RRT(
+        start=[0, 0],
+        goal=[gx, gy],
+        rand_area=[-2, 15],
+        obstacle_list=obstacleList)
     path = rrt.planning(animation=show_animation)
 
     if path is None:

PathPlanning/RRTStar/rrt_star.py

@@ -12,8 +12,7 @@
 
 import matplotlib.pyplot as plt
 
-sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
-                "/../RRT/")
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../RRT/")
 
 try:
     from rrt import RRT
@@ -33,15 +32,17 @@ def __init__(self, x, y):
             super().__init__(x, y)
             self.cost = 0.0
 
-    def __init__(self, start, goal, obstacle_list, rand_area,
+    def __init__(self,
+                 start,
+                 goal,
+                 obstacle_list,
+                 rand_area,
                  expand_dis=30.0,
                  path_resolution=1.0,
                  goal_sample_rate=20,
                  max_iter=300,
-                 connect_circle_dist=50.0
-                 ):
-        super().__init__(start, goal, obstacle_list,
-                         rand_area, expand_dis, path_resolution, goal_sample_rate, max_iter)
+                 connect_circle_dist=50.0,
+                 search_until_max_iter=False):
         """
         Setting Parameter
 
@@ -51,35 +52,46 @@ def __init__(self, start, goal, obstacle_list, rand_area,
         randArea:Random Sampling Area [min,max]
 
         """
+        super().__init__(start, goal, obstacle_list, rand_area, expand_dis,
+                         path_resolution, goal_sample_rate, max_iter)
         self.connect_circle_dist = connect_circle_dist
         self.goal_node = self.Node(goal[0], goal[1])
+        self.search_until_max_iter = search_until_max_iter
 
-    def planning(self, animation=True, search_until_max_iter=True):
+    def planning(self, animation=True):
         """
         rrt star path planning
 
-        animation: flag for animation on or off
-        search_until_max_iter: search until max iteration for path improving or not
+        animation: flag for animation on or off .
         """
 
         self.node_list = [self.start]
         for i in range(self.max_iter):
             print("Iter:", i, ", number of nodes:", len(self.node_list))
             rnd = self.get_random_node()
             nearest_ind = self.get_nearest_node_index(self.node_list, rnd)
-            new_node = self.steer(self.node_list[nearest_ind], rnd, self.expand_dis)
+            new_node = self.steer(self.node_list[nearest_ind], rnd,
+                                  self.expand_dis)
+            near_node = self.node_list[nearest_ind]
+            new_node.cost = near_node.cost + \
+                math.hypot(new_node.x-near_node.x,
+                           new_node.y-near_node.y)
 
             if self.check_collision(new_node, self.obstacle_list):
                 near_inds = self.find_near_nodes(new_node)
-                new_node = self.choose_parent(new_node, near_inds)
-                if new_node:
+                node_with_updated_parent = self.choose_parent(
+                    new_node, near_inds)
+                if node_with_updated_parent:
+                    self.rewire(node_with_updated_parent, near_inds)
+                    self.node_list.append(node_with_updated_parent)
+                else:
                     self.node_list.append(new_node)
-                    self.rewire(new_node, near_inds)
 
-            if animation and i % 5 == 0:
+            if animation:
                 self.draw_graph(rnd)
 
-            if (not search_until_max_iter) and new_node:  # check reaching the goal
+            if ((not self.search_until_max_iter)
+                    and new_node):  # if reaches goal
                 last_index = self.search_best_goal_node()
                 if last_index is not None:
                     return self.generate_final_course(last_index)
@@ -93,6 +105,21 @@ def planning(self, animation=True, search_until_max_iter=True):
         return None
 
     def choose_parent(self, new_node, near_inds):
+        """
+        Computes the cheapest point to new_node contained in the list
+        near_inds and set such a node as the parent of new_node.
+            Arguments:
+            --------
+                new_node, Node
+                    randomly generated node with a path from its neared point
+                    There are not coalitions between this node and th tree.
+                near_inds: list
+                    Indices of indices of the nodes what are near to new_node
+
+            Returns.
+            ------
+                Node, a copy of new_node
+        """
         if not near_inds:
             return None
 
@@ -113,14 +140,18 @@ def choose_parent(self, new_node, near_inds):
 
         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.cost = min_cost
 
         return new_node
 
     def search_best_goal_node(self):
-        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]
+        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
+        ]
 
         safe_goal_inds = []
         for goal_ind in goal_inds:
@@ -139,17 +170,48 @@ def search_best_goal_node(self):
         return None
 
     def find_near_nodes(self, new_node):
+        """
+        1) defines a ball centered on new_node
+        2) Returns all nodes of the three that are inside this ball
+            Arguments:
+            ---------
+                new_node: Node
+                    new randomly generated node, without collisions between
+                    its nearest node
+            Returns:
+            -------
+                list
+                    List with the indices of the nodes inside the ball of
+                    radius r
+        """
         nnode = len(self.node_list) + 1
         r = self.connect_circle_dist * math.sqrt((math.log(nnode) / nnode))
-        # if expand_dist exists, search vertices in a range no more than expand_dist
-        if hasattr(self, 'expand_dis'): 
+        # if expand_dist exists, search vertices in a range no more than
+        # expand_dist
+        if hasattr(self, 'expand_dis'):
             r = min(r, self.expand_dis)
-        dist_list = [(node.x - new_node.x) ** 2 +
-                     (node.y - new_node.y) ** 2 for node in self.node_list]
-        near_inds = [dist_list.index(i) for i in dist_list if i <= r ** 2]
+        dist_list = [(node.x - new_node.x)**2 + (node.y - new_node.y)**2
+                     for node in self.node_list]
+        near_inds = [dist_list.index(i) for i in dist_list if i <= r**2]
         return near_inds
 
     def rewire(self, new_node, near_inds):
+        """
+            For each node in near_inds, this will check if it is cheaper to
+            arrive to them from new_node.
+            In such a case, this will re-assign the parent of the nodes in
+            near_inds to new_node.
+            Parameters:
+            ----------
+                new_node, Node
+                    Node randomly added which can be joined to the tree
+
+                near_inds, list of uints
+                    A list of indices of the self.new_node which contains
+                    nodes within a circle of a given radius.
+            Remark: parent is designated in choose_parent.
+
+        """
         for i in near_inds:
             near_node = self.node_list[i]
             edge_node = self.steer(new_node, near_node)
@@ -161,7 +223,12 @@ def rewire(self, new_node, near_inds):
             improved_cost = near_node.cost > edge_node.cost
 
             if no_collision and improved_cost:
-                self.node_list[i] = edge_node
+                near_node.x = edge_node.x
+                near_node.y = edge_node.y
+                near_node.cost = edge_node.cost
+                near_node.path_x = edge_node.path_x
+                near_node.path_y = edge_node.path_y
+                near_node.parent = edge_node.parent
                 self.propagate_cost_to_leaves(new_node)
 
     def calc_new_cost(self, from_node, to_node):
@@ -192,10 +259,12 @@ def main():
     ]  # [x,y,size(radius)]
 
     # Set Initial parameters
-    rrt_star = RRTStar(start=[0, 0],
-                       goal=[6, 10],
-                       rand_area=[-2, 15],
-                       obstacle_list=obstacle_list)
+    rrt_star = RRTStar(
+        start=[0, 0],
+        goal=[6, 10],
+        rand_area=[-2, 15],
+        obstacle_list=obstacle_list,
+        expand_dis=1)
     path = rrt_star.planning(animation=show_animation)
 
     if path is None:
@@ -206,10 +275,9 @@ def main():
         # Draw final path
         if show_animation:
             rrt_star.draw_graph()
-            plt.plot([x for (x, y) in path], [y for (x, y) in path], '-r')
+            plt.plot([x for (x, y) in path], [y for (x, y) in path], 'r--')
             plt.grid(True)
-            plt.pause(0.01)  # Need for Mac
-            plt.show()
+    plt.show()
 
 
 if __name__ == '__main__':