update RRT

Sampling-based Planning/RRT.py

@@ -1,18 +1,14 @@
 import env
 import plotting
-import node
 
 import numpy as np
 import math
 
-import matplotlib.pyplot as plt
-import matplotlib.patches as patches
-
 
 class Node:
-    def __init__(self, x, y):
-        self.x = x
-        self.y = y
+    def __init__(self, n):
+        self.x = n[0]
+        self.y = n[1]
         self.path_x = []
         self.path_y = []
         self.parent = None
@@ -20,96 +16,39 @@ class Node:
 
 class RRT:
     def __init__(self, xI, xG):
-        self.xI = Node(xI[0], xI[1])
-        self.xG = node.Node(xG[0], xG[1])
+        self.xI = Node(xI)
+        self.xG = Node(xG)
+        self.expand_len = 0.8
+        self.goal_sample_rate = 0.05
+        self.iterations = 5000
+        self.node_list = [self.xI]
 
         self.env = env.Env()
-        # self.plotting = plotting.Plotting(xI, xG)
+        self.plotting = plotting.Plotting(xI, xG)
 
         self.x_range = self.env.x_range
         self.y_range = self.env.y_range
+        self.obs_circle = self.env.obs
+        self.obs_rectangle = self.env.obs_boundary
 
-        # self.obs_boundary = self.env.obs_boundary
-        # self.obs_circle = self.env.obs_circle
-
-        self.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]
-
-        self.expand_range = 0.8
-        self.goal_sample_rate = 0.05
-        self.iterations = 500
-        self.node_list = []
-
-        path = self.planning()
-
-        if path is None:
-            print("No path!")
-        else:
-            print("get it!")
-
-        self.draw_graph()
-
-        plt.plot([x[0] for x in path], [x[1] for x in path], '-r')
-        plt.grid(True)
-        plt.pause(0.01)  # Need for Mac
-        plt.show()
+        self.path = self.planning()
+        self.plotting.animation(self.node_list, self.path)
 
     def planning(self):
-        self.node_list = [self.xI]
         for i in range(self.iterations):
             node_rand = self.generate_random_node()
             node_near = self.get_nearest_node(self.node_list, node_rand)
+            node_new = self.new_node(node_near, node_rand, self.expand_len)
 
-            node_new = self.new_node(node_near, node_rand, self.expand_range)
-
-            if not self.check_collision(node_new, self.obstacleList):
+            if not self.check_collision(node_new, self.obs_circle, self.obs_rectangle):
                 self.node_list.append(node_new)
 
-            self.draw_graph(node_rand)
-
-            if self.cal_dis_to_goal(self.node_list[-1]) <= self.expand_range:
-                node_end = self.new_node(self.node_list[-1], node.Node(self.xG.x, self.xG.y), self.expand_range)
-                if not self.check_collision(node_end, self.obstacleList):
-                    return self.extract_path(self.node_list)
+            if self.cal_dis_to_goal(self.node_list[-1]) <= self.expand_len:
+                self.new_node(self.node_list[-1], self.xG, self.expand_len)
+                return self.extract_path(self.node_list)
 
         return None
 
-    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])
-        if rnd is not None:
-            plt.plot(rnd.x, rnd.y, "^k")
-        for node_x in self.node_list:
-            if node_x.parent:
-                plt.plot(node_x.path_x, node_x.path_y, "-g")
-
-        for (ox, oy, size) in self.obstacleList:
-            self.plot_circle(ox, oy, size)
-
-        plt.plot(self.xI.x, self.xI.y, "xr")
-        plt.plot(self.xG.x, self.xG.y, "xr")
-        plt.axis("equal")
-        plt.axis([-2, 15, -2, 15])
-        plt.grid(True)
-        plt.pause(0.01)
-
-    @staticmethod
-    def plot_circle(x, y, size, color="-b"):  # pragma: no cover
-        deg = list(range(0, 360, 5))
-        deg.append(0)
-        xl = [x + size * math.cos(np.deg2rad(d)) for d in deg]
-        yl = [y + size * math.sin(np.deg2rad(d)) for d in deg]
-        plt.plot(xl, yl, color)
-
     def extract_path(self, nodelist):
         path = [(self.xG.x, self.xG.y)]
         node_now = nodelist[-1]
@@ -121,20 +60,20 @@ class RRT:
         return path
 
     def cal_dis_to_goal(self, node_cal):
-        return math.hypot((node_cal.x - self.xG.x), (node_cal.y - self.xG.y))
+        return math.hypot(node_cal.x - self.xG.x, node_cal.y - self.xG.y)
 
-    def new_node(self, node_start, node_goal, expand_range):
-        new_node = node.Node(node_start.x, node_start.y)
+    def new_node(self, node_start, node_goal, expand_len):
+        new_node = Node((node_start.x, node_start.y))
         d, theta = self.calc_distance_and_angle(new_node, node_goal)
 
         new_node.path_x = [new_node.x]
         new_node.path_y = [new_node.y]
 
-        if d < expand_range:
-            expand_range = d
+        if d < expand_len:
+            expand_len = d
 
-        new_node.x += expand_range * math.cos(theta)
-        new_node.y += expand_range * math.sin(theta)
+        new_node.x += expand_len * math.cos(theta)
+        new_node.y += expand_len * math.sin(theta)
         new_node.path_x.append(new_node.x)
         new_node.path_y.append(new_node.y)
 
@@ -144,58 +83,38 @@ class RRT:
 
     def generate_random_node(self):
         if np.random.random() > self.goal_sample_rate:
-            return node.Node(np.random.uniform(self.x_range[0], self.x_range[1]),
-                             np.random.uniform(self.y_range[0], self.y_range[1]))
-        else:
-            return node.Node(self.xG.x, self.xG.y)
+            return Node((np.random.uniform(self.x_range[0], self.x_range[1]),
+                         np.random.uniform(self.y_range[0], self.y_range[1])))
+        return self.xG
 
-    def get_nearest_node(self, node_list, node_random):
-        dlist = [(nod.x - node_random.x) ** 2 + (nod.y - node_random.y) ** 2
-                 for nod in node_list]
-        minind = dlist.index(min(dlist))
-
-        return self.node_list[minind]
+    def get_nearest_node(self, node_list, n):
+        return self.node_list[int(np.argmin([math.hypot(nd.x - n.x, nd.y - n.y)
+                                             for nd in node_list]))]
 
     @staticmethod
     def calc_distance_and_angle(from_node, to_node):
         dx = to_node.x - from_node.x
         dy = to_node.y - from_node.y
-        d = math.hypot(dx, dy)
-        theta = math.atan2(dy, dx)
-        return d, theta
-
-    def check_collision(self, node_check, obstacleList):
+        return math.hypot(dx, dy), math.atan2(dy, dx)
 
-        if node_check is None:
+    @staticmethod
+    def check_collision(node_end, obs_circle, obs_rectangle):
+        if node_end is None:
             return True
 
-        for (ox, oy, size) in obstacleList:
-            dx_list = [ox - x for x in node_check.path_x]
-            dy_list = [oy - y for y in node_check.path_y]
-            d_list = [dx * dx + dy * dy for (dx, dy) in zip(dx_list, dy_list)]
-
-            if min(d_list) <= size ** 2:
-                return True  # collision
-
-        return False  # safe
-    #
-    # def check_collision(self, node_check):
-    #     for obs in self.obs_boundary:
-    #         dx = node_check.x - obs[0]
-    #         dy = node_check.y - obs[1]
-    #         if 0 <= dx <= obs[2] and 0 <= dy <= obs[2]:
-    #             return True
-    #
-    #     for obs in self.obs_circle:
-    #         d = (node_check.x - obs[0]) ** 2 + (node_check.y - obs[1]) ** 2
-    #         if d <= obs[2] ** 2:
-    #             return True
-    #
-    #     return False
+        for (ox, oy, r) in obs_circle:
+            if math.hypot(node_end.x - ox, node_end.y - oy) <= r:
+                return True
+
+        for (ox, oy, w, h) in obs_rectangle:
+            if 0 <= (node_end.x - ox) <= w and 0 <= (node_end.y - oy) <= h:
+                return True
+
+        return False
 
 
 if __name__ == '__main__':
-    x_Start = (0, 0)  # Starting node
-    x_Goal = (6, 10)  # Goal node
+    x_Start = (15, 5)  # Starting node
+    x_Goal = (45, 25)  # Goal node
 
     rrt = RRT(x_Start, x_Goal)

Sampling-based Planning/env.py

@@ -1,48 +1,34 @@
-import numpy as np
-
-
 class Env:
     def __init__(self):
-        self.x_range = (-2, 15)  # size of background
-        self.y_range = (-2, 15)
-        # self.obs_boundary = self.obs_boundary(self.x_range, self.y_range)
-        # self.obs_circle = self.obs_circle()
+        self.x_range = (0, 50)
+        self.y_range = (0, 30)
+        self.obs_boundary = self.obs_boundary()
+        self.obs = self.obs_circle()
 
     @staticmethod
-    def obs_boundary(x, y):
-        w = 1
-        obs_boundary = []
-
-        for i in np.linspace(x[0], x[1], (x[1] - x[0]) // w + 1):
-            obs_boundary.append((i, y[0], w))
-        for i in np.linspace(x[0], x[1], (x[1] - x[0]) // w + 1):
-            obs_boundary.append((i, y[1], w))
-        for j in np.linspace(y[0] + 1, y[1] - w, (y[1] - y[0] - 2 * w) // w + 1):
-            obs_boundary.append((x[0], j, w))
-        for j in np.linspace(y[0] + 1, y[1] - w, (y[1] - y[0] - 2 * w) // w + 1):
-            obs_boundary.append((x[1], j, w))
-
-        for i in np.linspace(10, 20, 10 // w + 1):
-            obs_boundary.append((i, 15, w))
-        for j in np.linspace(1, 14, 13 // w + 1):
-            obs_boundary.append((20, j, w))
-
-        for j in np.linspace(15, 29, 14 // w + 1):
-            obs_boundary.append((30, j, w))
-        for j in np.linspace(1, 14, 13 // w + 1):
-            obs_boundary.append((40, j, w))
+    def obs_boundary():
+        obs_boundary = [
+            (0, 0, 1, 30),
+            (0, 30, 50, 1),
+            (1, 0, 50, 1),
+            (50, 1, 1, 30),
+            (20, 1, 1, 15),
+            (10, 15, 10, 1),
+            (30, 15, 1, 15),
+            (40, 1, 1, 15)
+        ]
 
         return obs_boundary
 
     @staticmethod
     def obs_circle():
         obs_cir = [
-            (8, 8, 3),
-            (10, 23, 3),
-            (20, 25, 2.5),
-            (30, 7, 3),
-            (40, 25, 2),
-            (43, 20, 2.5)
+            (5, 10, 3),
+            (10, 22, 3.5),
+            (21, 23, 3),
+            (34, 9, 4),
+            (37, 23, 3),
+            (45, 20, 2)
         ]
 
         return obs_cir

Sampling-based Planning/node.py

@@ -1,7 +0,0 @@
-class Node:
-    def __init__(self, x, y):
-        self.x = x
-        self.y = y
-        self.path_x = []
-        self.path_y = []
-        self.parent = None

Sampling-based Planning/plotting.py

@@ -8,35 +8,23 @@ class Plotting:
         self.xI, self.xG = xI, xG
         self.env = env.Env()
         self.obs_bound = self.env.obs_boundary
-        self.obs_circle = self.env.obs_circle
+        self.obs_circle = self.env.obs
 
-    def animation(self, nodelist, node_rand=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])
-        if node_rand is not None:
-            plt.plot(node_rand.x, node_rand.y, "^k")
+    def animation(self, nodelist, path):
+        if path is None:
+            print("No path found!")
+            return
 
-        for node in nodelist:
-            if node.parent:
-                plt.plot(node.path_x, node.path_y, "-g")
-
-        plt.plot(self.xI[0], self.xI[1], "b*")
-        plt.plot(self.xG[0], self.xG[1], "g*")
-
-        plt.axis("equal")
-        plt.axis([-5, 55, -5, 35])
-        plt.title("RRT")
-        plt.grid(True)
-        plt.pause(0.01)
+        self.plot_visited(nodelist)
+        self.plot_path(path)
 
     def plot_grid(self, name):
         fig, ax = plt.subplots()
+
         for x in self.obs_bound:
             ax.add_patch(
                 patches.Rectangle(
-                    (x[0], x[1]), x[2], x[2],
+                    (x[0], x[1]), x[2], x[3],
                     edgecolor='black',
                     facecolor='black',
                     fill=True
@@ -47,44 +35,28 @@ class Plotting:
             ax.add_patch(
                 patches.Circle(
                     (x[0], x[1]), x[2],
-                    edgecolor='gray',
+                    edgecolor='black',
                     facecolor='gray',
                     fill=True
                 )
             )
 
-        plt.plot(self.xI[0], self.xI[1], "b*")
-        plt.plot(self.xG[0], self.xG[1], "g*")
-
+        plt.plot(self.xI[0], self.xI[1], "bs", linewidth=3)
+        plt.plot(self.xG[0], self.xG[1], "gs", linewidth=3)
         plt.title(name)
         plt.axis("equal")
-        plt.show()
-
-    def plot_visited(self, visited):
-        visited.remove(self.xI)
-        count = 0
-
-        for x in visited:
-            count += 1
-            plt.plot(x[0], x[1], linewidth='3', color='#808080', marker='o')
-            plt.gcf().canvas.mpl_connect('key_release_event',
-                                         lambda event: [exit(0) if event.key == 'escape' else None])
-
-            if count < len(visited) / 3:
-                length = 15
-            elif count < len(visited) * 2 / 3:
-                length = 30
-            else:
-                length = 45
 
-            if count % length == 0: plt.pause(0.001)
-
-    def plot_path(self, path):
-        path.remove(self.xI)
-        path.remove(self.xG)
-        path_x = [path[i][0] for i in range(len(path))]
-        path_y = [path[i][1] for i in range(len(path))]
+    @staticmethod
+    def plot_visited(nodelist):
+        for node in nodelist:
+            if node.parent:
+                plt.plot(node.path_x, node.path_y, "-g")
+                plt.gcf().canvas.mpl_connect('key_release_event',
+                                             lambda event: [exit(0) if event.key == 'escape' else None])
+                plt.pause(0.001)
 
-        plt.plot(path_x, path_y, linewidth='3', color='r', marker='o')
+    @staticmethod
+    def plot_path(path):
+        plt.plot([x[0] for x in path], [x[1] for x in path], '-r', linewidth=2)
         plt.pause(0.01)
         plt.show()

Search-based Planning/.idea/workspace.xml

@@ -20,9 +20,9 @@
   </component>
   <component name="ChangeListManager">
     <list default="true" id="025aff36-a6aa-4945-ab7e-b2c625055f47" name="Default Changelist" comment="">
-      <change afterPath="$PROJECT_DIR$/../Sampling-based Planning/node.py" afterDir="false" />
       <change beforePath="$PROJECT_DIR$/../Sampling-based Planning/RRT.py" beforeDir="false" afterPath="$PROJECT_DIR$/../Sampling-based Planning/RRT.py" afterDir="false" />
       <change beforePath="$PROJECT_DIR$/../Sampling-based Planning/env.py" beforeDir="false" afterPath="$PROJECT_DIR$/../Sampling-based Planning/env.py" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/../Sampling-based Planning/node.py" beforeDir="false" />
       <change beforePath="$PROJECT_DIR$/../Sampling-based Planning/plotting.py" beforeDir="false" afterPath="$PROJECT_DIR$/../Sampling-based Planning/plotting.py" afterDir="false" />
       <change beforePath="$PROJECT_DIR$/.idea/workspace.xml" beforeDir="false" afterPath="$PROJECT_DIR$/.idea/workspace.xml" afterDir="false" />
     </list>