From 42dda61e9f76d305bcbd0923e704d44574d1e2bb Mon Sep 17 00:00:00 2001
From: zhm-real <zhou.hm0420@gmail.com>
Date: Sun, 5 Jul 2020 15:10:51 -0700
Subject: [PATCH] update

---
 Search-based Planning/.idea/workspace.xml     | 17 +++-
 .../Search_2D/D_star_Lite.py                  | 24 +++--
 .../Search_2D/Field_D_star.py                 | 99 ++++++++++---------
 Search-based Planning/Search_2D/LRTAstar.py   |  8 +-
 Search-based Planning/Search_2D/RTAAstar.py   |  4 +-
 Search-based Planning/Search_2D/bfs.py        |  2 +-
 .../Search_2D/bidirectional_a_star.py         |  4 +-
 Search-based Planning/Search_2D/dijkstra.py   |  2 +-
 Search-based Planning/Search_3D/Astar3D.py    |  2 +-
 .../Search_3D/LRT_Astar3D.py                  |  2 +-
 10 files changed, 95 insertions(+), 69 deletions(-)

diff --git a/Search-based Planning/.idea/workspace.xml b/Search-based Planning/.idea/workspace.xml
index c01080e..90711fc 100644
--- a/Search-based Planning/.idea/workspace.xml	
+++ b/Search-based Planning/.idea/workspace.xml	
@@ -19,7 +19,18 @@
     <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$/Search_2D/D_star_Lite.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/D_star_Lite.py" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/Search_2D/Field_D_star.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/Field_D_star.py" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/Search_2D/LRTAstar.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/LRTAstar.py" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/Search_2D/RTAAstar.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/RTAAstar.py" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/Search_2D/bfs.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/bfs.py" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/Search_2D/bidirectional_a_star.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/bidirectional_a_star.py" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/Search_2D/dijkstra.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/dijkstra.py" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/Search_3D/Astar3D.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_3D/Astar3D.py" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/Search_3D/LRT_Astar3D.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_3D/LRT_Astar3D.py" afterDir="false" />
+    </list>
     <option name="EXCLUDED_CONVERTED_TO_IGNORED" value="true" />
     <option name="SHOW_DIALOG" value="false" />
     <option name="HIGHLIGHT_CONFLICTS" value="true" />
@@ -48,7 +59,7 @@
     <property name="ASKED_ADD_EXTERNAL_FILES" value="true" />
     <property name="RunOnceActivity.OpenProjectViewOnStart" value="true" />
     <property name="RunOnceActivity.ShowReadmeOnStart" value="true" />
-    <property name="last_opened_file_path" value="$PROJECT_DIR$" />
+    <property name="last_opened_file_path" value="$PROJECT_DIR$/../../PythonRobotics-master/PathPlanning" />
     <property name="restartRequiresConfirmation" value="false" />
     <property name="run.code.analysis.last.selected.profile" value="aDefault" />
     <property name="settings.editor.selected.configurable" value="com.jetbrains.python.configuration.PyActiveSdkModuleConfigurable" />
@@ -209,8 +220,8 @@
       <list>
         <item itemvalue="Python.Field_D_star" />
         <item itemvalue="Python.D_star_Lite" />
-        <item itemvalue="Python.D_star" />
         <item itemvalue="Python.LPAstar" />
+        <item itemvalue="Python.D_star" />
         <item itemvalue="Python.ARAstar" />
       </list>
     </recent_temporary>
diff --git a/Search-based Planning/Search_2D/D_star_Lite.py b/Search-based Planning/Search_2D/D_star_Lite.py
index 8b21a11..175920a 100644
--- a/Search-based Planning/Search_2D/D_star_Lite.py	
+++ b/Search-based Planning/Search_2D/D_star_Lite.py	
@@ -31,8 +31,8 @@ class DStar:
         self.g, self.rhs, self.U = {}, {}, {}
         self.km = 0
 
-        for i in range(self.Env.x_range):
-            for j in range(self.Env.y_range):
+        for i in range(1, self.Env.x_range - 1):
+            for j in range(1, self.Env.y_range - 1):
                 self.rhs[(i, j)] = float("inf")
                 self.g[(i, j)] = float("inf")
 
@@ -60,7 +60,7 @@ class DStar:
             s_curr = self.s_start
             s_last = self.s_start
             i = 0
-            path = []
+            path = [self.s_start]
 
             while s_curr != self.s_goal:
                 s_list = {}
@@ -188,19 +188,25 @@ class DStar:
         return nei_list
 
     def extract_path(self):
-        path = []
+        """
+        Extract the path based on the PARENT set.
+        :return: The planning path
+        """
+
+        path = [self.s_start]
         s = self.s_start
-        count = 0
-        while True:
-            count += 1
+
+        for k in range(100):
             g_list = {}
             for x in self.get_neighbor(s):
                 if not self.is_collision(s, x):
                     g_list[x] = self.g[x]
             s = min(g_list, key=g_list.get)
-            if s == self.s_goal or count > 100:
-                return list(reversed(path))
             path.append(s)
+            if s == self.s_goal:
+                break
+
+        return list(path)
 
     def plot_path(self, path):
         px = [x[0] for x in path]
diff --git a/Search-based Planning/Search_2D/Field_D_star.py b/Search-based Planning/Search_2D/Field_D_star.py
index e5193fa..f4fd563 100644
--- a/Search-based Planning/Search_2D/Field_D_star.py	
+++ b/Search-based Planning/Search_2D/Field_D_star.py	
@@ -28,21 +28,36 @@ class FieldDStar:
         self.x = self.Env.x_range
         self.y = self.Env.y_range
 
-        self.g, self.rhs, self.U = {}, {}, {}
+        self.g, self.rhs, self.OPEN = {}, {}, {}
         self.parent = {}
+        self.cknbr = {}
+        self.ccknbr = {}
+        self.bptr = {}
+        self.init_table()
 
         for i in range(self.Env.x_range):
             for j in range(self.Env.y_range):
                 self.rhs[(i, j)] = float("inf")
                 self.g[(i, j)] = float("inf")
-                self.parent[(i, j)] = (0, 0)
+                self.bptr[(i, j)] = (0, 0)
 
         self.rhs[self.s_goal] = 0.0
-        self.U[self.s_goal] = self.CalculateKey(self.s_goal)
+        self.OPEN[self.s_goal] = self.CalculateKey(self.s_goal)
         self.visited = set()
         self.count = 0
         self.fig = plt.figure()
 
+    def init_table(self):
+        for i in range(1, self.Env.x_range - 1):
+            for j in range(1, self.Env.y_range - 1):
+                s_neighbor = self.get_neighbor_pure((i, j))
+                s_neighbor.append(s_neighbor[0])
+                for k in range(8):
+                    self.cknbr[((i, j), s_neighbor[k])] = s_neighbor[k + 1]
+                s_neighbor = list(reversed(s_neighbor))
+                for k in range(8):
+                    self.ccknbr[((i, j), s_neighbor[k])] = s_neighbor[k + 1]
+
     def run(self):
         self.Plot.plot_grid("Field D*")
         self.ComputeShortestPath()
@@ -62,13 +77,19 @@ class FieldDStar:
             if (x, y) not in self.obs:
                 self.obs.add((x, y))
                 plt.plot(x, y, 'sk')
+                sn_list = self.get_neighbor((x, y))
             else:
                 self.obs.remove((x, y))
                 plt.plot(x, y, marker='s', color='white')
-                self.UpdateVertex((x, y))
+                sn_list = [(x, y)]
+                sn_list += self.get_neighbor((x, y))
 
-            for s_n in self.get_neighbor((x, y)):
-                self.UpdateVertex(s_n)
+            for s in sn_list:
+                v_list = []
+                for sn in self.get_neighbor(s):
+                    v_list.append(self.ComputeCost(s, sn, self.ccknbr[(s, sn)]))
+                self.rhs[s] = min(v_list)
+                self.UpdateVertex(s)
 
             self.ComputeShortestPath()
             self.plot_visited(self.visited)
@@ -82,40 +103,35 @@ class FieldDStar:
                     self.rhs[self.s_start] == self.g[self.s_start]:
                 break
 
-            k_old = v
-            self.U.pop(s)
-            self.visited.add(s)
-
-            if k_old < self.CalculateKey(s):
-                self.U[s] = self.CalculateKey(s)
-            elif self.g[s] > self.rhs[s]:
+            if self.g[s] > self.rhs[s]:
                 self.g[s] = self.rhs[s]
-                for x in self.get_neighbor(s):
-                    self.UpdateVertex(x)
+                self.OPEN.pop(s)
+                for sn in self.get_neighbor(s):
+                    if self.rhs[sn] > self.ComputeCost(sn, s, self.ccknbr[(sn, s)]):
+                        self.rhs[sn] = self.ComputeCost(sn, s, self.ccknbr[(sn, s)])
+                        self.bptr[sn] = s
+                    if self.rhs[sn] > self.ComputeCost(sn, s, self.cknbr[(sn, s)]):
+                        self.rhs[sn] = self.ComputeCost(sn, self.cknbr[(sn, s)], s)
+                        self.bptr[sn] = self.cknbr[(sn, s)]
+                    self.UpdateVertex(sn)
             else:
                 self.g[s] = float("inf")
+                for sn in self.get_neighbor(s):
+                    if self.bptr[sn] == s or self.bptr[sn] == self.cknbr[(sn, s)]:
+                        v_list = []
+                        ssn_list = self.get_neighbor(sn)
+                        for ssn in ssn_list:
+                            v_list.append(self.ComputeCost(sn, ssn, self.ccknbr[(sn, ssn)]))
+                        self.rhs[sn] = min(v_list)
+                        self.bptr[sn] = ssn_list[v_list.index(min(v_list))]
+                        self.UpdateVertex(sn)
                 self.UpdateVertex(s)
-                for x in self.get_neighbor(s):
-                    self.UpdateVertex(x)
 
     def UpdateVertex(self, s):
-        if s != self.s_goal:
-            value = []
-            s_plist = []
-            sn_list = self.get_neighbor_pure(s)
-            sn_list.append(sn_list[0])
-            for k in range(8):
-                v, sp = self.ComputeCost(s, sn_list[k], sn_list[k + 1])
-                value.append(v)
-                s_plist.append(sp)
-            self.rhs[s] = min(value)
-            self.parent[s] = s_plist[value.index(min(value))]
-
-        if s in self.U:
-            self.U.pop(s)
-
         if self.g[s] != self.rhs[s]:
-            self.U[s] = self.CalculateKey(s)
+            self.OPEN[s] = self.CalculateKey(s)
+        elif s in self.OPEN:
+            self.OPEN.pop(s)
 
     def get_neighbor_pure(self, s):
         s_list = []
@@ -138,7 +154,6 @@ class FieldDStar:
 
         c = self.cost(s, s2)
         b = self.cost(s, s1)
-        y = 0
 
         if min(c, b) == float("inf"):
             vs = float("inf")
@@ -149,6 +164,7 @@ class FieldDStar:
             if f <= b:
                 if c <= f:
                     vs = math.sqrt(2) * c + self.g[s2]
+                    print("test loop!")
                 else:
                     y = min(f / (math.sqrt(c ** 2 - f ** 2)), 1)
                     vs = c * math.sqrt(1 + y ** 2) + f * (1 - y) + self.g[s2]
@@ -159,17 +175,15 @@ class FieldDStar:
                     x = 1 - min(b / (math.sqrt(c ** 2 - b ** 2)), 1)
                     vs = c * math.sqrt(1 + (1 - x) ** 2) + b * x + self.g[s2]
 
-        ss = (y * s1[0] + (1 - y) * s2[0], y * s1[1] + (1 - y) * s2[1])
-
-        return vs, ss
+        return vs
 
     def TopKey(self):
         """
         :return: return the min key and its value.
         """
 
-        s = min(self.U, key=self.U.get)
-        return s, self.U[s]
+        s = min(self.OPEN, key=self.OPEN.get)
+        return s, self.OPEN[s]
 
     def h(self, s_start, s_goal):
         heuristic_type = self.heuristic_type  # heuristic type
@@ -225,12 +239,7 @@ class FieldDStar:
         count = 0
         while True:
             count += 1
-            g_list = {}
-            for x in self.get_neighbor(s):
-                if not self.is_collision(s, x):
-                    g_list[x] = self.g[x]
-            ss = self.parent[s]
-            s = min(g_list, key=g_list.get)
+            s = self.bptr[s]
             path.append(s)
 
             if s == self.s_goal or count > 100:
diff --git a/Search-based Planning/Search_2D/LRTAstar.py b/Search-based Planning/Search_2D/LRTAstar.py
index 36b6ef0..7363f0e 100644
--- a/Search-based Planning/Search_2D/LRTAstar.py	
+++ b/Search-based Planning/Search_2D/LRTAstar.py	
@@ -39,7 +39,7 @@ class LrtAstarN:
         s_start = self.s_start                               # initialize start node
 
         while True:
-            OPEN, CLOSED = self.Astar(s_start, self.N)  # U, CLOSED sets in each iteration
+            OPEN, CLOSED = self.Astar(s_start, self.N)  # OPEN, CLOSED sets in each iteration
 
             if OPEN == "FOUND":                         # reach the goal node
                 self.path.append(CLOSED)
@@ -50,7 +50,7 @@ class LrtAstarN:
             for x in h_value:
                 self.h_table[x] = h_value[x]
 
-            s_start, path_k = self.extract_path_in_CLOSE(s_start, h_value)      # s_start -> expected node in U set
+            s_start, path_k = self.extract_path_in_CLOSE(s_start, h_value)      # s_start -> expected node in OPEN set
             self.path.append(path_k)
 
     def extract_path_in_CLOSE(self, s_start, h_value):
@@ -68,7 +68,7 @@ class LrtAstarN:
             path.append(s_key)                                  # generate path
             s = s_key                                           # use end of this iteration as the start of next
 
-            if s_key not in h_value:                            # reach the expected node in U set
+            if s_key not in h_value:                            # reach the expected node in OPEN set
                 return s_key, path
 
     def iteration(self, CLOSED):
@@ -92,7 +92,7 @@ class LrtAstarN:
                 return h_value
 
     def Astar(self, x_start, N):
-        OPEN = queue.QueuePrior()                               # U set
+        OPEN = queue.QueuePrior()                               # OPEN set
         OPEN.put(x_start, self.h(x_start))
         CLOSED = []                                             # CLOSED set
         g_table = {x_start: 0, self.s_goal: float("inf")}           # cost to come
diff --git a/Search-based Planning/Search_2D/RTAAstar.py b/Search-based Planning/Search_2D/RTAAstar.py
index 1f4797b..3575eac 100644
--- a/Search-based Planning/Search_2D/RTAAstar.py	
+++ b/Search-based Planning/Search_2D/RTAAstar.py	
@@ -87,7 +87,7 @@ class RtaAstar:
                 return h_value
 
     def Astar(self, x_start, N):
-        OPEN = queue.QueuePrior()                               # U set
+        OPEN = queue.QueuePrior()                               # OPEN set
         OPEN.put(x_start, self.h_table[x_start])
         CLOSED = []                                             # CLOSED set
         g_table = {x_start: 0, self.s_goal: float("inf")}       # cost to come
@@ -149,7 +149,7 @@ class RtaAstar:
             path.append(s_key)                                  # generate path
             s = s_key                                           # use end of this iteration as the start of next
 
-            if s_key == s_end:                            # reach the expected node in U set
+            if s_key == s_end:                            # reach the expected node in OPEN set
                 return s_start, list(reversed(path))
 
     def extract_path(self, x_start, parent):
diff --git a/Search-based Planning/Search_2D/bfs.py b/Search-based Planning/Search_2D/bfs.py
index 0dd7e2e..efadfbb 100644
--- a/Search-based Planning/Search_2D/bfs.py	
+++ b/Search-based Planning/Search_2D/bfs.py	
@@ -24,7 +24,7 @@ class BFS:
         self.u_set = self.Env.motions                       # feasible input set
         self.obs = self.Env.obs                             # position of obstacles
 
-        self.OPEN = queue.QueueFIFO()                       # U set: visited nodes
+        self.OPEN = queue.QueueFIFO()                       # OPEN set: visited nodes
         self.OPEN.put(self.s_start)
         self.CLOSED = []                                    # CLOSED set: explored nodes
         self.PARENT = {self.s_start: self.s_start}
diff --git a/Search-based Planning/Search_2D/bidirectional_a_star.py b/Search-based Planning/Search_2D/bidirectional_a_star.py
index 30a0437..4f01dfd 100644
--- a/Search-based Planning/Search_2D/bidirectional_a_star.py	
+++ b/Search-based Planning/Search_2D/bidirectional_a_star.py	
@@ -28,10 +28,10 @@ class BidirectionalAstar:
         self.g_fore = {self.s_start: 0, self.s_goal: float("inf")}          # cost to come: from s_start
         self.g_back = {self.s_goal: 0, self.s_start: float("inf")}          # cost to come: form s_goal
 
-        self.OPEN_fore = queue.QueuePrior()                                 # U set for foreward searching
+        self.OPEN_fore = queue.QueuePrior()                                 # OPEN set for foreward searching
         self.OPEN_fore.put(self.s_start,
                            self.g_fore[self.s_start] + self.h(self.s_start, self.s_goal))
-        self.OPEN_back = queue.QueuePrior()                                 # U set for backward searching
+        self.OPEN_back = queue.QueuePrior()                                 # OPEN set for backward searching
         self.OPEN_back.put(self.s_goal,
                            self.g_back[self.s_goal] + self.h(self.s_goal, self.s_start))
 
diff --git a/Search-based Planning/Search_2D/dijkstra.py b/Search-based Planning/Search_2D/dijkstra.py
index 98a2e3c..1d98611 100644
--- a/Search-based Planning/Search_2D/dijkstra.py	
+++ b/Search-based Planning/Search_2D/dijkstra.py	
@@ -26,7 +26,7 @@ class Dijkstra:
         self.obs = self.Env.obs                                     # position of obstacles
 
         self.g = {self.s_start: 0, self.s_goal: float("inf")}       # cost to come
-        self.OPEN = queue.QueuePrior()                              # priority queue / U set
+        self.OPEN = queue.QueuePrior()                              # priority queue / OPEN set
         self.OPEN.put(self.s_start, 0)
         self.CLOSED = []                                            # closed set & visited
         self.PARENT = {self.s_start: self.s_start}
diff --git a/Search-based Planning/Search_3D/Astar3D.py b/Search-based Planning/Search_3D/Astar3D.py
index a969387..a79a8ea 100644
--- a/Search-based Planning/Search_3D/Astar3D.py	
+++ b/Search-based Planning/Search_3D/Astar3D.py	
@@ -75,7 +75,7 @@ class Weighted_A_star(object):
                             # update priority of xj
                             self.OPEN.put(strxj, a + 1 * self.h[strxj])
                         else:
-                            # add xj in to U set
+                            # add xj in to OPEN set
                             self.OPEN.put(strxj, a + 1 * self.h[strxj])
             # For specified expanded nodes, used primarily in LRTA*
             if N:
diff --git a/Search-based Planning/Search_3D/LRT_Astar3D.py b/Search-based Planning/Search_3D/LRT_Astar3D.py
index e03c91b..d2dd224 100644
--- a/Search-based Planning/Search_3D/LRT_Astar3D.py	
+++ b/Search-based Planning/Search_3D/LRT_Astar3D.py	
@@ -48,7 +48,7 @@ class LRT_A_star2:
         st = self.Astar.start
         ind = 0
         # find the lowest path down hill
-        while strst in self.Astar.CLOSED:  # when minchild in CLOSED then continue, when minchild in U, stop
+        while strst in self.Astar.CLOSED:  # when minchild in CLOSED then continue, when minchild in OPEN, stop
             # strChildren = self.children(st)
             strChildren = [hash3D(i) for i in self.Astar.children(st)]
             minh, minchild = np.inf, None