update

README.md

@@ -50,8 +50,8 @@ Directory Structure
 <div align=right>
 <table>
   <tr>
-    <td><img src="https://github.com/zhm-real/path-planning-algorithms/blob/master/Search_based_Planning/gif/Astar.gif" alt="astar" width="400"/></a></td>
-    <td><img src="https://github.com/zhm-real/path-planning-algorithms/blob/master/Search_based_Planning/gif/Bi-Astar.gif" alt="biastar" width="400"/></a></td>
+    <td><img src="https://github.com/zhm-real/path-planning-algorithms/blob/master/Search_based_Planning/gif/AStar.gif" alt="astar" width="400"/></a></td>
+    <td><img src="https://github.com/zhm-real/path-planning-algorithms/blob/master/Search_based_Planning/gif/Bi-AStar.gif" alt="biastar" width="400"/></a></td>
   </tr>
 </table>
 <table>

Search_based_Planning/Search_2D/Astar.py

@@ -20,15 +20,15 @@ class AStar:
         self.s_goal = s_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.OPEN = []                                              # priority queue / OPEN set
-        self.CLOSED = []                                            # CLOSED set / VISITED order
-        self.PARENT = dict()                                        # recorded parent
-        self.g = dict()                                             # cost to come
+        self.OPEN = []  # priority queue / OPEN set
+        self.CLOSED = []  # CLOSED set / VISITED order
+        self.PARENT = dict()  # recorded parent
+        self.g = dict()  # cost to come
 
     def searching(self):
         """
@@ -46,7 +46,7 @@ class AStar:
             _, s = heapq.heappop(self.OPEN)
             self.CLOSED.append(s)
 
-            if s == self.s_goal:                                    # stop condition
+            if s == self.s_goal:  # stop condition
                 break
 
             for s_n in self.get_neighbor(s):
@@ -55,7 +55,7 @@ class AStar:
                 if s_n not in self.g:
                     self.g[s_n] = math.inf
 
-                if new_cost < self.g[s_n]:                          # conditions for updating Cost
+                if new_cost < self.g[s_n]:  # conditions for updating Cost
                     self.g[s_n] = new_cost
                     self.PARENT[s_n] = s
                     heapq.heappush(self.OPEN, (self.f_value(s_n), s_n))
@@ -108,7 +108,7 @@ class AStar:
                 if s_n not in g:
                     g[s_n] = math.inf
 
-                if new_cost < g[s_n]:               # conditions for updating Cost
+                if new_cost < g[s_n]:  # conditions for updating Cost
                     g[s_n] = new_cost
                     PARENT[s_n] = s
                     heapq.heappush(OPEN, (g[s_n] + e * self.heuristic(s_n), s_n))
@@ -196,8 +196,8 @@ class AStar:
         :return: heuristic function value
         """
 
-        heuristic_type = self.heuristic_type                    # heuristic type
-        goal = self.s_goal                                      # goal node
+        heuristic_type = self.heuristic_type  # heuristic type
+        goal = self.s_goal  # goal node
 
         if heuristic_type == "manhattan":
             return abs(goal[0] - s[0]) + abs(goal[1] - s[1])
@@ -213,7 +213,7 @@ def main():
     plot = plotting.Plotting(s_start, s_goal)
 
     path, visited = astar.searching()
-    plot.animation(path, visited, "A*")                         # animation
+    plot.animation(path, visited, "A*")  # animation
 
     # path, visited = astar.searching_repeated_astar(2.5)               # initial weight e = 2.5
     # plot.animation_ara_star(path, visited, "Repeated A*")

Search_based_Planning/Search_2D/Best_First.py

@@ -22,12 +22,12 @@ class BestFirst:
         self.Env = env.Env()
         self.plotting = plotting.Plotting(self.s_start, self.s_goal)
 
-        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.OPEN = []                                          # OPEN set: visited nodes
-        self.CLOSED = []                                        # CLOSED set / visited order
-        self.PARENT = dict()                                    # recorded parent
+        self.OPEN = []  # OPEN set: visited nodes
+        self.CLOSED = []  # CLOSED set / visited order
+        self.PARENT = dict()  # recorded parent
 
     def searching(self):
         """
@@ -50,7 +50,7 @@ class BestFirst:
                 if self.is_collision(s, s_n):
                     continue
 
-                if s_n not in self.PARENT:                      # node not explored
+                if s_n not in self.PARENT:  # node not explored
                     heapq.heappush(self.OPEN, (self.heuristic(s_n), s_n))
                     self.PARENT[s_n] = s
 

Search_based_Planning/Search_2D/Bidirectional_a_star.py

@@ -20,19 +20,19 @@ class BidirectionalAStar:
         self.s_goal = s_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.OPEN_fore = []                                                 # OPEN set for forward searching
-        self.OPEN_back = []                                                 # OPEN set for backward searching
-        self.CLOSED_fore = []                                               # CLOSED set for forward
-        self.CLOSED_back = []                                               # CLOSED set for backward
-        self.PARENT_fore = dict()                                           # recorded parent for forward
-        self.PARENT_back = dict()                                           # recorded parent for backward
-        self.g_fore = dict()                                                # cost to come for forward
-        self.g_back = dict()                                                # cost to come for backward
+        self.OPEN_fore = []  # OPEN set for forward searching
+        self.OPEN_back = []  # OPEN set for backward searching
+        self.CLOSED_fore = []  # CLOSED set for forward
+        self.CLOSED_back = []  # CLOSED set for backward
+        self.PARENT_fore = dict()  # recorded parent for forward
+        self.PARENT_back = dict()  # recorded parent for backward
+        self.g_fore = dict()  # cost to come for forward
+        self.g_back = dict()  # cost to come for backward
 
     def init(self):
         """
@@ -220,7 +220,7 @@ def main():
 
     bastar = BidirectionalAStar(x_start, x_goal, "euclidean")
     plot = plotting.Plotting(x_start, x_goal)
-    
+
     path, visited_fore, visited_back = bastar.searching()
     plot.animation_bi_astar(path, visited_fore, visited_back, "Bidirectional-A*")  # animation
 

Search_based_Planning/Search_2D/D_star.py

@@ -11,8 +11,7 @@ import matplotlib.pyplot as plt
 sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
                 "/../../Search_based_Planning/")
 
-from Search_2D import plotting
-from Search_2D import env
+from Search_based_Planning.Search_2D import plotting, env
 
 
 class Dstar:

Search_based_Planning/Search_2D/Dijkstra.py

@@ -22,13 +22,13 @@ class Dijkstra:
         self.Env = env.Env()
         self.plotting = plotting.Plotting(self.s_start, self.s_goal)
 
-        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.OPEN = []                                              # priority queue / OPEN set
-        self.CLOSED = []                                            # closed set & visited
-        self.PARENT = dict()                                        # record parent
-        self.g = dict()                                             # Cost to come
+        self.OPEN = []  # priority queue / OPEN set
+        self.CLOSED = []  # closed set & visited
+        self.PARENT = dict()  # record parent
+        self.g = dict()  # Cost to come
 
     def searching(self):
         """
@@ -132,7 +132,7 @@ def main():
     plot = plotting.Plotting(s_start, s_goal)
 
     path, visited = dijkstra.searching()
-    plot.animation(path, visited, "Dijkstra's")                         # animation generate
+    plot.animation(path, visited, "Dijkstra's")  # animation generate
 
 
 if __name__ == '__main__':

Search_based_Planning/Search_2D/LRTAstar.py

@@ -11,46 +11,51 @@ import math
 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
+from Search_based_Planning.Search_2D import queue, plotting, env
 
 
-class LrtAstarN:
+class LrtAStarN:
     def __init__(self, s_start, s_goal, N, heuristic_type):
         self.s_start, self.s_goal = s_start, s_goal
         self.heuristic_type = heuristic_type
 
         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 = N                                      # number of expand nodes each iteration
-        self.visited = []                               # order of visited nodes in planning
-        self.path = []                                  # path of each iteration
-        self.h_table = {}
+        self.N = N  # number of expand nodes each iteration
+        self.visited = []  # order of visited nodes in planning
+        self.path = []  # path of each iteration
+        self.h_table = {}  # h_value table
+
+    def init(self):
+        """
+        initialize the h_value of all nodes in the environment.
+        it is a global table.
+        """
 
         for i in range(self.Env.x_range):
             for j in range(self.Env.y_range):
-                self.h_table[(i, j)] = self.h((i, j))   # initialize h_value
+                self.h_table[(i, j)] = self.h((i, j))
 
     def searching(self):
-        s_start = self.s_start                               # initialize start node
+        self.init()
+        s_start = self.s_start  # 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)  # OPEN, CLOSED sets in each iteration
 
-            if OPEN == "FOUND":                         # reach the goal node
+            if OPEN == "FOUND":  # reach the goal node
                 self.path.append(CLOSED)
                 break
 
-            h_value = self.iteration(CLOSED)            # h_value table of CLOSED nodes
+            h_value = self.iteration(CLOSED)  # h_value table of CLOSED nodes
 
             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)      # x_init -> expected node in OPEN set
+            s_start, path_k = self.extract_path_in_CLOSE(s_start, h_value)  # x_init -> expected node in OPEN set
             self.path.append(path_k)
 
     def extract_path_in_CLOSE(self, s_start, h_value):
@@ -59,23 +64,25 @@ class LrtAstarN:
 
         while True:
             h_list = {}
+
             for s_n in self.get_neighbor(s):
                 if s_n in h_value:
                     h_list[s_n] = h_value[s_n]
                 else:
                     h_list[s_n] = self.h_table[s_n]
-            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
+            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
+            h_value[s] = float("inf")  # initialize h_value of CLOSED nodes
 
         while True:
             h_value_rec = copy.deepcopy(h_value)
@@ -86,25 +93,25 @@ class LrtAstarN:
                         h_list.append(self.cost(s, s_n) + self.h_table[s_n])
                     else:
                         h_list.append(self.cost(s, s_n) + h_value[s_n])
-                h_value[s] = min(h_list)                        # update h_value of current node
+                h_value[s] = min(h_list)  # update h_value of current node
 
-            if h_value == h_value_rec:                          # h_value table converged
+            if h_value == h_value_rec:  # h_value table converged
                 return h_value
 
-    def Astar(self, x_start, N):
-        OPEN = queue.QueuePrior()                               # OPEN set
+    def AStar(self, x_start, N):
+        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
-        PARENT = {x_start: x_start}                             # relations
-        count = 0                                               # counter
+        CLOSED = []  # CLOSED set
+        g_table = {x_start: 0, self.s_goal: float("inf")}  # Cost to come
+        PARENT = {x_start: x_start}  # relations
+        count = 0  # counter
 
         while not OPEN.empty():
             count += 1
             s = OPEN.get()
             CLOSED.append(s)
 
-            if s == self.s_goal:                                                # reach the goal node
+            if s == self.s_goal:  # reach the goal node
                 self.visited.append(CLOSED)
                 return "FOUND", self.extract_path(x_start, PARENT)
 
@@ -113,15 +120,15 @@ class LrtAstarN:
                     new_cost = g_table[s] + self.cost(s, s_n)
                     if s_n not in g_table:
                         g_table[s_n] = float("inf")
-                    if new_cost < g_table[s_n]:                                 # conditions for updating Cost
+                    if new_cost < g_table[s_n]:  # conditions for updating Cost
                         g_table[s_n] = new_cost
                         PARENT[s_n] = s
                         OPEN.put(s_n, g_table[s_n] + self.h_table[s_n])
 
-            if count == N:                                                      # expand needed CLOSED nodes
+            if count == N:  # expand needed CLOSED nodes
                 break
 
-        self.visited.append(CLOSED)                                             # visited nodes in each iteration
+        self.visited.append(CLOSED)  # visited nodes in each iteration
 
         return OPEN, CLOSED
 
@@ -211,7 +218,7 @@ def main():
     s_start = (10, 5)
     s_goal = (45, 25)
 
-    lrta = LrtAstarN(s_start, s_goal, 250, "euclidean")
+    lrta = LrtAStarN(s_start, s_goal, 250, "euclidean")
     plot = plotting.Plotting(s_start, s_goal)
 
     lrta.searching()

Search_based_Planning/Search_2D/RTAAstar.py → Search_based_Planning/Search_2D/RTAAStar.py

@@ -11,38 +11,43 @@ import math
 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
+from Search_based_Planning.Search_2D import queue, plotting, env
 
 
-class RtaAstar:
+class RTAAStar:
     def __init__(self, s_start, s_goal, N, heuristic_type):
         self.s_start, self.s_goal = s_start, s_goal
         self.heuristic_type = heuristic_type
 
         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 = N                                      # number of expand nodes each iteration
-        self.visited = []                               # order of visited nodes in planning
-        self.path = []                                  # path of each iteration
-        self.h_table = {}
+        self.N = N  # number of expand nodes each iteration
+        self.visited = []  # order of visited nodes in planning
+        self.path = []  # path of each iteration
+        self.h_table = {}  # h_value table
+
+    def init(self):
+        """
+        initialize the h_value of all nodes in the environment.
+        it is a global table.
+        """
 
         for i in range(self.Env.x_range):
             for j in range(self.Env.y_range):
-                self.h_table[(i, j)] = self.h((i, j))   # initialize h_value
+                self.h_table[(i, j)] = self.h((i, j))
 
     def searching(self):
-        s_start = self.s_start                               # initialize start node
+        self.init()
+        s_start = self.s_start  # initialize start node
 
         while True:
             OPEN, CLOSED, g_table, PARENT = \
                 self.Astar(s_start, self.N)
 
-            if OPEN == "FOUND":                         # reach the goal node
+            if OPEN == "FOUND":  # reach the goal node
                 self.path.append(CLOSED)
                 break
 
@@ -70,7 +75,7 @@ class RtaAstar:
         h_value = {}
 
         for s in CLOSED:
-            h_value[s] = float("inf")                           # initialize h_value of CLOSED nodes
+            h_value[s] = float("inf")  # initialize h_value of CLOSED nodes
 
         while True:
             h_value_rec = copy.deepcopy(h_value)
@@ -81,25 +86,25 @@ class RtaAstar:
                         h_list.append(self.cost(s, s_n) + self.h_table[s_n])
                     else:
                         h_list.append(self.cost(s, s_n) + h_value[s_n])
-                h_value[s] = min(h_list)                        # update h_value of current node
+                h_value[s] = min(h_list)  # update h_value of current node
 
-            if h_value == h_value_rec:                          # h_value table converged
+            if h_value == h_value_rec:  # h_value table converged
                 return h_value
 
     def Astar(self, x_start, N):
-        OPEN = queue.QueuePrior()                               # OPEN 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
-        PARENT = {x_start: x_start}                             # relations
-        count = 0                                               # counter
+        CLOSED = []  # CLOSED set
+        g_table = {x_start: 0, self.s_goal: float("inf")}  # Cost to come
+        PARENT = {x_start: x_start}  # relations
+        count = 0  # counter
 
         while not OPEN.empty():
             count += 1
             s = OPEN.get()
             CLOSED.append(s)
 
-            if s == self.s_goal:                                                # reach the goal node
+            if s == self.s_goal:  # reach the goal node
                 self.visited.append(CLOSED)
                 return "FOUND", self.extract_path(x_start, PARENT), [], []
 
@@ -108,15 +113,15 @@ class RtaAstar:
                     new_cost = g_table[s] + self.cost(s, s_n)
                     if s_n not in g_table:
                         g_table[s_n] = float("inf")
-                    if new_cost < g_table[s_n]:                           # conditions for updating Cost
+                    if new_cost < g_table[s_n]:  # conditions for updating Cost
                         g_table[s_n] = new_cost
                         PARENT[s_n] = s
                         OPEN.put(s_n, g_table[s_n] + self.h_table[s_n])
 
-            if count == N:                                                   # expand needed CLOSED nodes
+            if count == N:  # expand needed CLOSED nodes
                 break
 
-        self.visited.append(CLOSED)                                         # visited nodes in each iteration
+        self.visited.append(CLOSED)  # visited nodes in each iteration
 
         return OPEN, CLOSED, g_table, PARENT
 
@@ -145,11 +150,11 @@ class RtaAstar:
             for s_n in self.get_neighbor(s):
                 if s_n in h_value:
                     h_list[s_n] = h_value[s_n]
-            s_key = max(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
+            s_key = max(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 == s_end:                            # reach the expected node in OPEN 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):
@@ -176,8 +181,8 @@ class RtaAstar:
         :return: heuristic function value
         """
 
-        heuristic_type = self.heuristic_type                    # heuristic type
-        goal = self.s_goal                                      # goal node
+        heuristic_type = self.heuristic_type  # heuristic type
+        goal = self.s_goal  # goal node
 
         if heuristic_type == "manhattan":
             return abs(goal[0] - s[0]) + abs(goal[1] - s[1])
@@ -220,7 +225,7 @@ def main():
     s_start = (10, 5)
     s_goal = (45, 25)
 
-    rtaa = RtaAstar(s_start, s_goal, 240, "euclidean")
+    rtaa = RTAAStar(s_start, s_goal, 240, "euclidean")
     plot = plotting.Plotting(s_start, s_goal)
 
     rtaa.searching()

Search_based_Planning/Search_2D/bfs.py

@@ -21,12 +21,12 @@ class BFS:
         self.Env = env.Env()
         self.plotting = plotting.Plotting(self.s_start, self.s_goal)
 
-        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.OPEN = deque()                                 # OPEN set: visited nodes
-        self.PARENT = dict()                                # recorded parent
-        self.CLOSED = []                                    # CLOSED set: explored nodes
+        self.OPEN = deque()  # OPEN set: visited nodes
+        self.PARENT = dict()  # recorded parent
+        self.CLOSED = []  # CLOSED set: explored nodes
 
     def searching(self):
         """
@@ -47,7 +47,7 @@ class BFS:
             for s_n in self.get_neighbor(s):
                 if self.is_collision(s, s_n):
                     continue
-                if s_n not in self.PARENT:                  # node not explored
+                if s_n not in self.PARENT:  # node not explored
                     self.OPEN.append(s_n)
                     self.PARENT[s_n] = s
 

Search_based_Planning/Search_2D/dfs.py

@@ -21,12 +21,12 @@ class DFS:
         self.Env = env.Env()
         self.plotting = plotting.Plotting(self.s_start, self.s_goal)
 
-        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.OPEN = deque()                                     # OPEN set: visited nodes
-        self.PARENT = dict()                                    # recorded parent
-        self.CLOSED = []                                        # CLOSED set / visited order
+        self.OPEN = deque()  # OPEN set: visited nodes
+        self.PARENT = dict()  # recorded parent
+        self.CLOSED = []  # CLOSED set / visited order
 
     def searching(self):
         """
@@ -47,7 +47,7 @@ class DFS:
             for s_n in self.get_neighbor(s):
                 if self.is_collision(s, s_n):
                     continue
-                if s_n not in self.PARENT:                      # node not explored
+                if s_n not in self.PARENT:  # node not explored
                     self.OPEN.append(s_n)
                     self.PARENT[s_n] = s