update 2D

Search-based Planning/.idea/workspace.xml

@@ -21,9 +21,23 @@
   <component name="ChangeListManager">
     <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/ARA_star.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/ARA_star.py" afterDir="false" />
       <change beforePath="$PROJECT_DIR$/Search_2D/IDA_star.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/IDA_star.py" afterDir="false" />
       <change beforePath="$PROJECT_DIR$/Search_2D/LRTA_star.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/LRTA_star.py" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/Search_2D/a_star.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/a_star.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/plotting.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/plotting.py" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/Search_2D/test.py" beforeDir="false" />
+      <change beforePath="$PROJECT_DIR$/gif/ARA_star.gif" beforeDir="false" afterPath="$PROJECT_DIR$/gif/ARA_star.gif" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/gif/Astar.gif" beforeDir="false" afterPath="$PROJECT_DIR$/gif/Astar.gif" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/gif/BFS.gif" beforeDir="false" afterPath="$PROJECT_DIR$/gif/BFS.gif" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/gif/Bi-Astar.gif" beforeDir="false" afterPath="$PROJECT_DIR$/gif/Bi-Astar.gif" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/gif/DFS.gif" beforeDir="false" afterPath="$PROJECT_DIR$/gif/DFS.gif" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/gif/Dijkstra.gif" beforeDir="false" afterPath="$PROJECT_DIR$/gif/Dijkstra.gif" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/gif/LRTA_star.gif" beforeDir="false" afterPath="$PROJECT_DIR$/gif/LRTA_star.gif" afterDir="false" />
     </list>
     <option name="EXCLUDED_CONVERTED_TO_IGNORED" value="true" />
     <option name="SHOW_DIALOG" value="false" />
@@ -69,8 +83,8 @@
       </list>
     </option>
   </component>
-  <component name="RunManager" selected="Python.LRTA_star_N">
-    <configuration name="IDA_star" type="PythonConfigurationType" factoryName="Python" temporary="true">
+  <component name="RunManager" selected="Python.LRTA_star">
+    <configuration name="ARA_star" type="PythonConfigurationType" factoryName="Python" temporary="true">
       <module name="Search-based Planning" />
       <option name="INTERPRETER_OPTIONS" value="" />
       <option name="PARENT_ENVS" value="true" />
@@ -82,7 +96,7 @@
       <option name="IS_MODULE_SDK" value="true" />
       <option name="ADD_CONTENT_ROOTS" value="true" />
       <option name="ADD_SOURCE_ROOTS" value="true" />
-      <option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_2D/IDA_star.py" />
+      <option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_2D/ARA_star.py" />
       <option name="PARAMETERS" value="" />
       <option name="SHOW_COMMAND_LINE" value="false" />
       <option name="EMULATE_TERMINAL" value="false" />
@@ -112,7 +126,7 @@
       <option name="INPUT_FILE" value="" />
       <method v="2" />
     </configuration>
-    <configuration name="LRTA_star_N" type="PythonConfigurationType" factoryName="Python" temporary="true">
+    <configuration name="bfs" type="PythonConfigurationType" factoryName="Python" temporary="true">
       <module name="Search-based Planning" />
       <option name="INTERPRETER_OPTIONS" value="" />
       <option name="PARENT_ENVS" value="true" />
@@ -124,7 +138,7 @@
       <option name="IS_MODULE_SDK" value="true" />
       <option name="ADD_CONTENT_ROOTS" value="true" />
       <option name="ADD_SOURCE_ROOTS" value="true" />
-      <option name="SCRIPT_NAME" value="C:\Users\Huiming Zhou\Desktop\path planning algorithms\Search-based Planning\Search_2D\LRTA_star.py" />
+      <option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_2D/bfs.py" />
       <option name="PARAMETERS" value="" />
       <option name="SHOW_COMMAND_LINE" value="false" />
       <option name="EMULATE_TERMINAL" value="false" />
@@ -133,7 +147,7 @@
       <option name="INPUT_FILE" value="" />
       <method v="2" />
     </configuration>
-    <configuration name="bidirectionalAstar3D" type="PythonConfigurationType" factoryName="Python" temporary="true">
+    <configuration name="bidirectional_a_star" type="PythonConfigurationType" factoryName="Python" temporary="true">
       <module name="Search-based Planning" />
       <option name="INTERPRETER_OPTIONS" value="" />
       <option name="PARENT_ENVS" value="true" />
@@ -141,11 +155,11 @@
         <env name="PYTHONUNBUFFERED" value="1" />
       </envs>
       <option name="SDK_HOME" value="" />
-      <option name="WORKING_DIRECTORY" value="$PROJECT_DIR$/Search_3D" />
+      <option name="WORKING_DIRECTORY" value="$PROJECT_DIR$/Search_2D" />
       <option name="IS_MODULE_SDK" value="true" />
       <option name="ADD_CONTENT_ROOTS" value="true" />
       <option name="ADD_SOURCE_ROOTS" value="true" />
-      <option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_3D/bidirectionalAstar3D.py" />
+      <option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_2D/bidirectional_a_star.py" />
       <option name="PARAMETERS" value="" />
       <option name="SHOW_COMMAND_LINE" value="false" />
       <option name="EMULATE_TERMINAL" value="false" />
@@ -154,7 +168,7 @@
       <option name="INPUT_FILE" value="" />
       <method v="2" />
     </configuration>
-    <configuration name="bidirectional_a_star" type="PythonConfigurationType" factoryName="Python" temporary="true">
+    <configuration name="dfs" type="PythonConfigurationType" factoryName="Python" temporary="true">
       <module name="Search-based Planning" />
       <option name="INTERPRETER_OPTIONS" value="" />
       <option name="PARENT_ENVS" value="true" />
@@ -166,7 +180,7 @@
       <option name="IS_MODULE_SDK" value="true" />
       <option name="ADD_CONTENT_ROOTS" value="true" />
       <option name="ADD_SOURCE_ROOTS" value="true" />
-      <option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_2D/bidirectional_a_star.py" />
+      <option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_2D/dfs.py" />
       <option name="PARAMETERS" value="" />
       <option name="SHOW_COMMAND_LINE" value="false" />
       <option name="EMULATE_TERMINAL" value="false" />
@@ -198,19 +212,19 @@
     </configuration>
     <list>
       <item itemvalue="Python.dijkstra" />
+      <item itemvalue="Python.ARA_star" />
+      <item itemvalue="Python.bfs" />
       <item itemvalue="Python.bidirectional_a_star" />
-      <item itemvalue="Python.bidirectionalAstar3D" />
+      <item itemvalue="Python.dfs" />
       <item itemvalue="Python.LRTA_star" />
-      <item itemvalue="Python.IDA_star" />
-      <item itemvalue="Python.LRTA_star_N" />
     </list>
     <recent_temporary>
       <list>
-        <item itemvalue="Python.LRTA_star_N" />
         <item itemvalue="Python.LRTA_star" />
-        <item itemvalue="Python.IDA_star" />
+        <item itemvalue="Python.dfs" />
         <item itemvalue="Python.bidirectional_a_star" />
-        <item itemvalue="Python.bidirectionalAstar3D" />
+        <item itemvalue="Python.bfs" />
+        <item itemvalue="Python.ARA_star" />
       </list>
     </recent_temporary>
   </component>

Search-based Planning/Search_2D/ARA_star.py

@@ -1,5 +1,5 @@
 """
-ARA_star 2D
+ARA_star 2D (Anytime Repairing A*)
 @author: huiming zhou
 """
 
@@ -15,66 +15,66 @@ from Search_2D import env
 
 
 class AraStar:
-    def __init__(self, x_start, x_goal, heuristic_type):
+    def __init__(self, x_start, x_goal, e, heuristic_type):
         self.xI, self.xG = x_start, x_goal
         self.heuristic_type = heuristic_type
 
-        self.Env = env.Env()  # class Env
+        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_set = self.Env.motions                           # feasible input set
+        self.obs = self.Env.obs                                 # position of obstacles
+        self.e = e                                              # initial weight
+        self.g = {self.xI: 0, self.xG: float("inf")}            # cost to come
 
-        self.e = 2.5
-        self.g = {self.xI: 0, self.xG: float("inf")}
-        self.fig_name = "ARA_Star Algorithm"
-
-        self.OPEN = queue.QueuePrior()  # priority queue / OPEN
-        self.CLOSED = []
-        self.INCONS = []
-        self.parent = {self.xI: self.xI}
-
-        self.path = []
-        self.visited = []
+        self.OPEN = queue.QueuePrior()                          # priority queue / OPEN
+        self.CLOSED = set()                                     # closed set
+        self.INCONS = []                                        # incons set
+        self.PARENT = {self.xI: self.xI}                        # relations
+        self.path = []                                          # planning path
+        self.visited = []                                       # order of visited nodes
 
     def searching(self):
         self.OPEN.put(self.xI, self.fvalue(self.xI))
         self.ImprovePath()
         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
+        while self.update_e() > 1:                              # continue condition
+            self.e -= 0.5                                       # increase weight
+            OPEN_mid = [x for (p, x) in self.OPEN.enumerate()] + self.INCONS        # combine two sets
             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.OPEN.put(x, self.fvalue(x))                # update priority
 
             self.INCONS = []
-            self.CLOSED = []
-            self.ImprovePath()
+            self.CLOSED = set()
+            self.ImprovePath()                                  # improve path
             self.path.append(self.extract_path())
 
         return self.path, self.visited
 
     def ImprovePath(self):
+        """
+        :return: a e'-suboptimal path
+        """
+
         visited_each = []
+
         while (self.fvalue(self.xG) >
                min([self.fvalue(x) for (p, x) in self.OPEN.enumerate()])):
             s = self.OPEN.get()
 
             if s not in self.CLOSED:
-                self.CLOSED.append(s)
+                self.CLOSED.add(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
+                        self.PARENT[s_next] = s
                         visited_each.append(s_next)
 
                         if s_next not in self.CLOSED:
@@ -87,10 +87,10 @@ class AraStar:
     def update_e(self):
         c_OPEN, c_INCONS = float("inf"), float("inf")
 
-        if not self.OPEN.empty():
+        if self.OPEN:
             c_OPEN = min(self.g[x] + self.Heuristic(x) for (p, x) in self.OPEN.enumerate())
 
-        if len(self.INCONS) != 0:
+        if self.INCONS:
             c_INCONS = min(self.g[x] + self.Heuristic(x) for x in self.INCONS)
 
         if min(c_OPEN, c_INCONS) == float("inf"):
@@ -99,14 +99,12 @@ class AraStar:
         return min(self.e, self.g[self.xG] / min(c_OPEN, c_INCONS))
 
     def fvalue(self, x):
-        h = self.e * self.Heuristic(x)
-        return self.g[x] + h
+        return self.g[x] + self.e * self.Heuristic(x)
 
     def extract_path(self):
         """
         Extract the path based on the relationship of nodes.
 
-        :param policy: Action needed for transfer between two nodes
         :return: The planning path
         """
 
@@ -114,7 +112,7 @@ class AraStar:
         x_current = self.xG
 
         while True:
-            x_current = self.parent[x_current]
+            x_current = self.PARENT[x_current]
             path_back.append(x_current)
 
             if x_current == self.xI:
@@ -126,7 +124,6 @@ class AraStar:
     def get_cost(x, u):
         """
         Calculate cost for this motion
-
         :param x: current node
         :param u: input
         :return:  cost for this motion
@@ -139,8 +136,6 @@ class AraStar:
         """
         Calculate heuristic.
         :param state: current node (state)
-        :param goal: goal node (state)
-        :param heuristic_type: choosing different heuristic functions
         :return: heuristic
         """
 
@@ -159,10 +154,10 @@ def main():
     x_start = (5, 5)  # Starting node
     x_goal = (49, 5)  # Goal node
 
-    arastar = AraStar(x_start, x_goal, "manhattan")
+    arastar = AraStar(x_start, x_goal, 2.5, "manhattan")
     plot = plotting.Plotting(x_start, x_goal)
 
-    fig_name = "ARA* algorithm"
+    fig_name = "Anytime Repairing A* (ARA*)"
     path, visited = arastar.searching()
 
     plot.animation_ara_star(path, visited, fig_name)

Search-based Planning/Search_2D/IDA_star.py

@@ -1,5 +1,5 @@
 """
-IDA_Star 2D
+IDA_Star 2D (Iteratively Deepening A*)
 @author: huiming zhou
 """
 
@@ -19,10 +19,10 @@ class IdaStar:
         self.xI, self.xG = x_start, x_goal
         self.heuristic_type = heuristic_type
 
-        self.Env = env.Env()  # class Env
+        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_set = self.Env.motions           # feasible input set
+        self.obs = self.Env.obs                 # position of obstacles
 
         self.visited = []
 
@@ -75,23 +75,21 @@ class IdaStar:
 
 
 def main():
-    x_start = (5, 5)  # Starting node
-    x_goal = (15, 20)  # Goal node
+    x_start = (5, 5)
+    x_goal = (15, 20)
 
     ida_star = IdaStar(x_start, x_goal, "manhattan")
     plot = plotting.Plotting(x_start, x_goal)
 
     path, visited = ida_star.ida_star()
-    print(len(visited))
 
     if path:
-        plot.plot_grid("IDA_star")
+        plot.plot_grid("Iteratively Deepening A*")
         plot.plot_path(visited, 'gray', True)
         plot.plot_path(path)
         plt.show()
     else:
         print("Path not found!")
-    plot.plot_grid("IDA")
 
 
 if __name__ == '__main__':

Search-based Planning/Search_2D/LRTA_star.py

@@ -1,5 +1,5 @@
 """
-LRTA_star_N 2D
+LRTA_star 2D (Learning Real-time A*)
 @author: huiming zhou
 """
 
@@ -17,92 +17,93 @@ from Search_2D import env
 
 
 class LrtAstarN:
-    def __init__(self, x_start, x_goal, heuristic_type):
+    def __init__(self, x_start, x_goal, N, heuristic_type):
         self.xI, self.xG = x_start, x_goal
         self.heuristic_type = heuristic_type
 
-        self.Env = env.Env()  # class Env
+        self.Env = env.Env()
 
-        self.u_set = self.Env.motions  # feasible input set
-        self.obs = self.Env.obs  # position of obstacles
+        self.u_set = self.Env.motions                   # feasible input set
+        self.obs = self.Env.obs                         # position of obstacles
 
-        self.N = 150
-        self.visited = []
+        self.N = N                                      # number of expand nodes each iteration
+        self.visited = []                               # order of visited nodes in planning
+        self.path = []                                  # path of each iteration
 
     def searching(self):
-        s_start = self.xI
+        s_start = self.xI                               # initialize start node
 
-        path = []
-        count = 0
+        while True:
+            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)
+                break
+
+            h_value = self.iteration(CLOSED)            # h_value table of CLOSED nodes
+            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):
+        path = [s_start]
+        s = s_start
 
         while True:
-            # if count == 2:
-            #     return path
-            # count += 1
-
-            h_table = {}
-            OPEN, CLOSED = self.Astar(s_start, self.N)
-
-            if OPEN == "end":
-                path.append(CLOSED)
-                return path
-
-            for x in CLOSED:
-                h_table[x] = 2000
-
-            while True:
-                h_table_rec = copy.deepcopy(h_table)
-                for s in CLOSED:
-                    h_list = []
-                    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:
-                            if s_next not in CLOSED:
-                                h_list.append(self.get_cost(s, s_next) + self.h(s_next))
-                            else:
-                                h_list.append(self.get_cost(s, s_next) + h_table[s_next])
-                    h_table[s] = min(h_list)
-                if h_table == h_table_rec:
-                    break
-
-            path_k = [s_start]
-            x = s_start
-            while True:
-                h_xlist = {}
+            h_list = {}
+            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:
+                    if s_next in h_value:
+                        h_list[s_next] = h_value[s_next]
+                    else:
+                        h_list[s_next] = self.h(s_next)
+            s_key = min(h_list, key=h_list.get)                 # move to the smallest node with min h_value
+            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
+                return s_key, path
+
+    def iteration(self, CLOSED):
+        h_value = {}
+
+        for s in CLOSED:
+            h_value[s] = float("inf")                           # initialize h_value of CLOSED nodes
+
+        while True:
+            h_value_rec = copy.deepcopy(h_value)
+            for s in CLOSED:
+                h_list = []
                 for u in self.u_set:
-                    x_next = tuple([x[i] + u[i] for i in range(2)])
-                    if x_next not in self.obs:
-                        if x_next in CLOSED:
-                            h_xlist[x_next] = h_table[x_next]
+                    s_next = tuple([s[i] + u[i] for i in range(2)])
+                    if s_next not in self.obs:
+                        if s_next not in CLOSED:
+                            h_list.append(self.get_cost(s, s_next) + self.h(s_next))
                         else:
-                            h_xlist[x_next] = self.h(x_next)
-                s_key = min(h_xlist, key=h_xlist.get)
-                path_k.append(s_key)
-                x = s_key
-                if s_key not in CLOSED:
-                    break
-            s_start = path_k[-1]
+                            h_list.append(self.get_cost(s, s_next) + h_value[s_next])
+                h_value[s] = min(h_list)                        # update h_value of current node
 
-            path.append(path_k)
+            if h_value == h_value_rec:                          # h_value table converged
+                return h_value
 
     def Astar(self, x_start, N):
-        OPEN = queue.QueuePrior()
+        OPEN = queue.QueuePrior()                               # OPEN set
         OPEN.put(x_start, self.h(x_start))
-        CLOSED = set()
-        g_table = {x_start: 0, self.xG: float("inf")}
-        parent = {x_start: x_start}
-        count = 0
-        visited = []
+        CLOSED = set()                                          # CLOSED set
+        g_table = {x_start: 0, self.xG: float("inf")}           # cost to come
+        PARENT = {x_start: x_start}                             # relations
+        visited = []                                            # order of visited nodes
+        count = 0                                               # counter
 
         while not OPEN.empty():
             count += 1
             s = OPEN.get()
             CLOSED.add(s)
             visited.append(s)
-            if s == self.xG:
-                path = self.extract_path(x_start, parent)
+
+            if s == self.xG:                                                # reach the goal node
                 self.visited.append(visited)
-                return "end", path
+                return "FOUND", self.extract_path(x_start, PARENT)
 
             for u in self.u_set:
                 s_next = tuple([s[i] + u[i] for i in range(len(s))])
@@ -110,14 +111,15 @@ class LrtAstarN:
                     new_cost = g_table[s] + self.get_cost(s, u)
                     if s_next not in g_table:
                         g_table[s_next] = float("inf")
-                    if new_cost < g_table[s_next]:  # conditions for updating cost
+                    if new_cost < g_table[s_next]:                           # conditions for updating cost
                         g_table[s_next] = new_cost
-                        parent[s_next] = s
+                        PARENT[s_next] = s
                         OPEN.put(s_next, g_table[s_next] + self.h(s_next))
 
-            if count == N:
+            if count == N:                                                   # expand needed CLOSED nodes
                 break
-        self.visited.append(visited)
+
+        self.visited.append(visited)                                         # visited nodes in each iteration
 
         return OPEN, CLOSED
 
@@ -166,29 +168,15 @@ class LrtAstarN:
 
 
 def main():
-    x_start = (10, 5)  # Starting node
-    x_goal = (45, 25)  # Goal node
+    x_start = (10, 5)
+    x_goal = (45, 25)
 
-    lrtastarn = LrtAstarN(x_start, x_goal, "euclidean")
+    lrta = LrtAstarN(x_start, x_goal, 150, "euclidean")
     plot = plotting.Plotting(x_start, x_goal)
+    fig_name = "Learning Real-time A* (LRTA*)"
 
-    path = lrtastarn.searching()
-    plot.plot_grid("LRTA_star_N")
-
-    for k in range(len(path)):
-        plot.plot_visited(lrtastarn.visited[k])
-        plt.pause(0.5)
-        plot.plot_path(path[k])
-        plt.pause(0.5)
-    plt.pause(0.5)
-
-    path_u = []
-    for i in range(len(path)):
-        for j in range(len(path[i])):
-            path_u.append(path[i][j])
-    plot.plot_path(path_u)
-    plt.pause(0.2)
-    plt.show()
+    lrta.searching()
+    plot.animation_lrta(lrta.path, lrta.visited, fig_name)
 
 
 if __name__ == '__main__':

Search-based Planning/Search_2D/a_star.py

@@ -19,48 +19,53 @@ class Astar:
         self.xI, self.xG = x_start, x_goal
         self.heuristic_type = heuristic_type
 
-        self.Env = env.Env()  # class Env
+        self.Env = env.Env()                                # class Env
 
-        self.e = e
-        self.u_set = self.Env.motions  # feasible input set
-        self.obs = self.Env.obs  # position of obstacles
+        self.e = e                                          # weighted A*: e >= 1
+        self.u_set = self.Env.motions                       # feasible input set
+        self.obs = self.Env.obs                             # position of obstacles
 
-        self.g = {self.xI: 0, self.xG: float("inf")}
-        self.OPEN = queue.QueuePrior()  # priority queue / OPEN
+        self.g = {self.xI: 0, self.xG: float("inf")}        # cost to come
+        self.OPEN = queue.QueuePrior()                      # priority queue / OPEN set
         self.OPEN.put(self.xI, self.fvalue(self.xI))
-        self.CLOSED = []
-        self.Parent = {self.xI: self.xI}
+        self.CLOSED = []                                    # closed set & visited
+        self.PARENT = {self.xI: self.xI}                    # relations
 
     def searching(self):
         """
         Searching using A_star.
 
-        :return: planning path, action in each node, visited nodes in the planning process
+        :return: path, order of visited nodes in the planning
         """
 
         while not self.OPEN.empty():
             s = self.OPEN.get()
             self.CLOSED.append(s)
 
-            if s == self.xG:  # stop condition
+            if s == self.xG:                                                # stop condition
                 break
 
-            for u_next in self.u_set:  # explore neighborhoods of current node
-                s_next = tuple([s[i] + u_next[i] for i in range(len(s))])
+            for u in self.u_set:                                       # explore neighborhoods of current node
+                s_next = tuple([s[i] + u[i] for i in range(2)])
                 if s_next not in self.obs and s_next not in self.CLOSED:
-                    new_cost = self.g[s] + self.get_cost(s, u_next)
+                    new_cost = self.g[s] + self.get_cost(s, u)
                     if s_next not in self.g:
                         self.g[s_next] = float("inf")
                     if new_cost < self.g[s_next]:  # conditions for updating cost
                         self.g[s_next] = new_cost
-                        self.Parent[s_next] = s
+                        self.PARENT[s_next] = s
                         self.OPEN.put(s_next, self.fvalue(s_next))
 
         return self.extract_path(), self.CLOSED
 
     def fvalue(self, x):
-        h = self.e * self.Heuristic(x)
-        return self.g[x] + h
+        """
+        f = g + h. (g: cost to come, h: heuristic function)
+        :param x: current state
+        :return: f
+        """
+
+        return self.g[x] + self.e * self.Heuristic(x)
 
     def extract_path(self):
         """
@@ -73,7 +78,7 @@ class Astar:
         x_current = self.xG
 
         while True:
-            x_current = self.Parent[x_current]
+            x_current = self.PARENT[x_current]
             path_back.append(x_current)
 
             if x_current == self.xI:
@@ -87,7 +92,7 @@ class Astar:
         Calculate cost for this motion
 
         :param x: current node
-        :param u: input
+        :param u: current input
         :return:  cost for this motion
         :note: cost function could be more complicate!
         """
@@ -99,13 +104,11 @@ class Astar:
         Calculate heuristic.
 
         :param state: current node (state)
-        :param goal: goal node (state)
-        :param heuristic_type: choosing different heuristic functions
-        :return: heuristic
+        :return: heuristic function value
         """
 
-        heuristic_type = self.heuristic_type
-        goal = self.xG
+        heuristic_type = self.heuristic_type                    # heuristic type
+        goal = self.xG                                          # goal node
 
         if heuristic_type == "manhattan":
             return abs(goal[0] - state[0]) + abs(goal[1] - state[1])
@@ -116,15 +119,15 @@ class Astar:
 
 
 def main():
-    x_start = (5, 5)  # Starting node
-    x_goal = (49, 25)  # Goal node
+    x_start = (5, 5)
+    x_goal = (45, 25)
 
-    astar = Astar(x_start, x_goal, 1, "euclidean")
-    plot = plotting.Plotting(x_start, x_goal)  # class Plotting
+    astar = Astar(x_start, x_goal, 1, "euclidean")              # weight e = 1
+    plot = plotting.Plotting(x_start, x_goal)                   # class Plotting
 
-    fig_name = "A* Algorithm"
+    fig_name = "A*"
     path, visited = astar.searching()
-    plot.animation(path, visited, fig_name)  # animation generate
+    plot.animation(path, visited, fig_name)                     # animation generate
 
 
 if __name__ == '__main__':

Search-based Planning/Search_2D/bfs.py

@@ -1,5 +1,5 @@
 """
-BFS 2D
+BFS 2D (Breadth-first Searching)
 @author: huiming zhou
 """
 
@@ -21,69 +21,62 @@ class BFS:
         self.Env = env.Env()
         self.plotting = plotting.Plotting(self.xI, self.xG)
 
-        self.u_set = self.Env.motions  # feasible input set
-        self.obs = self.Env.obs  # position of obstacles
+        self.u_set = self.Env.motions                       # feasible input set
+        self.obs = self.Env.obs                             # position of obstacles
 
-        [self.path, self.policy, self.visited] = self.searching(self.xI, self.xG)
+        self.OPEN = queue.QueueFIFO()                       # OPEN set: visited nodes
+        self.OPEN.put(self.xI)
+        self.CLOSED = []                                    # CLOSED set: explored nodes
+        self.PARENT = {self.xI: self.xI}                    # relations
 
-        self.fig_name = "Breadth-first Searching"
-        self.plotting.animation(self.path, self.visited, self.fig_name)  # animation generate
-
-    def searching(self, xI, xG):
+    def searching(self):
         """
-        Searching using BFS.
-
-        :return: planning path, action in each node, visited nodes in the planning process
+        :return: path, order of visited nodes in the planning
         """
 
-        q_bfs = queue.QueueFIFO()  # first-in-first-out queue
-        q_bfs.put(xI)
-        parent = {xI: xI}  # record parents of nodes
-        action = {xI: (0, 0)}  # record actions of nodes
-        visited = []
-
-        while not q_bfs.empty():
-            x_current = q_bfs.get()
-            if x_current == xG:
+        while not self.OPEN.empty():
+            s = self.OPEN.get()
+            if s == self.xG:
                 break
-            visited.append(x_current)
-            for u_next in self.u_set:  # explore neighborhoods of current node
-                x_next = tuple([x_current[i] + u_next[i] for i in range(len(x_current))])
-                if x_next not in parent and x_next not in self.obs:  # node not visited and not in obstacles
-                    q_bfs.put(x_next)
-                    parent[x_next], action[x_next] = x_current, u_next
+            self.CLOSED.append(s)
 
-        [path, policy] = self.extract_path(xI, xG, parent, action)  # extract path
+            for u_next in self.u_set:                                       # explore neighborhoods
+                s_next = tuple([s[i] + u_next[i] for i in range(2)])
+                if s_next not in self.PARENT and s_next not in self.obs:    # node not visited and not in obstacles
+                    self.OPEN.put(s_next)
+                    self.PARENT[s_next] = s
 
-        return path, policy, visited
+        return self.extract_path(), self.CLOSED
 
-    @staticmethod
-    def extract_path(xI, xG, parent, policy):
+    def extract_path(self):
         """
         Extract the path based on the relationship of nodes.
-
-        :param xI: Starting node
-        :param xG: Goal node
-        :param parent: Relationship between nodes
-        :param policy: Action needed for transfer between two nodes
         :return: The planning path
         """
 
-        path_back = [xG]
-        acts_back = [policy[xG]]
-        x_current = xG
-        while True:
-            x_current = parent[x_current]
-            path_back.append(x_current)
-            acts_back.append(policy[x_current])
+        path = [self.xG]
+        s = self.xG
 
-            if x_current == xI:
+        while True:
+            s = self.PARENT[s]
+            path.append(s)
+            if s == self.xI:
                 break
 
-        return list(path_back), list(acts_back)
+        return list(path)
+
+
+def main():
+    x_start = (5, 5)  # Starting node
+    x_goal = (49, 25)  # Goal node
+
+    bfs = BFS(x_start, x_goal)
+    plot = plotting.Plotting(x_start, x_goal)
+    fig_name = "Breadth-first Searching (BFS)"
+
+    path, visited = bfs.searching()
+    plot.animation(path, visited, fig_name)  # animation
 
 
 if __name__ == '__main__':
-    x_Start = (5, 5)  # Starting node
-    x_Goal = (49, 25)  # Goal node
-    bfs = BFS(x_Start, x_Goal)
+    main()

Search-based Planning/Search_2D/bidirectional_a_star.py

@@ -19,54 +19,54 @@ class BidirectionalAstar:
         self.xI, self.xG = x_start, x_goal
         self.heuristic_type = heuristic_type
 
-        self.Env = env.Env()  # class Env
+        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_set = self.Env.motions                           # feasible input set
+        self.obs = self.Env.obs                                 # position of obstacles
 
-        self.g_fore = {self.xI: 0, self.xG: float("inf")}
-        self.g_back = {self.xG: 0, self.xI: float("inf")}
+        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()
+        self.OPEN_fore = queue.QueuePrior()                     # OPEN 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()
+        self.OPEN_back = queue.QueuePrior()                     # OPEN set for backward searching
         self.OPEN_back.put(self.xG, self.g_back[self.xG] + self.h(self.xG, self.xI))
 
-        self.CLOSED_fore = []
-        self.CLOSED_back = []
+        self.CLOSED_fore = []                                   # CLOSED set for foreward
+        self.CLOSED_back = []                                   # CLOSED set for backward
 
-        self.Parent_fore = {self.xI: self.xI}
-        self.Parent_back = {self.xG: self.xG}
+        self.PARENT_fore = {self.xI: self.xI}
+        self.PARENT_back = {self.xG: self.xG}
 
     def searching(self):
-        visited_fore, visited_back = [], []
         s_meet = self.xI
 
         while not self.OPEN_fore.empty() and not self.OPEN_back.empty():
-
             # solve foreward-search
             s_fore = self.OPEN_fore.get()
-            if s_fore in self.Parent_back:
+            if s_fore in self.PARENT_back:
                 s_meet = s_fore
                 break
-            visited_fore.append(s_fore)
+            self.CLOSED_fore.append(s_fore)
+
             for u in self.u_set:
-                s_next = tuple([s_fore[i] + u[i] for i in range(len(s_fore))])
+                s_next = tuple([s_fore[i] + u[i] for i in range(2)])
                 if s_next not in self.obs:
                     new_cost = self.g_fore[s_fore] + self.get_cost(s_fore, u)
                     if s_next not in self.g_fore:
                         self.g_fore[s_next] = float("inf")
                     if new_cost < self.g_fore[s_next]:
                         self.g_fore[s_next] = new_cost
-                        self.Parent_fore[s_next] = s_fore
+                        self.PARENT_fore[s_next] = s_fore
                         self.OPEN_fore.put(s_next, new_cost + self.h(s_next, self.xG))
 
             # solve backward-search
             s_back = self.OPEN_back.get()
-            if s_back in self.Parent_fore:
+            if s_back in self.PARENT_fore:
                 s_meet = s_back
                 break
-            visited_back.append(s_back)
+            self.CLOSED_back.append(s_back)
+
             for u in self.u_set:
                 s_next = tuple([s_back[i] + u[i] for i in range(len(s_back))])
                 if s_next not in self.obs:
@@ -75,48 +75,48 @@ class BidirectionalAstar:
                         self.g_back[s_next] = float("inf")
                     if new_cost < self.g_back[s_next]:
                         self.g_back[s_next] = new_cost
-                        self.Parent_back[s_next] = s_back
+                        self.PARENT_back[s_next] = s_back
                         self.OPEN_back.put(s_next, new_cost + self.h(s_next, self.xI))
 
-        return self.extract_path(s_meet), visited_fore, visited_back
+        return self.extract_path(s_meet), self.CLOSED_fore, self.CLOSED_back
 
-    def extract_path(self, s):
-        path_back_fore = [s]
-        s_current = s
+    def extract_path(self, s_meet):
+        # extract path for foreward part
+        path_fore = [s_meet]
+        s = s_meet
 
         while True:
-            s_current = self.Parent_fore[s_current]
-            path_back_fore.append(s_current)
-
-            if s_current == self.xI:
+            s = self.PARENT_fore[s]
+            path_fore.append(s)
+            if s == self.xI:
                 break
 
-        path_back_back = []
-        s_current = s
+        # extract path for backward part
+        path_back = []
+        s = s_meet
 
         while True:
-            s_current = self.Parent_back[s_current]
-            path_back_back.append(s_current)
-
-            if s_current == self.xG:
+            s = self.PARENT_back[s]
+            path_back.append(s)
+            if s == self.xG:
                 break
 
-        return list(reversed(path_back_fore)) + list(path_back_back)
+        return list(reversed(path_fore)) + list(path_back)
 
-    def h(self, state, goal):
+    def h(self, s, goal):
         """
-        Calculate heuristic.
-        :param state: current node (state)
+        Calculate heuristic value.
+        :param s: current node (state)
         :param goal: goal node (state)
-        :return: heuristic
+        :return: heuristic value
         """
 
         heuristic_type = self.heuristic_type
 
         if heuristic_type == "manhattan":
-            return abs(goal[0] - state[0]) + abs(goal[1] - state[1])
+            return abs(goal[0] - s[0]) + abs(goal[1] - s[1])
         elif heuristic_type == "euclidean":
-            return ((goal[0] - state[0]) ** 2 + (goal[1] - state[1]) ** 2) ** (1 / 2)
+            return ((goal[0] - s[0]) ** 2 + (goal[1] - s[1]) ** 2) ** (1 / 2)
         else:
             print("Please choose right heuristic type!")
 
@@ -134,15 +134,15 @@ class BidirectionalAstar:
 
 
 def main():
-    x_start = (5, 5)  # Starting node
-    x_goal = (49, 25)  # Goal node
+    x_start = (5, 5)
+    x_goal = (45, 25)
 
     bastar = BidirectionalAstar(x_start, x_goal, "euclidean")
-    plot = plotting.Plotting(x_start, x_goal)  # class Plotting
-
-    fig_name = "Bidirectional-A* Algorithm"
-    path, v_fore, v_back = bastar.searching()
-    plot.animation_bi_astar(path, v_fore, v_back, fig_name)  # animation generate
+    plot = plotting.Plotting(x_start, x_goal)
+    fig_name = "Bidirectional-A*"
+    
+    path, visited_fore, visited_back = bastar.searching()
+    plot.animation_bi_astar(path, visited_fore, visited_back, fig_name)  # animation
 
 
 if __name__ == '__main__':

Search-based Planning/Search_2D/dfs.py

@@ -21,69 +21,64 @@ class DFS:
         self.Env = env.Env()
         self.plotting = plotting.Plotting(self.xI, self.xG)
 
-        self.u_set = self.Env.motions  # feasible input set
-        self.obs = self.Env.obs  # position of obstacles
+        self.u_set = self.Env.motions                           # feasible input set
+        self.obs = self.Env.obs                                 # position of obstacles
 
-        [self.path, self.policy, self.visited] = self.searching(self.xI, self.xG)
+        self.OPEN = queue.QueueLIFO()                           # OPEN set: visited nodes
+        self.OPEN.put(self.xI)
+        self.CLOSED = []                                        # CLOSED set: explored nodes
+        self.PARENT = {self.xI: self.xI}                        # relations
 
-        self.fig_name = "Depth-first Searching"
-        self.plotting.animation(self.path, self.visited, self.fig_name)  # animation generate
-
-    def searching(self, xI, xG):
+    def searching(self):
         """
         Searching using DFS.
 
         :return: planning path, action in each node, visited nodes in the planning process
         """
 
-        q_dfs = queue.QueueLIFO()  # last-in-first-out queue
-        q_dfs.put(xI)
-        parent = {xI: xI}  # record parents of nodes
-        action = {xI: (0, 0)}  # record actions of nodes
-        visited = []
-
-        while not q_dfs.empty():
-            x_current = q_dfs.get()
-            if x_current == xG:
+        while not self.OPEN.empty():
+            s = self.OPEN.get()
+            if s == self.xG:
                 break
-            visited.append(x_current)
-            for u_next in self.u_set:  # explore neighborhoods of current node
-                x_next = tuple([x_current[i] + u_next[i] for i in range(len(x_current))])
-                if x_next not in parent and x_next not in self.obs:  # node not visited and not in obstacles
-                    q_dfs.put(x_next)
-                    parent[x_next], action[x_next] = x_current, u_next
+            self.CLOSED.append(s)
 
-        [path, policy] = self.extract_path(xI, xG, parent, action)
+            for u in self.u_set:                                            # explore neighborhoods
+                s_next = tuple([s[i] + u[i] for i in range(2)])
+                if s_next not in self.PARENT and s_next not in self.obs:    # node not visited and not in obstacles
+                    self.OPEN.put(s_next)
+                    self.PARENT[s_next] = s
 
-        return path, policy, visited
+        return self.extract_path(), self.CLOSED
 
-    @staticmethod
-    def extract_path(xI, xG, parent, policy):
+    def extract_path(self):
         """
         Extract the path based on the relationship of nodes.
-
-        :param xI: Starting node
-        :param xG: Goal node
-        :param parent: Relationship between nodes
-        :param policy: Action needed for transfer between two nodes
         :return: The planning path
         """
 
-        path_back = [xG]
-        acts_back = [policy[xG]]
-        x_current = xG
-        while True:
-            x_current = parent[x_current]
-            path_back.append(x_current)
-            acts_back.append(policy[x_current])
+        path = [self.xG]
+        s = self.xG
 
-            if x_current == xI:
+        while True:
+            s = self.PARENT[s]
+            path.append(s)
+            if s == self.xI:
                 break
 
-        return list(path_back), list(acts_back)
+        return list(path)
+
+
+def main():
+    x_start = (5, 5)
+    x_goal = (45, 25)
+
+    dfs = DFS(x_start, x_goal)
+    plot = plotting.Plotting(x_start, x_goal)
+    fig_name = "Depth-first Searching (DFS)"
+
+    path, visited = dfs.searching()
+    plot.animation(path, visited, fig_name)  # animation
 
 
 if __name__ == '__main__':
-    x_Start = (5, 5)  # Starting node
-    x_Goal = (49, 25)  # Goal node
-    dfs = DFS(x_Start, x_Goal)
+    main()

Search-based Planning/Search_2D/dijkstra.py

@@ -13,6 +13,7 @@ from Search_2D import queue
 from Search_2D import plotting
 from Search_2D import env
 
+
 class Dijkstra:
     def __init__(self, x_start, x_goal):
         self.xI, self.xG = x_start, x_goal
@@ -20,46 +21,58 @@ class Dijkstra:
         self.Env = env.Env()
         self.plotting = plotting.Plotting(self.xI, self.xG)
 
-        self.u_set = self.Env.motions  # feasible input set
-        self.obs = self.Env.obs  # position of obstacles
-
-        [self.path, self.policy, self.visited] = self.searching(self.xI, self.xG)
+        self.u_set = self.Env.motions                               # feasible input set
+        self.obs = self.Env.obs                                     # position of obstacles
 
-        self.fig_name = "Dijkstra's Algorithm"
-        self.plotting.animation(self.path, self.visited, self.fig_name)  # animation generate
+        self.g = {self.xI: 0, self.xG: float("inf")}                # cost to come
+        self.OPEN = queue.QueuePrior()                              # priority queue / OPEN set
+        self.OPEN.put(self.xI, 0)
+        self.CLOSED = []                                            # closed set & visited
+        self.PARENT = {self.xI: self.xI}                            # relations
 
-    def searching(self, xI, xG):
+    def searching(self):
         """
         Searching using Dijkstra.
+        :return: path, order of visited nodes in the planning
+        """
 
-        :return: planning path, action in each node, visited nodes in the planning process
+        while not self.OPEN.empty():
+            s = self.OPEN.get()
+            if s == self.xG:                                        # stop condition
+                break
+            self.CLOSED.append(s)
+
+            for u in self.u_set:                                    # explore neighborhoods
+                s_next = tuple([s[i] + u[i] for i in range(2)])
+                if s_next not in self.obs:                          # node not visited and not in obstacles
+                    new_cost = self.g[s] + self.get_cost(s, u)
+                    if s_next not in self.g:
+                        self.g[s_next] = float("inf")
+                    if new_cost < self.g[s_next]:
+                        self.g[s_next] = new_cost
+                        self.OPEN.put(s_next, new_cost)
+                        self.PARENT[s_next] = s
+
+        return self.extract_path(), self.CLOSED
+
+    def extract_path(self):
         """
+        Extract the path based on the relationship of nodes.
 
-        q_dijk = queue.QueuePrior()  # priority queue
-        q_dijk.put(xI, 0)
-        parent = {xI: xI}  # record parents of nodes
-        action = {xI: (0, 0)}  # record actions of nodes
-        visited = []  # record visited nodes
-        cost = {xI: 0}
+        :return: The planning path
+        """
 
-        while not q_dijk.empty():
-            x_current = q_dijk.get()
-            if x_current == xG:  # stop condition
-                break
-            visited.append(x_current)
-            for u_next in self.u_set:  # explore neighborhoods of current node
-                x_next = tuple([x_current[i] + u_next[i] for i in range(len(x_current))])
-                if x_next not in self.obs:  # node not visited and not in obstacles
-                    new_cost = cost[x_current] + self.get_cost(x_current, u_next)
-                    if x_next not in cost or new_cost < cost[x_next]:
-                        cost[x_next] = new_cost
-                        priority = new_cost
-                        q_dijk.put(x_next, priority)  # put node into queue using cost to come as priority
-                        parent[x_next], action[x_next] = x_current, u_next
+        path_back = [self.xG]
+        x_current = self.xG
 
-        [path, policy] = self.extract_path(xI, xG, parent, action)
+        while True:
+            x_current = self.PARENT[x_current]
+            path_back.append(x_current)
 
-        return path, policy, visited
+            if x_current == self.xI:
+                break
+
+        return list(path_back)
 
     @staticmethod
     def get_cost(x, u):
@@ -74,31 +87,18 @@ class Dijkstra:
 
         return 1
 
-    @staticmethod
-    def extract_path(xI, xG, parent, policy):
-        """
-        Extract the path based on the relationship of nodes.
 
-        :param xI: Starting node
-        :param xG: Goal node
-        :param parent: Relationship between nodes
-        :param policy: Action needed for transfer between two nodes
-        :return: The planning path
-        """
+def main():
+    x_start = (5, 5)
+    x_goal = (45, 25)
 
-        path_back = [xG]
-        acts_back = [policy[xG]]
-        x_current = xG
-        while True:
-            x_current = parent[x_current]
-            path_back.append(x_current)
-            acts_back.append(policy[x_current])
-            if x_current == xI: break
+    dijkstra = Dijkstra(x_start, x_goal)
+    plot = plotting.Plotting(x_start, x_goal)  # class Plotting
 
-        return list(path_back), list(acts_back)
+    fig_name = "Dijkstra's"
+    path, visited = dijkstra.searching()
+    plot.animation(path, visited, fig_name)  # animation generate
 
 
 if __name__ == '__main__':
-    x_Start = (5, 5)  # Starting node
-    x_Goal = (49, 25)  # Goal node
-    dijkstra = Dijkstra(x_Start, x_Goal)
+    main()

Search-based Planning/Search_2D/plotting.py

@@ -25,6 +25,38 @@ class Plotting:
         self.plot_path(path)
         plt.show()
 
+    def animation_lrta(self, path, visited, name):
+        self.plot_grid(name)
+        cl = self.color_list_2()
+        path_combine = []
+
+        for k in range(len(path)):
+            self.plot_visited(visited[k], cl[k])
+            plt.pause(0.2)
+            self.plot_path(path[k])
+            path_combine += path[k]
+            plt.pause(0.2)
+
+        self.plot_path(path_combine)
+        plt.show()
+
+    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()
+
+    def animation_bi_astar(self, path, v_fore, v_back, name):
+        self.plot_grid(name)
+        self.plot_visited_bi(v_fore, v_back)
+        self.plot_path(path)
+        plt.show()
+
     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))]
@@ -78,23 +110,6 @@ class Plotting:
 
         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()
-
-    def animation_bi_astar(self, path, v_fore, v_back, name):
-        self.plot_grid(name)
-        self.plot_visited_bi(v_fore, v_back)
-        self.plot_path(path)
-        plt.show()
-
     def plot_visited_bi(self, v_fore, v_back):
         if self.xI in v_fore:
             v_fore.remove(self.xI)
@@ -119,6 +134,29 @@ class Plotting:
 
     @staticmethod
     def color_list():
-        cl_v = ['silver', 'wheat', 'lightskyblue', 'plum', 'slategray']
-        cl_p = ['gray', 'orange', 'deepskyblue', 'red', 'm']
+        cl_v = ['silver',
+                'wheat',
+                'lightskyblue',
+                'plum',
+                'slategray']
+        cl_p = ['gray',
+                'orange',
+                'deepskyblue',
+                'red',
+                'm']
         return cl_v, cl_p
+
+    @staticmethod
+    def color_list_2():
+        cl = ['silver',
+              'steelblue',
+              'dimgray',
+              'cornflowerblue',
+              'dodgerblue',
+              'royalblue',
+              'plum',
+              'mediumslateblue',
+              'mediumpurple',
+              'blueviolet',
+              ]
+        return cl

Search-based Planning/Search_2D/test.py

@@ -1,20 +0,0 @@
-"""
-A_star 2D
-@author: huiming zhou
-"""
-
-import os
-import sys
-
-sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
-                "/../../Search-based Planning/")
-
-from Search_2D import queue
-
-q = queue.QueuePrior()
-q.put((1, 2), 3)
-print(q.enumerate())
-q.put((1, 2), 2)
-print(q.enumerate())
-q.put((1, 2), 4)
-print(q.enumerate())