code clean up again

Author: AtsushiSakai

PathPlanning/RRT/rrt.py

@@ -27,24 +27,27 @@ class Node():
         def __init__(self, x, y):
             self.x = x
             self.y = y
+            self.path_x = []
+            self.path_y = []
             self.parent = None
 
-    def __init__(self, start, goal, obstacle_list,
-                 rand_area, expand_dis=1.0, goal_sample_rate=5, max_iter=500):
+    def __init__(self, start, goal, obstacle_list, rand_area,
+                 expand_dis=3.0, path_resolution=1.0, goal_sample_rate=5, max_iter=500):
         """
         Setting Parameter
 
         start:Start Position [x,y]
         goal:Goal Position [x,y]
         obstacleList:obstacle Positions [[x,y,size],...]
-        randArea:Ramdom Samping Area [min,max]
+        randArea:Random Sampling Area [min,max]
 
         """
         self.start = self.Node(start[0], start[1])
         self.end = self.Node(goal[0], goal[1])
         self.min_rand = rand_area[0]
         self.max_rand = rand_area[1]
         self.expand_dis = expand_dis
+        self.path_resolution = path_resolution
         self.goal_sample_rate = goal_sample_rate
         self.max_iter = max_iter
         self.obstacle_list = obstacle_list
@@ -63,7 +66,7 @@ def planning(self, animation=True):
             nearest_ind = self.get_nearest_list_index(self.node_list, rnd_node)
             nearest_node = self.node_list[nearest_ind]
 
-            new_node = self.steer(nearest_node, rnd_node)
+            new_node = self.steer(nearest_node, rnd_node, self.expand_dis)
             new_node.parent = nearest_node
 
             if not self.check_collision(new_node, self.obstacle_list):
@@ -82,16 +85,25 @@ def planning(self, animation=True):
 
         return None  # cannot find path
 
-    def steer(self, from_node, to_node):
-        d, theta = self.calc_distance_and_angle(from_node, to_node)
-        if d > self.expand_dis:
-            x = from_node.x + self.expand_dis * math.cos(theta)
-            y = from_node.y + self.expand_dis * math.sin(theta)
-        else:
-            x = to_node.x
-            y = to_node.y
+    def steer(self, from_node, to_node, extend_length=float("inf")):
+
+        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]
+
+        if extend_length > d:
+            extend_length = d
+
+        n_expand = math.floor(extend_length / 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 = self.Node(x, y)
         new_node.parent = from_node
 
         return new_node
@@ -149,10 +161,11 @@ def get_nearest_list_index(node_list, rnd_node):
     @staticmethod
     def check_collision(node, obstacleList):
         for (ox, oy, size) in obstacleList:
-            dx = ox - node.x
-            dy = oy - node.y
-            d = dx * dx + dy * dy
-            if d <= size ** 2:
+            dx_list = [ox - x for x in node.path_x]
+            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:
                 return False  # collision
 
         return True  # safe

PathPlanning/RRTStar/rrt_star.py

@@ -6,7 +6,6 @@
 
 """
 
-import copy
 import math
 import os
 import sys
@@ -29,21 +28,20 @@ class RRTStar(RRT):
     Class for RRT Star planning
     """
 
-    class Node:
+    class Node(RRT.Node):
         def __init__(self, x, y):
-            self.x = x
-            self.y = y
+            super().__init__(x, y)
             self.cost = 0.0
-            self.parent = None
 
     def __init__(self, start, goal, obstacle_list, rand_area,
-                 expand_dis=0.5,
+                 expand_dis=3.0,
+                 path_resolution=1.0,
                  goal_sample_rate=20,
-                 max_iter=500,
+                 max_iter=300,
                  connect_circle_dist=50.0
                  ):
         super().__init__(start, goal, obstacle_list,
-                         rand_area, expand_dis, goal_sample_rate, max_iter)
+                         rand_area, expand_dis, path_resolution, goal_sample_rate, max_iter)
         """
         Setting Parameter
 
@@ -65,11 +63,12 @@ def planning(self, animation=True, search_until_maxiter=True):
 
         self.node_list = [self.start]
         for i in range(self.max_iter):
-            rnd = self.get_random_point()
+            print(i, len(self.node_list))
+            rnd = self.get_random_node()
             nearest_ind = self.get_nearest_list_index(self.node_list, rnd)
-            new_node = self.steer(rnd, self.node_list[nearest_ind])
+            new_node = self.steer(self.node_list[nearest_ind], rnd, self.expand_dis)
 
-            if self.check_collision(new_node, self.obstacleList):
+            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:
@@ -79,7 +78,7 @@ 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
+            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)
@@ -92,16 +91,36 @@ 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
 
         # search nearest cost in near_inds
         costs = []
         for i in near_inds:
-            d, theta = self.calc_distance_and_angle(self.node_list[i], new_node)
-            if self.check_collision_extend(self.node_list[i], theta, d):
-                costs.append(self.node_list[i].cost + d)
+            near_node = self.node_list[i]
+            t_node = self.steer(near_node, new_node)
+            if self.check_collision(t_node, self.obstacle_list):
+                costs.append(self.calc_new_cost(near_node, new_node))
             else:
                 costs.append(float("inf"))  # the cost of collision node
         min_cost = min(costs)
@@ -110,9 +129,10 @@ def choose_parent(self, new_node, near_inds):
             print("There is no good path.(min_cost is inf)")
             return None
 
-        new_node.cost = min_cost
         min_ind = near_inds[costs.index(min_cost)]
         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
 
@@ -141,34 +161,27 @@ def find_near_nodes(self, new_node):
     def rewire(self, new_node, near_inds):
         for i in near_inds:
             near_node = self.node_list[i]
-            d, theta = self.calc_distance_and_angle(near_node, new_node)
-            new_cost = new_node.cost + d
+            edge_node = self.steer(new_node, near_node)
+            edge_node.cost = self.calc_new_cost(near_node, new_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.parent = new_node
+                near_node.cost = edge_node.cost
+                self.propagate_cost_to_leaves(new_node)
 
-            if near_node.cost > new_cost:
-                if self.check_collision_extend(near_node, theta, d):
-                    near_node.parent = new_node
-                    near_node.cost = new_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:
-                d, _ = self.calc_distance_and_angle(parent_node, node)
-                node.cost = parent_node.cost + d
+                node.cost = self.calc_new_cost(parent_node, node)
                 self.propagate_cost_to_leaves(node)
 
-    def check_collision_extend(self, near_node, theta, d):
-
-        tmp_node = copy.deepcopy(near_node)
-
-        for i in range(int(d / self.expand_dis)):
-            tmp_node.x += self.expand_dis * math.cos(theta)
-            tmp_node.y += self.expand_dis * math.sin(theta)
-            if not self.check_collision(tmp_node, self.obstacleList):
-                return False
-
-        return True
-
 
 def main():
     print("Start " + __file__)
@@ -184,11 +197,11 @@ def main():
     ]  # [x,y,size(radius)]
 
     # Set Initial parameters
-    rrt = RRTStar(start=[0, 0],
-                  goal=[10, 10],
-                  rand_area=[-2, 15],
-                  obstacle_list=obstacle_list)
-    path = rrt.planning(animation=show_animation, search_until_maxiter=False)
+    rrt_star = RRTStar(start=[0, 0],
+                       goal=[10, 10],
+                       rand_area=[-2, 15],
+                       obstacle_list=obstacle_list)
+    path = rrt_star.planning(animation=show_animation)
 
     if path is None:
         print("Cannot find path")
@@ -197,7 +210,7 @@ def main():
 
         # Draw final path
         if show_animation:
-            rrt.draw_graph()
+            rrt_star.draw_graph()
             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

tests/test_rrt_star.py

@@ -7,7 +7,7 @@
 
 try:
     import rrt_star as m
-except:
+except ImportError:
     raise