update

Sampling-based Planning/.idea/.gitignore

@@ -0,0 +1,3 @@
+# Default ignored files
+/shelf/
+/workspace.xml

Sampling-based Planning/.idea/Sampling-based Planning.iml

@@ -0,0 +1,11 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<module type="PYTHON_MODULE" version="4">
+  <component name="NewModuleRootManager">
+    <content url="file://$MODULE_DIR$" />
+    <orderEntry type="inheritedJdk" />
+    <orderEntry type="sourceFolder" forTests="false" />
+  </component>
+  <component name="TestRunnerService">
+    <option name="PROJECT_TEST_RUNNER" value="pytest" />
+  </component>
+</module>

Sampling-based Planning/.idea/inspectionProfiles/profiles_settings.xml

@@ -0,0 +1,6 @@
+<component name="InspectionProjectProfileManager">
+  <settings>
+    <option name="USE_PROJECT_PROFILE" value="false" />
+    <version value="1.0" />
+  </settings>
+</component>

Sampling-based Planning/.idea/misc.xml

@@ -0,0 +1,4 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="ProjectRootManager" version="2" project-jdk-name="Python 3.7 (Search-based Planning)" project-jdk-type="Python SDK" />
+</project>

Sampling-based Planning/.idea/modules.xml

@@ -0,0 +1,8 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="ProjectModuleManager">
+    <modules>
+      <module fileurl="file://$PROJECT_DIR$/.idea/Sampling-based Planning.iml" filepath="$PROJECT_DIR$/.idea/Sampling-based Planning.iml" />
+    </modules>
+  </component>
+</project>

Sampling-based Planning/.idea/vcs.xml

@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="VcsDirectoryMappings">
+    <mapping directory="$PROJECT_DIR$/.." vcs="Git" />
+  </component>
+</project>

Sampling-based Planning/RRT.py

@@ -0,0 +1,49 @@
+import env
+import plotting
+import matplotlib.pyplot as plt
+import matplotlib.patches as patches
+
+
+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
+            )
+        )
+
+        plt.axis("equal")
+        plt.show()
+
+
+    def planning(self):
+        return
+
+
+if __name__ == '__main__':
+    x_Start = (5, 5)  # Starting node
+    x_Goal = (49, 5)  # Goal node
+
+    rrt = RRT(x_Start, x_Goal)

Sampling-based Planning/env.py

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

Sampling-based Planning/plotting.py

@@ -0,0 +1,53 @@
+import matplotlib.pyplot as plt
+import env
+
+
+class Plotting:
+    def __init__(self, xI, xG):
+        self.xI, self.xG = xI, xG
+        self.env = env.Env()
+        self.obs = self.env.obs_map()
+
+    def animation(self, path, visited, name):
+        self.plot_grid(name)
+        self.plot_visited(visited)
+        self.plot_path(path)
+
+    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))]
+
+        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")
+
+    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))]
+
+        plt.plot(path_x, path_y, linewidth='3', color='r', marker='o')
+        plt.pause(0.01)
+        plt.show()