Merge branch 'master' of github.com:zhm-real/path-planning-algorithms

Search-based Planning/.idea/workspace.xml

@@ -20,11 +20,9 @@
   </component>
   <component name="ChangeListManager">
     <list default="true" id="025aff36-a6aa-4945-ab7e-b2c625055f47" name="Default Changelist" comment="">
+      <change afterPath="$PROJECT_DIR$/Search_2D/LPAstar.py" afterDir="false" />
       <change beforePath="$PROJECT_DIR$/.idea/workspace.xml" beforeDir="false" afterPath="$PROJECT_DIR$/.idea/workspace.xml" afterDir="false" />
-      <change beforePath="$PROJECT_DIR$/Search_2D/ARAstar.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/ARAstar.py" afterDir="false" />
       <change beforePath="$PROJECT_DIR$/Search_2D/astar.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/astar.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$/gif/ARA_star.gif" beforeDir="false" afterPath="$PROJECT_DIR$/gif/ARA_star.gif" afterDir="false" />
     </list>
     <option name="EXCLUDED_CONVERTED_TO_IGNORED" value="true" />
     <option name="SHOW_DIALOG" value="false" />

Search-based Planning/Search_2D/LPAstar.py

@@ -0,0 +1,17 @@
+"""
+LPA_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
+
+class LpaStar:
+    def __init__(self):

Search-based Planning/Search_2D/astar.py

@@ -28,7 +28,8 @@ class Astar:
         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 = []  # closed set & visited
+        self.CLOSED = set()  # closed set & visited
+        self.VISITED = []
         self.PARENT = {self.xI: self.xI}  # relations
 
     def searching(self):
@@ -40,7 +41,8 @@ class Astar:
 
         while not self.OPEN.empty():
             s = self.OPEN.get()
-            self.CLOSED.append(s)
+            self.CLOSED.add(s)
+            self.VISITED.append(s)
 
             if s == self.xG:  # stop condition
                 break
@@ -56,7 +58,7 @@ class Astar:
                         self.PARENT[s_next] = s
                         self.OPEN.put(s_next, self.fvalue(s_next))
 
-        return self.extract_path(self.PARENT), self.CLOSED
+        return self.extract_path(self.PARENT), self.VISITED
 
     def repeated_Searching(self, xI, xG, e):
         path, visited = [], []
@@ -162,16 +164,16 @@ def main():
     x_start = (5, 5)
     x_goal = (45, 25)
 
-    astar = Astar(x_start, x_goal, 1, "manhattan")  # weight e = 1
+    astar = Astar(x_start, x_goal, 1, "euclidean")  # weight e = 1
     plot = plotting.Plotting(x_start, x_goal)  # class Plotting
-    #
-    # fig_name = "A*"
-    # path, visited = astar.searching()
-    # plot.animation(path, visited, fig_name)  # animation generate
-
-    fig_name = "Repeated A*"
-    path, visited = astar.repeated_Searching(x_start, x_goal, 2.5)
-    plot.animation_ara_star(path, visited, fig_name)
+
+    fig_name = "A*"
+    path, visited = astar.searching()
+    plot.animation(path, visited, fig_name)  # animation generate
+
+    # fig_name = "Repeated A*"
+    # path, visited = astar.repeated_Searching(x_start, x_goal, 2.5)
+    # plot.animation_ara_star(path, visited, fig_name)
 
 
 if __name__ == '__main__':