change menu

2026-08-30 00:50:46 +08:00 · 2020-06-23 19:20:17 -07:00
parent dfee5555c5
commit a308ca7ee4
8 changed files with 0 additions and 0 deletions
@@ -0,0 +1,79 @@
+import matplotlib.pyplot as plt
+import matplotlib.patches as patches
+import env
+
+
+class Plotting:
+    def __init__(self, xI, xG):
+        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_rectangle = self.env.obs_rectangle
+
+    def animation(self, nodelist, path, animation=False):
+        if path is None:
+            print("No path found!")
+            return
+        self.plot_grid("RRT")
+        self.plot_visited(nodelist, animation)
+        self.plot_path(path)
+
+    def plot_grid(self, name):
+        fig, ax = plt.subplots()
+
+        for (ox, oy, w, h) in self.obs_bound:
+            ax.add_patch(
+                patches.Rectangle(
+                    (ox, oy), w, h,
+                    edgecolor='black',
+                    facecolor='black',
+                    fill=True
+                )
+            )
+
+        for (ox, oy, w, h) in self.obs_rectangle:
+            ax.add_patch(
+                patches.Rectangle(
+                    (ox, oy), w, h,
+                    edgecolor='black',
+                    facecolor='gray',
+                    fill=True
+                )
+            )
+
+        for (ox, oy, r) in self.obs_circle:
+            ax.add_patch(
+                patches.Circle(
+                    (ox, oy), r,
+                    edgecolor='black',
+                    facecolor='gray',
+                    fill=True
+                )
+            )
+
+        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")
+
+    @staticmethod
+    def plot_visited(nodelist, animation):
+        if animation:
+            for node in nodelist:
+                if node.parent:
+                    plt.plot([node.parent.x, node.x], [node.parent.y, node.y], "-g")
+                    plt.gcf().canvas.mpl_connect('key_release_event',
+                                                 lambda event: [exit(0) if event.key == 'escape' else None])
+                    plt.pause(0.001)
+        else:
+            for node in nodelist:
+                if node.parent:
+                    plt.plot([node.parent.x, node.x], [node.parent.y, node.y], "-g")
+
+
+    @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()