From 3a87e5d5770f7a88af23b1cf0cf579c63bb5f346 Mon Sep 17 00:00:00 2001
From: zhm-real <zhou.hm0420@gmail.com>
Date: Sat, 27 Jun 2020 00:21:29 -0700
Subject: [PATCH] update ARA*

---
 Search-based Planning/.idea/workspace.xml     |  26 +++++----
 .../__pycache__/plotting.cpython-37.pyc       | Bin 2651 -> 3402 bytes
 Search-based Planning/ara_star.py             |  53 +++++++++---------
 Search-based Planning/plotting.py             |  52 +++++++++++++----
 4 files changed, 82 insertions(+), 49 deletions(-)

diff --git a/Search-based Planning/.idea/workspace.xml b/Search-based Planning/.idea/workspace.xml
index 27cfbb5..4682921 100644
--- a/Search-based Planning/.idea/workspace.xml	
+++ b/Search-based Planning/.idea/workspace.xml	
@@ -19,7 +19,11 @@
     <select />
   </component>
   <component name="ChangeListManager">
-    <list default="true" id="025aff36-a6aa-4945-ab7e-b2c625055f47" name="Default Changelist" comment="" />
+    <list default="true" id="025aff36-a6aa-4945-ab7e-b2c625055f47" name="Default Changelist" comment="">
+      <change beforePath="$PROJECT_DIR$/.idea/workspace.xml" beforeDir="false" afterPath="$PROJECT_DIR$/.idea/workspace.xml" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/ara_star.py" beforeDir="false" afterPath="$PROJECT_DIR$/ara_star.py" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/plotting.py" beforeDir="false" afterPath="$PROJECT_DIR$/plotting.py" afterDir="false" />
+    </list>
     <option name="SHOW_DIALOG" value="false" />
     <option name="HIGHLIGHT_CONFLICTS" value="true" />
     <option name="HIGHLIGHT_NON_ACTIVE_CHANGELIST" value="false" />
@@ -51,7 +55,7 @@
     <property name="restartRequiresConfirmation" value="false" />
     <property name="settings.editor.selected.configurable" value="com.jetbrains.python.configuration.PyActiveSdkModuleConfigurable" />
   </component>
-  <component name="RunManager" selected="Python.a_star">
+  <component name="RunManager" selected="Python.ara_star">
     <configuration name="a_star" type="PythonConfigurationType" factoryName="Python" temporary="true">
       <module name="Search-based Planning" />
       <option name="INTERPRETER_OPTIONS" value="" />
@@ -159,8 +163,8 @@
     </configuration>
     <recent_temporary>
       <list>
-        <item itemvalue="Python.a_star" />
         <item itemvalue="Python.ara_star" />
+        <item itemvalue="Python.a_star" />
         <item itemvalue="Python.bfs" />
         <item itemvalue="Python.dfs" />
         <item itemvalue="Python.dijkstra" />
@@ -198,22 +202,22 @@
     </state>
     <state x="2700" y="297" width="424" height="482" key="FileChooserDialogImpl/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1592802293738" />
     <state x="819" y="314" key="FileChooserDialogImpl/65.24.1855.1056/1920.0.1920.1080@65.24.1855.1056" timestamp="1592933974409" />
-    <state width="1832" height="206" key="GridCell.Tab.0.bottom" timestamp="1593133319558">
+    <state width="1832" height="206" key="GridCell.Tab.0.bottom" timestamp="1593242428508">
       <screen x="1920" y="0" width="1920" height="1080" />
     </state>
-    <state width="1832" height="206" key="GridCell.Tab.0.bottom/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1593133319558" />
-    <state width="1832" height="206" key="GridCell.Tab.0.center" timestamp="1593133319558">
+    <state width="1832" height="206" key="GridCell.Tab.0.bottom/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1593242428508" />
+    <state width="1832" height="206" key="GridCell.Tab.0.center" timestamp="1593242428508">
       <screen x="1920" y="0" width="1920" height="1080" />
     </state>
-    <state width="1832" height="206" key="GridCell.Tab.0.center/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1593133319558" />
-    <state width="1832" height="206" key="GridCell.Tab.0.left" timestamp="1593133319558">
+    <state width="1832" height="206" key="GridCell.Tab.0.center/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1593242428508" />
+    <state width="1832" height="206" key="GridCell.Tab.0.left" timestamp="1593242428508">
       <screen x="1920" y="0" width="1920" height="1080" />
     </state>
-    <state width="1832" height="206" key="GridCell.Tab.0.left/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1593133319558" />
-    <state width="1832" height="206" key="GridCell.Tab.0.right" timestamp="1593133319558">
+    <state width="1832" height="206" key="GridCell.Tab.0.left/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1593242428508" />
+    <state width="1832" height="206" key="GridCell.Tab.0.right" timestamp="1593242428508">
       <screen x="1920" y="0" width="1920" height="1080" />
     </state>
-    <state width="1832" height="206" key="GridCell.Tab.0.right/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1593133319558" />
+    <state width="1832" height="206" key="GridCell.Tab.0.right/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1593242428508" />
     <state x="2406" y="174" key="SettingsEditor" timestamp="1592801555194">
       <screen x="1920" y="0" width="1920" height="1080" />
     </state>
diff --git a/Search-based Planning/__pycache__/plotting.cpython-37.pyc b/Search-based Planning/__pycache__/plotting.cpython-37.pyc
index b091284ac433d337150543a83bdfdcdd52e6ab3c..eb5521404e9237804e0052d2485927a9fc408bd2 100644
GIT binary patch
delta 1729
zcmah}O=x3P6u#%apS+i(Y4g)`+Ay7|)}L2rsxV_6aHKj8RYb##3WH(BkmkOoX<w7f
zeXqa1kVOL)L9jk><s!j>Q5S-^a4ooUHMo-JMwfyJBDiwlIWI};tbB03ckem(_xsMd
zKj(g!NnINoi!=PL{`L2}_s!$ft>d#MynfuAznB$<FuPFFMNTBe7`9>}CC0m~WZ0&d
z5a}*Egi>5&-eT6|*Pj;v7J~0#F6}8jwa<VFh5;yATJQ(VCqkv=c%ka9H+O>!e?Df=
zrV#(;P5ul(COA}f^-1)|{Kx8KehOPzG?ydjOaevD1W&1!CdaWIyrVt_*`T970qNkr
zy3h&Lnyl<s6^zlRqtOO0qqz?de73=S*wkg-haOO`4?tcXc`ppToV)_s%HBslTxWNV
z+N!Sz{v2y7p0c6(s(@*R$fuc5d@fX>eW(HRI!aILvkko$6S{~Aqs#gnmsdQ)heHr%
zfo(GRfrz)Y#_WkVkz>QiJ(0wSD#qHHulQOXkFd$*5)x^O6dmdA(k&MC#-jrgu@l81
zD|ow*!=>O$?K5~XDC#dQO{43#M-giOeDUXxZ>-)}uDzH0>Xly)y31Bv5=Wsf?XBjH
zEmiD9jLJgrtv*|rp`g~PY`eClOQKL_$i=Hp@VowawtyXX9s#9;Bqs~b#injU3JxZ3
zI8D#1H)@M5Y?OEEZr!s*2y&%L+t5e4#L(yr?xzT(#1SK1j@ZMxMBWSJ4fbMlH`Vvq
z8VJ;)Yj}#M9%1SoqC$s2l(`3<wxRo~uaP1o0jbi+oqAELq1V$;B%=?(PqEj|4>AtV
zVS!BJPnrZ&50X;wYH-b{b<T_e7MK4s;RPHz%Gp_(c8TB|f~Cmw)Se@F1mRQ?Ba%d3
zq)4O(=_g5vNSTf#(&Zw;NRsHNPotxxt~r%jNpDrW_43~EzaQOVbff4pDIEJOnwvv7
zm2L)RxC(im;|H0C#iIp>s&xjEk#tcxIoR-753_qvLOtlS+q|u|wY^t8?kSY<B+Z>G
zZ5_v{KKFGh8p5b5SWojI@@a$u`U;j1)lO7?cjqzJ^`0?$LOPaGe7i>HX6%<`%aloM
zgz;+AY09!wcRhK^*BFK*U3QSu#F?BxuwsK;j)<d#mh`%WYSk(4MA!;pb0Gdn285YP
zqrO$~I`w9wT#=Qs>s90mPIGC40f`yNK^{{4pm0LpVA6l*RB1SU^q6UA5sg$t0tCmL
z#+<EbZ8ms1gel_K!#B!@HI9YeNfb-ETX(QpOWN+bUGYjur(Rq4+|B(}XWK5REoXbH
zWV%kpv*{I-XL0G0-jtO_%`PQ`ZMQ~46-foTHAoOHDJA3*L3HbrtINyiyNTwa>PR9_
zQGI7m?s&?55z~ye!_uHsU$GLwC*}g=g73_1?h1OsB+};9t6R3W-W2k3@W7mVkq9)v
t&<x#($7kYcBO!?!OAFITIK3WDeRZ+5PpwcN{0E8>eUjrMQ^EE4)W05YZ&3gM

delta 977
zcmaiy&rcIU6vyYy{_N700;SM`2ny<=5>OLK)c7;e7-@(eFiq5eQ=#QY>+A|zx8Y!8
zxR^-hA80i(F)>Et(Tit~##8me(W8mp4Dr2E@yEs8%y-_t*?IHcXWskNr+D;CB;qi9
zE?xM3J#DT>ryP!7r*L>j)(eB3mar=<V~UV)aJEFt73PLtzv}_mr9Q(zNHBCj+SOE)
z52_RVm|@~x_GXoj0<@`D+R9i5e#<zD3(_V#L_(*^PLy^W9vx~j3FJ(utGa8{wc>)W
z9_m9F<%PZ%V(Ozlyn2&<q{qNP!Or6Fo}mO#WdX}W4S)$K^Sh4&ILEH7m9>Biew3BD
z&+}TK2}m+jo@7E3x-f+K$N=W^s$OGxqiPDQScEF9#(@Rgw*q(o!s%i2Oy)(VrK~UY
zZM(y9)?BKH2(IWNTGj&|=m|_=p34M=wM8qXbUQJ{8mwaJW|f$89S)bQ-Q%P5s!ii5
zGJRs6-q(jqUk;+o{&@cN!^F(wNjFqCWp=T!oRu2-Ws)SNl2%_=H=1>$IDN;<y2cjY
z7`oJDYtO9|8eT67gD~ja0ULVJa(E0(h=a*>9%iM^$%TUN=ayziifG(e&UraME4E4N
zxZT1*2YbsXYwR~^vel+F09^Ik$sx@*@+Jakk`-2itx-j$1i?3|c8#f`eX;+~rf9{W
z|1`xYc1}utvnN&)zZP9C|Fda~GHiAdRb2-uci`-5l90-sB>gC}_v0_mZ<H#?RqiKe
z11c3NyGV#>lQAKb2T(G)H&<A3^)1Advf2oBA8+K|=;bK7=+Zng2zVG$km7N0pw#|1
z;^MSFmv(KH3lGPr{biUWLK0PP!vlvXpbfTxuVZ&es>wAnZHv=BUC}~rW~6wJtc<xe
K%cN?LjQ<2h4Y;lV

diff --git a/Search-based Planning/ara_star.py b/Search-based Planning/ara_star.py
index c5f9683..2b2928e 100644
--- a/Search-based Planning/ara_star.py	
+++ b/Search-based Planning/ara_star.py	
@@ -15,7 +15,7 @@ class AraStar:
         self.u_set = self.Env.motions  # feasible input set
         self.obs = self.Env.obs  # position of obstacles
 
-        self.e = 3
+        self.e = 2.5
         self.g = {self.xI: 0, self.xG: float("inf")}
         self.fig_name = "ARA_Star Algorithm"
 
@@ -24,47 +24,57 @@ class AraStar:
         self.INCONS = []
         self.parent = {self.xI: self.xI}
 
-    def searching(self):
-        path = []
+        self.path = []
+        self.visited = []
 
+    def searching(self):
         self.OPEN.put(self.xI, self.fvalue(self.xI))
         self.ImprovePath()
-
-        path.append(self.extract_path())
+        self.path.append(self.extract_path())
 
         while self.update_e() > 1:
             self.e -= 0.5
+            print(self.e)
             OPEN_mid = [x for (p, x) in self.OPEN.enumerate()] + self.INCONS
             self.OPEN = queue.QueuePrior()
+            self.OPEN.put(self.xI, self.fvalue(self.xI))
 
             for x in OPEN_mid:
                 self.OPEN.put(x, self.fvalue(x))
+
             self.INCONS = []
             self.CLOSED = []
             self.ImprovePath()
+            self.path.append(self.extract_path())
 
-            path.append(self.extract_path())
-
-        return path
+        return self.path, self.visited
 
     def ImprovePath(self):
-        while (not self.OPEN.empty() and self.fvalue(self.xG) >
+        visited_each = []
+        while (self.fvalue(self.xG) >
                min([self.fvalue(x) for (p, x) in self.OPEN.enumerate()])):
             s = self.OPEN.get()
-            self.CLOSED.append(s)
+
+            if s not in self.CLOSED:
+                self.CLOSED.append(s)
 
             for u_next in self.u_set:
                 s_next = tuple([s[i] + u_next[i] for i in range(len(s))])
+
                 if s_next not in self.obs:
                     new_cost = self.g[s] + self.get_cost(s, u_next)
                     if s_next not in self.g or new_cost < self.g[s_next]:
                         self.g[s_next] = new_cost
                         self.parent[s_next] = s
+                        visited_each.append(s_next)
+
                         if s_next not in self.CLOSED:
                             self.OPEN.put(s_next, self.fvalue(s_next))
                         else:
                             self.INCONS.append(s_next)
 
+        self.visited.append(visited_each)
+
     def update_e(self):
         c_OPEN, c_INCONS = float("inf"), float("inf")
 
@@ -76,8 +86,8 @@ class AraStar:
 
         if min(c_OPEN, c_INCONS) == float("inf"):
             return 1
-        else:
-            return min(self.e, self.g[self.xG] / min(c_OPEN, c_INCONS))
+
+        return min(self.e, self.g[self.xG] / min(c_OPEN, c_INCONS))
 
     def fvalue(self, x):
         h = self.e * self.Heuristic(x)
@@ -143,23 +153,10 @@ def main():
     arastar = AraStar(x_start, x_goal, "manhattan")
     plot = plotting.Plotting(x_start, x_goal)
 
-    path = arastar.searching()
+    fig_name = "ARA* algorithm"
+    path, visited = arastar.searching()
 
-    plot.plot_grid("ARA*")
-
-    print(arastar.e)
-
-    for path_i in path:
-        path_i.remove(x_start)
-        path_i.remove(x_goal)
-
-        path_x = [path_i[i][0] for i in range(len(path_i))]
-        path_y = [path_i[i][1] for i in range(len(path_i))]
-
-        plt.plot(path_x, path_y, linewidth='3', marker='o')
-        plt.pause(1)
-
-    plt.show()
+    plot.animation_ara_star(path, visited, fig_name)
 
 
 if __name__ == '__main__':
diff --git a/Search-based Planning/plotting.py b/Search-based Planning/plotting.py
index 625529c..7bfd526 100644
--- a/Search-based Planning/plotting.py	
+++ b/Search-based Planning/plotting.py	
@@ -12,6 +12,7 @@ class Plotting:
         self.plot_grid(name)
         self.plot_visited(visited)
         self.plot_path(path)
+        plt.show()
 
     def plot_grid(self, name):
         obs_x = [self.obs[i][0] for i in range(len(self.obs))]
@@ -23,31 +24,62 @@ class Plotting:
         plt.title(name)
         plt.axis("equal")
 
-    def plot_visited(self, visited):
-        visited.remove(self.xI)
+    def plot_visited(self, visited, cl='gray'):
+        if self.xI in visited:
+            visited.remove(self.xI)
+
+        if self.xG in visited:
+            visited.remove(self.xG)
+
         count = 0
 
         for x in visited:
             count += 1
-            plt.plot(x[0], x[1], linewidth='3', color='#808080', marker='o')
+            plt.plot(x[0], x[1], linewidth='3', color=cl, 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
+                length = 25
             else:
-                length = 45
+                length = 35
 
-            if count % length == 0: plt.pause(0.001)
+            if count % length == 0:
+                plt.pause(0.001)
+        plt.pause(0.01)
+
+    def plot_path(self, path, cl='r', flag=False):
+        if self.xI in path:
+            path.remove(self.xI)
+
+        if self.xG in path:
+            path.remove(self.xG)
 
-    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')
+        if not flag:
+            plt.plot(path_x, path_y, linewidth='3', color='r', marker='o')
+        else:
+            plt.plot(path_x, path_y, linewidth='3', color=cl, marker='o')
+
         plt.pause(0.01)
+
+    def animation_ara_star(self, path, visited, name):
+        self.plot_grid(name)
+        cl_v, cl_p = self.color_list()
+
+        for k in range(len(path)):
+            self.plot_visited(visited[k], cl_v[k])
+            self.plot_path(path[k], cl_p[k], True)
+            plt.pause(0.5)
+
         plt.show()
+
+    @staticmethod
+    def color_list():
+        cl_v = ['silver', 'wheat', 'lightskyblue', 'plum', 'slategray']
+        cl_p = ['gray', 'orange', 'deepskyblue', 'red', 'm']
+        return cl_v, cl_p