RRT

Sampling-based Planning/RRT.py

@@ -1,49 +1,201 @@
 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
+        self.path_x = []
+        self.path_y = []
+        self.parent = None
+
+
 class RRT:
     def __init__(self, xI, xG):
-        # Plotting = plotting.Plotting(xI, xG)
-        # Plotting.animation([xI, xG], [xI, xG], "zhou")
-        fig, ax = plt.subplots()
-
-        plt.axis([-5, 5, -5, 5])
-
-        ax.plot()
-
-        ax.add_patch(
-            patches.Rectangle(
-                (1, 1),
-                0.5,
-                0.5,
-                edgecolor='black',
-                facecolor='black',
-                fill=True
-            ))
-
-        ax.add_patch(
-            patches.Circle(
-                (3, 3),
-                0.5,
-                edgecolor='black',
-                facecolor='black',
-                fill=True
-            )
-        )
+        self.xI = Node(xI[0], xI[1])
+        self.xG = node.Node(xG[0], xG[1])
 
-        plt.axis("equal")
-        plt.show()
+        self.env = env.Env()
+        # self.plotting = plotting.Plotting(xI, xG)
+
+        self.x_range = self.env.x_range
+        self.y_range = self.env.y_range
+
+        # 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()
 
     def planning(self):
-        return
+        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_range)
+
+            if not self.check_collision(node_new, self.obstacleList):
+                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)
+
+        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]
+
+        while node_now.parent is not None:
+            node_now = node_now.parent
+            path.append((node_now.x, node_now.y))
+
+        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))
+
+    def new_node(self, node_start, node_goal, expand_range):
+        new_node = 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
+
+        new_node.x += expand_range * math.cos(theta)
+        new_node.y += expand_range * math.sin(theta)
+        new_node.path_x.append(new_node.x)
+        new_node.path_y.append(new_node.y)
+
+        new_node.parent = node_start
+
+        return new_node
+
+    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)
+
+    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]
+
+    @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):
+
+        if node_check 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
 
 
 if __name__ == '__main__':
-    x_Start = (5, 5)  # Starting node
-    x_Goal = (49, 5)  # Goal node
+    x_Start = (0, 0)  # Starting node
+    x_Goal = (6, 10)  # Goal node
 
     rrt = RRT(x_Start, x_Goal)

Sampling-based Planning/env.py

@@ -1,30 +1,48 @@
 import numpy as np
 
+
 class Env:
     def __init__(self):
-        self.x_range = (0, 50)  # size of background
-        self.y_range = (0, 30)
-        self.obs = self.obs_map()
-
-    def obs_map(self):
-        """
-        Initialize obstacles' positions
-
-        :return: map of obstacles
-        """
-
-        x = self.x_range
-        y = self.y_range
-        w = 2
+        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()
 
+    @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):
+        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):
+        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], y[1], (y[1]-y[0])//w+1):
-            obs_boundary.append((j, x[0], w))
-        for j in np.linspace(y[0], y[1], (y[1]-y[0])//w+1):
-            obs_boundary.append((j, x[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))
+
+        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)
+        ]
 
+        return obs_cir

Sampling-based Planning/node.py

@@ -0,0 +1,7 @@
+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

@@ -1,4 +1,5 @@
 import matplotlib.pyplot as plt
+import matplotlib.patches as patches
 import env
 
 
@@ -6,22 +7,58 @@ class Plotting:
     def __init__(self, xI, xG):
         self.xI, self.xG = xI, xG
         self.env = env.Env()
-        self.obs = self.env.obs_map()
+        self.obs_bound = self.env.obs_boundary
+        self.obs_circle = self.env.obs_circle
 
-    def animation(self, path, visited, name):
-        self.plot_grid(name)
-        self.plot_visited(visited)
-        self.plot_path(path)
+    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")
+
+        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)
 
     def plot_grid(self, name):
-        obs_x = [self.obs[i][0] for i in range(len(self.obs))]
-        obs_y = [self.obs[i][1] for i in range(len(self.obs))]
+        fig, ax = plt.subplots()
+        for x in self.obs_bound:
+            ax.add_patch(
+                patches.Rectangle(
+                    (x[0], x[1]), x[2], x[2],
+                    edgecolor='black',
+                    facecolor='black',
+                    fill=True
+                )
+            )
+
+        for x in self.obs_circle:
+            ax.add_patch(
+                patches.Circle(
+                    (x[0], x[1]), x[2],
+                    edgecolor='gray',
+                    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")
-        plt.plot(self.xG[0], self.xG[1], "gs")
-        plt.plot(obs_x, obs_y, "sk")
         plt.title(name)
         plt.axis("equal")
+        plt.show()
 
     def plot_visited(self, visited):
         visited.remove(self.xI)
@@ -30,8 +67,8 @@ class Plotting:
         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])
+            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

Search-based Planning/.idea/workspace.xml

@@ -20,12 +20,10 @@
   </component>
   <component name="ChangeListManager">
     <list default="true" id="025aff36-a6aa-4945-ab7e-b2c625055f47" name="Default Changelist" comment="">
-      <change afterPath="$PROJECT_DIR$/../Sampling-based Planning/.idea/.gitignore" afterDir="false" />
-      <change afterPath="$PROJECT_DIR$/../Sampling-based Planning/.idea/misc.xml" afterDir="false" />
-      <change afterPath="$PROJECT_DIR$/../Sampling-based Planning/.idea/vcs.xml" afterDir="false" />
-      <change afterPath="$PROJECT_DIR$/../Sampling-based Planning/RRT.py" afterDir="false" />
-      <change afterPath="$PROJECT_DIR$/../Sampling-based Planning/env.py" afterDir="false" />
-      <change afterPath="$PROJECT_DIR$/../Sampling-based Planning/plotting.py" afterDir="false" />
+      <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/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>
     <option name="SHOW_DIALOG" value="false" />