add LPA*

Search-based Planning/.idea/workspace.xml

@@ -20,9 +20,12 @@
   </component>
   <component name="ChangeListManager">
     <list default="true" id="025aff36-a6aa-4945-ab7e-b2c625055f47" name="Default Changelist" comment="">
+      <change afterPath="$PROJECT_DIR$/Search_2D/test.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_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" />
+      <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/bfs.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/bfs.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$/gif/BFS.gif" beforeDir="false" afterPath="$PROJECT_DIR$/gif/BFS.gif" afterDir="false" />
     </list>
     <option name="EXCLUDED_CONVERTED_TO_IGNORED" value="true" />
     <option name="SHOW_DIALOG" value="false" />
@@ -68,8 +71,8 @@
       </list>
     </option>
   </component>
-  <component name="RunManager" selected="Python.LRT_Astar3D">
-    <configuration name="ARAstar" type="PythonConfigurationType" factoryName="Python" temporary="true">
+  <component name="RunManager" selected="Python.bfs">
+    <configuration name="LPAstar" type="PythonConfigurationType" factoryName="Python" temporary="true">
       <module name="Search-based Planning" />
       <option name="INTERPRETER_OPTIONS" value="" />
       <option name="PARENT_ENVS" value="true" />
@@ -81,7 +84,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/ARAstar.py" />
+      <option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_2D/LPAstar.py" />
       <option name="PARAMETERS" value="" />
       <option name="SHOW_COMMAND_LINE" value="false" />
       <option name="EMULATE_TERMINAL" value="false" />
@@ -90,7 +93,7 @@
       <option name="INPUT_FILE" value="" />
       <method v="2" />
     </configuration>
-    <configuration name="Astar3D" type="PythonConfigurationType" factoryName="Python" temporary="true">
+    <configuration name="LRT_Astar3D" type="PythonConfigurationType" factoryName="Python" temporary="true">
       <module name="Search-based Planning" />
       <option name="INTERPRETER_OPTIONS" value="" />
       <option name="PARENT_ENVS" value="true" />
@@ -102,7 +105,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_3D/Astar3D.py" />
+      <option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_3D/LRT_Astar3D.py" />
       <option name="PARAMETERS" value="" />
       <option name="SHOW_COMMAND_LINE" value="false" />
       <option name="EMULATE_TERMINAL" value="false" />
@@ -111,7 +114,7 @@
       <option name="INPUT_FILE" value="" />
       <method v="2" />
     </configuration>
-    <configuration name="LRT_Astar3D" type="PythonConfigurationType" factoryName="Python" temporary="true">
+    <configuration name="astar" type="PythonConfigurationType" factoryName="Python" temporary="true">
       <module name="Search-based Planning" />
       <option name="INTERPRETER_OPTIONS" value="" />
       <option name="PARENT_ENVS" value="true" />
@@ -119,11 +122,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/LRT_Astar3D.py" />
+      <option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_2D/astar.py" />
       <option name="PARAMETERS" value="" />
       <option name="SHOW_COMMAND_LINE" value="false" />
       <option name="EMULATE_TERMINAL" value="false" />
@@ -132,7 +135,7 @@
       <option name="INPUT_FILE" value="" />
       <method v="2" />
     </configuration>
-    <configuration name="RTAAstar" 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" />
@@ -144,7 +147,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/RTAAstar.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" />
@@ -153,7 +156,7 @@
       <option name="INPUT_FILE" value="" />
       <method v="2" />
     </configuration>
-    <configuration name="astar" type="PythonConfigurationType" factoryName="Python" temporary="true">
+    <configuration name="dijkstra" type="PythonConfigurationType" factoryName="Python" nameIsGenerated="true">
       <module name="Search-based Planning" />
       <option name="INTERPRETER_OPTIONS" value="" />
       <option name="PARENT_ENVS" value="true" />
@@ -165,7 +168,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/astar.py" />
+      <option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_2D/dijkstra.py" />
       <option name="PARAMETERS" value="" />
       <option name="SHOW_COMMAND_LINE" value="false" />
       <option name="EMULATE_TERMINAL" value="false" />
@@ -174,7 +177,7 @@
       <option name="INPUT_FILE" value="" />
       <method v="2" />
     </configuration>
-    <configuration name="dijkstra" type="PythonConfigurationType" factoryName="Python" nameIsGenerated="true">
+    <configuration name="test" type="PythonConfigurationType" factoryName="Python" temporary="true">
       <module name="Search-based Planning" />
       <option name="INTERPRETER_OPTIONS" value="" />
       <option name="PARENT_ENVS" value="true" />
@@ -186,7 +189,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/dijkstra.py" />
+      <option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_2D/test.py" />
       <option name="PARAMETERS" value="" />
       <option name="SHOW_COMMAND_LINE" value="false" />
       <option name="EMULATE_TERMINAL" value="false" />
@@ -197,19 +200,19 @@
     </configuration>
     <list>
       <item itemvalue="Python.dijkstra" />
-      <item itemvalue="Python.RTAAstar" />
-      <item itemvalue="Python.ARAstar" />
       <item itemvalue="Python.astar" />
       <item itemvalue="Python.LRT_Astar3D" />
-      <item itemvalue="Python.Astar3D" />
+      <item itemvalue="Python.test" />
+      <item itemvalue="Python.LPAstar" />
+      <item itemvalue="Python.bfs" />
     </list>
     <recent_temporary>
       <list>
-        <item itemvalue="Python.LRT_Astar3D" />
-        <item itemvalue="Python.Astar3D" />
+        <item itemvalue="Python.bfs" />
+        <item itemvalue="Python.LPAstar" />
+        <item itemvalue="Python.test" />
         <item itemvalue="Python.astar" />
-        <item itemvalue="Python.ARAstar" />
-        <item itemvalue="Python.RTAAstar" />
+        <item itemvalue="Python.LRT_Astar3D" />
       </list>
     </recent_temporary>
   </component>

Search-based Planning/Search_2D/LPAstar.py

@@ -5,6 +5,7 @@ LPA_star 2D
 
 import os
 import sys
+import matplotlib.pyplot as plt
 
 sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
                 "/../../Search-based Planning/")
@@ -13,6 +14,122 @@ from Search_2D import queue
 from Search_2D import plotting
 from Search_2D import env
 
+
 class LpaStar:
-    def __init__(self):
-        return
+    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 / OPEN set
+        self.g, self.rhs = {}, {}
+
+        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.xI] = 0
+        self.U.put(self.xI, [self.h(self.xI), 0])
+
+    def searching(self):
+        self.computePath()
+        path = self.extract_path()
+        return path
+
+    def computePath(self):
+        while self.U.top_key() < self.CalculateKey(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.get_neighbor(s):
+                    self.UpdateVertex(x)
+            else:
+                self.g[s] = float("inf")
+                self.UpdateVertex(s)
+                for x in self.get_neighbor(s):
+                    self.UpdateVertex(x)
+
+    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 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 CalculateKey(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] + 1)
+            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!")
+
+    @staticmethod
+    def get_cost(x, u):
+        """
+        Calculate cost for this motion
+
+        :param x: current node
+        :param u: current input
+        :return:  cost for this motion
+        :note: cost function could be more complicate!
+        """
+
+        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 = lpastar.searching()
+    plot.plot_grid("test")
+    px = [x[0] for x in path]
+    py = [x[1] for x in path]
+    plt.plot(px, py, color='red', marker='o')
+    plt.show()
+
+
+if __name__ == '__main__':
+    main()

Search-based Planning/Search_2D/bfs.py

@@ -68,7 +68,7 @@ class BFS:
 
 def main():
     x_start = (5, 5)  # Starting node
-    x_goal = (49, 25)  # Goal node
+    x_goal = (45, 25)  # Goal node
 
     bfs = BFS(x_start, x_goal)
     plot = plotting.Plotting(x_start, x_goal)

Search-based Planning/Search_2D/queue.py

@@ -67,3 +67,11 @@ 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 top_key(self):
+        return self.queue[0][0]

Search-based Planning/Search_2D/test.py

@@ -0,0 +1,20 @@
+"""
+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
+from Search_2D import plotting
+from Search_2D import env
+
+
+U = queue.QueuePrior()
+U.put((1, 2), [2, 3])
+U.put((2, 3), [1, 5])
+print(U.get())