update

Search-based Planning/.idea/workspace.xml

@@ -20,11 +20,21 @@
   </component>
   <component name="ChangeListManager">
     <list default="true" id="025aff36-a6aa-4945-ab7e-b2c625055f47" name="Default Changelist" comment="">
+      <change afterPath="$PROJECT_DIR$/Search_2D/D_star_Lite.py" afterDir="false" />
+      <change afterPath="$PROJECT_DIR$/Search_2D/LPAstar_backup.py" afterDir="false" />
       <change beforePath="$PROJECT_DIR$/.idea/workspace.xml" beforeDir="false" afterPath="$PROJECT_DIR$/.idea/workspace.xml" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/Search_2D/ARAstar.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/ARAstar.py" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/Search_2D/LPAstar.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/LPAstar.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$/gif/LRTA_star.gif" beforeDir="false" afterPath="$PROJECT_DIR$/gif/LRTA_star.gif" afterDir="false" />
-      <change beforePath="$PROJECT_DIR$/gif/RTAA_star.gif" beforeDir="false" afterPath="$PROJECT_DIR$/gif/RTAA_star.gif" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/Search_2D/astar.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/astar.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/dfs.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/dfs.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_2D/queue.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/queue.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" />
@@ -70,7 +80,7 @@
       </list>
     </option>
   </component>
-  <component name="RunManager" selected="Python.RTAAstar">
+  <component name="RunManager" selected="Python.LPAstar">
     <configuration name="Astar3D" type="PythonConfigurationType" factoryName="Python" temporary="true">
       <module name="Search-based Planning" />
       <option name="INTERPRETER_OPTIONS" value="" />
@@ -207,11 +217,11 @@
     </list>
     <recent_temporary>
       <list>
+        <item itemvalue="Python.LPAstar" />
         <item itemvalue="Python.RTAAstar" />
         <item itemvalue="Python.LRTAstar" />
         <item itemvalue="Python.LRT_Astar3D" />
         <item itemvalue="Python.Astar3D" />
-        <item itemvalue="Python.LPAstar" />
       </list>
     </recent_temporary>
   </component>

Search-based Planning/Search_2D/ARAstar.py

@@ -26,7 +26,7 @@ class AraStar:
         self.e = e                                              # initial weight
         self.g = {self.xI: 0, self.xG: float("inf")}            # cost to come
 
-        self.OPEN = queue.QueuePrior()                          # priority queue / OPEN
+        self.OPEN = queue.QueuePrior()                          # priority queue / U
         self.CLOSED = set()                                     # closed set
         self.INCONS = []                                        # incons set
         self.PARENT = {self.xI: self.xI}                        # relations

Search-based Planning/Search_2D/D_star_Lite.py

@@ -0,0 +1,67 @@
+"""
+D_star_Lite 2D
+@author: huiming zhou
+"""
+
+import os
+import sys
+import math
+import matplotlib.pyplot as plt
+
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
+                "/../../Search-based Planning/")
+
+from Search_2D import queue
+from Search_2D import plotting
+from Search_2D import env
+
+
+class DStarLite:
+    def __init__(self, x_start, x_goal, heuristic_type):
+        self.xI, self.xG = x_start, x_goal
+        self.heuristic_type = heuristic_type
+
+        self.Env = env.Env()  # class Env
+
+        self.u_set = self.Env.motions  # feasible input set
+        self.obs = self.Env.obs  # position of obstacles
+
+        self.U = queue.QueuePrior()  # priority queue / U set
+        self.g, self.rhs = {}, {}
+        self.km = 0
+
+        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.rhs[self.xG] = 0
+        self.U.put(self.xG, self.CalculateKey(self.xG))
+
+    def CalculateKey(self, s):
+        return [min(self.g[s], self.rhs[s]) + self.h(self.xI, s) + self.km,
+                min(self.g[s], self.rhs[s])]
+
+    def h(self, s_start, s):
+        heuristic_type = self.heuristic_type  # heuristic type
+
+        if heuristic_type == "manhattan":
+            return abs(s[0] - s_start[0]) + abs(s[1] - s_start[1])
+        else:
+            return math.hypot(s[0] - s_start[0], s[1] - s_start[1])
+
+    def UpdateVertex(self, s):
+        if s != self.xG:
+
+
+    def getNeighbor(self, s):
+        v_list = set()
+        for u in self.u_set:
+            s_next = tuple([s[i] + u[i] for i in range(2)])
+            if s_next not in self.obs:
+                v_list.add(s_next)
+
+        return v_list
+
+    def getCost(self, s_start, s_end):
+

Search-based Planning/Search_2D/LPAstar.py

@@ -5,6 +5,7 @@ LPA_star 2D
 
 import os
 import sys
+import math
 import matplotlib.pyplot as plt
 
 sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
@@ -21,11 +22,14 @@ class LpaStar:
         self.heuristic_type = heuristic_type
 
         self.Env = env.Env()  # class Env
+        self.Plot = plotting.Plotting(x_start, x_goal)
 
         self.u_set = self.Env.motions  # feasible input set
         self.obs = self.Env.obs  # position of obstacles
+        self.x = self.Env.x_range
+        self.y = self.Env.y_range
 
-        self.U = queue.QueuePrior()  # priority queue / OPEN set
+        self.U = queue.QueuePrior()  # priority queue / U set
         self.g, self.rhs = {}, {}
 
         for i in range(self.Env.x_range):
@@ -34,51 +38,102 @@ class LpaStar:
                 self.g[(i, j)] = float("inf")
 
         self.rhs[self.xI] = 0
-        self.U.put(self.xI, self.CalculateKey(self.xI))
+        self.U.put(self.xI, self.Key(self.xI))
 
     def searching(self):
-        self.computePath()
-        path = [self.extract_path()]
-
-        obs_change = set()
-        for j in range(14, 15):
-            self.obs.add((30, j))
-            obs_change.add((30, j))
-        for s in obs_change:
-            self.rhs[s] = float("inf")
-            self.g[s] = float("inf")
-            for x in self.get_neighbor(s):
-                self.UpdateVertex(x)
-        # for x in obs_change:
-        #     self.obs.remove(x)
-        # for x in obs_change:
-        #     self.UpdateVertex(x)
-        print(self.g[(29, 15)])
-        print(self.g[(29, 14)])
-        print(self.g[(29, 13)])
-        print(self.g[(30, 13)])
-        print(self.g[(31, 13)])
-        print(self.g[(32, 13)])
-        print(self.g[(33, 13)])
-        print(self.g[(34, 13)])
-
-        self.computePath()
-        path.append(self.extract_path_test())
-
-        return path, obs_change
-
-    def computePath(self):
-        while self.U.top_key() < self.CalculateKey(self.xG) \
-                or self.rhs[self.xG] != self.g[self.xG]:
+        self.fig = plt.figure()
+        self.Plot.plot_grid("Lifelong Planning A*")
+
+        self.ComputePath()
+        self.plot_path(self.extract_path_test())
+
+        self.fig.canvas.mpl_connect('button_press_event', self.on_press)
+        print("hahha")
+
+        plt.show()
+
+    def on_press(self, event):
+        x, y = event.xdata, event.ydata
+        if x < 0 or x > self.x - 1 or y < 0 or y > self.y - 1:
+            print("Please choose right area!")
+        else:
+            x, y = int(x), int(y)
+            print("Change position: x =", x, ",", "y =", y)
+            if (x, y) not in self.obs:
+                self.obs.add((x, y))
+                plt.plot(x, y, 'sk')
+                self.rhs[(x, y)] = float("inf")
+                self.g[(x, y)] = float("inf")
+                for node in self.getSucc((x, y)):
+                    self.UpdateVertex(node)
+            else:
+                self.obs.remove((x, y))
+                plt.plot(x, y, marker='s', color='white')
+                self.UpdateVertex((x, y))
+            self.ComputePath()
+            self.plot_path(self.extract_path_test())
+            self.fig.canvas.draw_idle()
+
+    @staticmethod
+    def plot_path(path):
+        px = [x[0] for x in path]
+        py = [x[1] for x in path]
+        plt.plot(px, py, marker='o')
+
+    def ComputePath(self):
+        while self.U.top_key() < self.Key(self.xG) or \
+                self.rhs[self.xG] != self.g[self.xG]:
             s = self.U.get()
             if self.g[s] > self.rhs[s]:
                 self.g[s] = self.rhs[s]
+                for x in self.getSucc(s):
+                    self.UpdateVertex(x)
             else:
                 self.g[s] = float("inf")
                 self.UpdateVertex(s)
-            for x in self.get_neighbor(s):
-                self.UpdateVertex(x)
-        # return self.extract_path()
+                for x in self.getSucc(s):
+                    self.UpdateVertex(x)
+
+    def getSucc(self, s):
+        nei_list = set()
+        for u in self.u_set:
+            s_next = tuple([s[i] + u[i] for i in range(2)])
+            if s_next not in self.obs and self.g[s_next] > self.g[s]:
+                nei_list.add(s_next)
+        return nei_list
+
+    def getPred(self, s):
+        nei_list = set()
+        for u in self.u_set:
+            s_next = tuple([s[i] + u[i] for i in range(2)])
+            if s_next not in self.obs and self.g[s_next] < self.g[s]:
+                nei_list.add(s_next)
+        return nei_list
+
+    def UpdateVertex(self, s):
+        if s != self.xI:
+            u_min = float("inf")
+            for x in self.getPred(s):
+                u_min = min(u_min, self.g[x] + self.get_cost(x, s))
+            self.rhs[s] = u_min
+        self.U.remove(s)
+        if self.g[s] != self.rhs[s]:
+            self.U.put(s, self.Key(s))
+
+    def print_g(self):
+        print("he")
+        for k in range(self.Env.y_range):
+            j = self.Env.y_range - k - 1
+            string = ""
+            for i in range(self.Env.x_range):
+                if self.g[(i, j)] == float("inf"):
+                    string += ("00" + ', ')
+                else:
+                    if self.g[(i, j)] // 10 == 0:
+                        string += ("0" + str(self.g[(i, j)]) + ', ')
+                    else:
+                        string += (str(self.g[(i, j)]) + ', ')
+            print(string)
 
     def extract_path(self):
         path = []
@@ -97,11 +152,13 @@ class LpaStar:
         path = []
         s = self.xG
 
-        for k in range(30):
+        for k in range(100):
             g_list = {}
             for x in self.get_neighbor(s):
                 g_list[x] = self.g[x]
             s = min(g_list, key=g_list.get)
+            if s == self.xI:
+                return list(reversed(path))
             path.append(s)
         return list(reversed(path))
 
@@ -114,32 +171,21 @@ class LpaStar:
 
         return nei_list
 
-    def CalculateKey(self, s):
+    def Key(self, s):
         return [min(self.g[s], self.rhs[s]) + self.h(s),
                 min(self.g[s], self.rhs[s])]
 
-    def UpdateVertex(self, u):
-        if u != self.xI:
-            u_min = float("inf")
-            for x in self.get_neighbor(u):
-                u_min = min(u_min, self.g[x] + self.get_cost(u, x))
-            self.rhs[u] = u_min
-        self.U.check_remove(u)
-        if self.g[u] != self.rhs[u]:
-            self.U.put(u, self.CalculateKey(u))
-
     def h(self, s):
         heuristic_type = self.heuristic_type  # heuristic type
         goal = self.xG  # goal node
 
         if heuristic_type == "manhattan":
             return abs(goal[0] - s[0]) + abs(goal[1] - s[1])
-        elif heuristic_type == "euclidean":
-            return ((goal[0] - s[0]) ** 2 + (goal[1] - s[1]) ** 2) ** (1 / 2)
         else:
-            print("Please choose right heuristic type!")
+            return math.hypot(goal[0] - s[0], goal[1] - s[1])
 
-    def get_cost(self, s_start, s_end):
+    @staticmethod
+    def get_cost(s_start, s_end):
         """
         Calculate cost for this motion
 
@@ -149,12 +195,7 @@ class LpaStar:
         :note: cost function could be more complicate!
         """
 
-        if s_start not in self.obs:
-            if s_end not in self.obs:
-                return 1
-            else:
-                return float("inf")
-        return float("inf")
+        return 1
 
 
 def main():
@@ -162,23 +203,7 @@ def main():
     x_goal = (45, 25)
 
     lpastar = LpaStar(x_start, x_goal, "euclidean")
-    plot = plotting.Plotting(x_start, x_goal)
-
-    path, obs = lpastar.searching()
-
-    plot.plot_grid("Lifelong Planning A*")
-    p = path[0]
-    px = [x[0] for x in p]
-    py = [x[1] for x in p]
-    plt.plot(px, py, marker='o')
-    plt.pause(0.5)
-
-    p = path[1]
-    px = [x[0] for x in p]
-    py = [x[1] for x in p]
-    plt.plot(px, py, marker='o')
-    plt.pause(0.01)
-    plt.show()
+    lpastar.searching()
 
 
 if __name__ == '__main__':

Search-based Planning/Search_2D/LPAstar_backup.py

@@ -0,0 +1,209 @@
+"""
+LPA_star 2D
+@author: huiming zhou
+"""
+
+import os
+import sys
+import matplotlib.pyplot as plt
+
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
+                "/../../Search-based Planning/")
+
+from Search_2D import queue
+from Search_2D import plotting
+from Search_2D import env
+
+
+class LpaStar:
+    def __init__(self, x_start, x_goal, heuristic_type):
+        self.xI, self.xG = x_start, x_goal
+        self.heuristic_type = heuristic_type
+
+        self.Env = env.Env()  # class Env
+
+        self.u_set = self.Env.motions  # feasible input set
+        self.obs = self.Env.obs  # position of obstacles
+
+        self.OPEN = queue.QueuePrior()  # priority queue / U set
+        self.g, self.v = {}, {}
+
+        for i in range(self.Env.x_range):
+            for j in range(self.Env.y_range):
+                self.v[(i, j)] = float("inf")
+                self.g[(i, j)] = float("inf")
+
+        self.v[self.xI] = 0
+        self.OPEN.put(self.xI, self.Key(self.xI))
+        self.CLOSED = set()
+
+    def searching(self):
+        self.ComputePath()
+        path = [self.extract_path()]
+        # self.print_g()
+
+        obs_change = set()
+        for i in range(25, 30):
+            self.obs.add((i, 15))
+            obs_change.add((i, 15))
+
+        self.obs.add((30, 14))
+        obs_change.add((30, 14))
+
+        for s in obs_change:
+            self.v[s] = float("inf")
+            self.g[s] = float("inf")
+            for x in self.get_neighbor(s):
+                self.UpdateMembership(x)
+
+        # for x in obs_change:
+        #     self.obs.remove(x)
+        # for x in obs_change:
+        #     self.UpdateVertex(x)
+
+        self.ComputePath()
+        path.append(self.extract_path_test())
+        self.print_g()
+
+        return path, obs_change
+
+    def ComputePath(self):
+        while self.Key(self.xG) > self.OPEN.top_key() \
+                or self.v[self.xG] < self.g[self.xG]:
+            s = self.OPEN.get()
+            if self.v[s] > self.g[s]:
+                self.v[s] = self.g[s]
+                self.CLOSED.add(s)
+
+
+
+        while self.OPEN.top_key() < self.Key(self.xG) \
+                or self.v[self.xG] != self.g[self.xG]:
+            s = self.OPEN.get()
+            if self.g[s] > self.v[s]:
+                self.g[s] = self.v[s]
+            else:
+                self.g[s] = float("inf")
+                self.UpdateMembership(s)
+            for x in self.get_neighbor(s):
+                self.UpdateMembership(x)
+        # return self.extract_path()
+
+    def UpdateMembership(self, s):
+        if self.v[s] != self.g[s]:
+            if s not in self.CLOSED:
+                self.OPEN.put(s, self.Key(s))
+        else:
+            if s in self.OPEN:
+                self.OPEN.remove(s)
+
+    def print_g(self):
+        print("he")
+        for k in range(self.Env.y_range):
+            j = self.Env.y_range - k - 1
+            string = ""
+            for i in range(self.Env.x_range):
+                if self.g[(i, j)] == float("inf"):
+                    string += ("00" + ', ')
+                else:
+                    if self.g[(i, j)] // 10 == 0:
+                        string += ("0" + str(self.g[(i, j)]) + ', ')
+                    else:
+                        string += (str(self.g[(i, j)]) + ', ')
+            print(string)
+
+    def extract_path(self):
+        path = []
+        s = self.xG
+
+        while True:
+            g_list = {}
+            for x in self.get_neighbor(s):
+                g_list[x] = self.g[x]
+            s = min(g_list, key=g_list.get)
+            if s == self.xI:
+                return list(reversed(path))
+            path.append(s)
+
+    def extract_path_test(self):
+        path = []
+        s = self.xG
+
+        for k in range(70):
+            g_list = {}
+            for x in self.get_neighbor(s):
+                g_list[x] = self.g[x]
+            s = min(g_list, key=g_list.get)
+            if s == self.xI:
+                return list(reversed(path))
+            path.append(s)
+        return list(reversed(path))
+
+    def get_neighbor(self, s):
+        nei_list = set()
+        for u in self.u_set:
+            s_next = tuple([s[i] + u[i] for i in range(2)])
+            if s_next not in self.obs:
+                nei_list.add(s_next)
+
+        return nei_list
+
+    def Key(self, s):
+        return [min(self.g[s], self.v[s]) + self.h(s),
+                min(self.g[s], self.v[s])]
+
+    def h(self, s):
+        heuristic_type = self.heuristic_type  # heuristic type
+        goal = self.xG  # goal node
+
+        if heuristic_type == "manhattan":
+            return abs(goal[0] - s[0]) + abs(goal[1] - s[1])
+        elif heuristic_type == "euclidean":
+            return ((goal[0] - s[0]) ** 2 + (goal[1] - s[1]) ** 2) ** (1 / 2)
+        else:
+            print("Please choose right heuristic type!")
+
+    def get_cost(self, s_start, s_end):
+        """
+        Calculate cost for this motion
+
+        :param s_start:
+        :param s_end:
+        :return:  cost for this motion
+        :note: cost function could be more complicate!
+        """
+
+        # if s_start not in self.obs:
+        #     if s_end not in self.obs:
+        #         return 1
+        #     else:
+        #         return float("inf")
+        # return float("inf")
+        return 1
+
+def main():
+    x_start = (5, 5)
+    x_goal = (45, 25)
+
+    lpastar = LpaStar(x_start, x_goal, "manhattan")
+    plot = plotting.Plotting(x_start, x_goal)
+
+    path, obs = lpastar.searching()
+
+    plot.plot_grid("Lifelong Planning A*")
+    p = path[0]
+    px = [x[0] for x in p]
+    py = [x[1] for x in p]
+    plt.plot(px, py, marker='o')
+    plt.pause(0.5)
+
+    p = path[1]
+    px = [x[0] for x in p]
+    py = [x[1] for x in p]
+    plt.plot(px, py, marker='o')
+    plt.pause(0.01)
+    plt.show()
+
+
+if __name__ == '__main__':
+    main()

Search-based Planning/Search_2D/LRTAstar.py

@@ -6,7 +6,6 @@ LRTA_star 2D (Learning Real-time A*)
 import os
 import sys
 import copy
-import matplotlib.pyplot as plt
 
 sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
                 "/../../Search-based Planning/")
@@ -39,7 +38,7 @@ class LrtAstarN:
         s_start = self.xI                               # initialize start node
 
         while True:
-            OPEN, CLOSED = self.Astar(s_start, self.N)  # OPEN, CLOSED sets in each iteration
+            OPEN, CLOSED = self.Astar(s_start, self.N)  # U, CLOSED sets in each iteration
 
             if OPEN == "FOUND":                         # reach the goal node
                 self.path.append(CLOSED)
@@ -50,7 +49,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 OPEN set
+            s_start, path_k = self.extract_path_in_CLOSE(s_start, h_value)      # s_start -> expected node in U set
             self.path.append(path_k)
 
     def extract_path_in_CLOSE(self, s_start, h_value):
@@ -70,7 +69,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 OPEN set
+            if s_key not in h_value:                            # reach the expected node in U set
                 return s_key, path
 
     def iteration(self, CLOSED):
@@ -96,7 +95,7 @@ class LrtAstarN:
                 return h_value
 
     def Astar(self, x_start, N):
-        OPEN = queue.QueuePrior()                               # OPEN set
+        OPEN = queue.QueuePrior()                               # U set
         OPEN.put(x_start, self.h(x_start))
         CLOSED = set()                                          # CLOSED set
         g_table = {x_start: 0, self.xG: float("inf")}           # cost to come
@@ -180,7 +179,7 @@ def main():
     x_start = (10, 5)
     x_goal = (45, 25)
 
-    lrta = LrtAstarN(x_start, x_goal, 150, "euclidean")
+    lrta = LrtAstarN(x_start, x_goal, 200, "euclidean")
     plot = plotting.Plotting(x_start, x_goal)
     fig_name = "Learning Real-time A* (LRTA*)"
 

Search-based Planning/Search_2D/RTAAstar.py

@@ -6,7 +6,6 @@ RTAAstar 2D (Real-time Adaptive A*)
 import os
 import sys
 import copy
-import matplotlib.pyplot as plt
 
 sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
                 "/../../Search-based Planning/")
@@ -80,7 +79,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 OPEN set
+            if s_key == s_end:                            # reach the expected node in U set
                 return s_start, list(reversed(path))
 
     def iteration(self, CLOSED):
@@ -106,7 +105,7 @@ class RtaAstar:
                 return h_value
 
     def Astar(self, x_start, N):
-        OPEN = queue.QueuePrior()                               # OPEN set
+        OPEN = queue.QueuePrior()                               # U set
         OPEN.put(x_start, self.h_table[x_start])
         CLOSED = set()                                          # CLOSED set
         g_table = {x_start: 0, self.xG: float("inf")}           # cost to come
@@ -190,7 +189,7 @@ def main():
     x_start = (10, 5)
     x_goal = (45, 25)
 
-    rtaa = RtaAstar(x_start, x_goal, 150, "euclidean")
+    rtaa = RtaAstar(x_start, x_goal, 200, "euclidean")
     plot = plotting.Plotting(x_start, x_goal)
     fig_name = "Real-time Adaptive A* (RTAA*)"
 

Search-based Planning/Search_2D/astar.py

@@ -26,7 +26,7 @@ class Astar:
         self.obs = self.Env.obs  # position of obstacles
 
         self.g = {self.xI: 0, self.xG: float("inf")}  # cost to come
-        self.OPEN = queue.QueuePrior()  # priority queue / OPEN set
+        self.OPEN = queue.QueuePrior()  # priority queue / U set
         self.OPEN.put(self.xI, self.fvalue(self.xI))
         self.CLOSED = set()  # closed set & visited
         self.VISITED = []

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()                       # OPEN set: visited nodes
+        self.OPEN = queue.QueueFIFO()                       # U set: visited nodes
         self.OPEN.put(self.xI)
         self.CLOSED = []                                    # CLOSED set: explored nodes
         self.PARENT = {self.xI: self.xI}                    # relations

Search-based Planning/Search_2D/bidirectional_a_star.py

@@ -27,9 +27,9 @@ class BidirectionalAstar:
         self.g_fore = {self.xI: 0, self.xG: float("inf")}       # cost to come: from x_start
         self.g_back = {self.xG: 0, self.xI: float("inf")}       # cost to come: form x_goal
 
-        self.OPEN_fore = queue.QueuePrior()                     # OPEN set for foreward searching
+        self.OPEN_fore = queue.QueuePrior()                     # U set for foreward searching
         self.OPEN_fore.put(self.xI, self.g_fore[self.xI] + self.h(self.xI, self.xG))
-        self.OPEN_back = queue.QueuePrior()                     # OPEN set for backward searching
+        self.OPEN_back = queue.QueuePrior()                     # U set for backward searching
         self.OPEN_back.put(self.xG, self.g_back[self.xG] + self.h(self.xG, self.xI))
 
         self.CLOSED_fore = []                                   # CLOSED set for foreward

Search-based Planning/Search_2D/dfs.py

@@ -24,7 +24,7 @@ class DFS:
         self.u_set = self.Env.motions                           # feasible input set
         self.obs = self.Env.obs                                 # position of obstacles
 
-        self.OPEN = queue.QueueLIFO()                           # OPEN set: visited nodes
+        self.OPEN = queue.QueueLIFO()                           # U set: visited nodes
         self.OPEN.put(self.xI)
         self.CLOSED = []                                        # CLOSED set: explored nodes
         self.PARENT = {self.xI: self.xI}                        # relations

Search-based Planning/Search_2D/dijkstra.py

@@ -25,7 +25,7 @@ class Dijkstra:
         self.obs = self.Env.obs                                     # position of obstacles
 
         self.g = {self.xI: 0, self.xG: float("inf")}                # cost to come
-        self.OPEN = queue.QueuePrior()                              # priority queue / OPEN set
+        self.OPEN = queue.QueuePrior()                              # priority queue / U set
         self.OPEN.put(self.xI, 0)
         self.CLOSED = []                                            # closed set & visited
         self.PARENT = {self.xI: self.xI}                            # relations

Search-based Planning/Search_2D/queue.py

@@ -53,13 +53,15 @@ class QueuePrior:
         return len(self.queue) == 0
 
     def put(self, item, priority):
+        flag = 0
         count = 0
         for (p, x) in self.queue:
             if x == item:
                 self.queue[count] = (priority, item)
+                flag = 1
                 break
             count += 1
-        if count == len(self.queue):
+        if flag == 0:
             heapq.heappush(self.queue, (priority, item))  # reorder x using priority
 
     def get(self):
@@ -68,10 +70,10 @@ class QueuePrior:
     def enumerate(self):
         return self.queue
 
-    def check_remove(self, item):
-        for (p, x) in self.queue:
-            if item == x:
-                self.queue.remove((p, x))
+    def remove(self, item):
+        for x in self.queue:
+            if item == x[1]:
+                self.queue.remove(x)
 
     def top_key(self):
         return self.queue[0][0]

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 OPEN set
+                            # add xj in to U set
                             self.OPEN.put(strxj, a + 1 * self.h[strxj])
             # For specified expanded nodes, used primarily in LRTA*
             if N:

Search-based Planning/Search_3D/LRT_Astar3D.py

@@ -56,7 +56,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 OPEN, stop
+        while strst in self.Astar.CLOSED:  # when minchild in CLOSED then continue, when minchild in U, stop
             # strChildren = self.children(st)
             strChildren = [hash3D(i) for i in self.Astar.children(st)]
             minh, minchild = np.inf, None