update

Search-based Planning/.idea/Search-based Planning.iml

@@ -2,7 +2,7 @@
 <module type="PYTHON_MODULE" version="4">
   <component name="NewModuleRootManager">
     <content url="file://$MODULE_DIR$" />
-    <orderEntry type="jdk" jdkName="Python 3.7" jdkType="Python SDK" />
+    <orderEntry type="jdk" jdkName="Python 3.7 (Search-based Planning)" jdkType="Python SDK" />
     <orderEntry type="sourceFolder" forTests="false" />
   </component>
 </module>

Search-based Planning/.idea/misc.xml

@@ -1,4 +1,4 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <project version="4">
-  <component name="ProjectRootManager" version="2" project-jdk-name="Python 3.7" project-jdk-type="Python SDK" />
+  <component name="ProjectRootManager" version="2" project-jdk-name="Python 3.7 (Search-based Planning)" project-jdk-type="Python SDK" />
 </project>

Search-based Planning/.idea/workspace.xml

@@ -20,11 +20,15 @@
   </component>
   <component name="ChangeListManager">
     <list default="true" id="025aff36-a6aa-4945-ab7e-b2c625055f47" name="Default Changelist" comment="">
+      <change afterPath="$PROJECT_DIR$/Search_2D/D_star.py" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/.idea/Search-based Planning.iml" beforeDir="false" afterPath="$PROJECT_DIR$/.idea/Search-based Planning.iml" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/.idea/misc.xml" beforeDir="false" afterPath="$PROJECT_DIR$/.idea/misc.xml" 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/D_star_Lite.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/D_star_Lite.py" afterDir="false" />
-      <change beforePath="$PROJECT_DIR$/Search_2D/LPAstar_backup.py" beforeDir="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/plotting.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/plotting.py" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/Search_2D/queue.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/queue.py" afterDir="false" />
     </list>
-    <option name="EXCLUDED_CONVERTED_TO_IGNORED" value="true" />
     <option name="SHOW_DIALOG" value="false" />
     <option name="HIGHLIGHT_CONFLICTS" value="true" />
     <option name="HIGHLIGHT_NON_ACTIVE_CHANGELIST" value="false" />
@@ -52,7 +56,7 @@
     <property name="ASKED_ADD_EXTERNAL_FILES" value="true" />
     <property name="RunOnceActivity.OpenProjectViewOnStart" value="true" />
     <property name="RunOnceActivity.ShowReadmeOnStart" value="true" />
-    <property name="last_opened_file_path" value="$PROJECT_DIR$/../../PythonRobotics-master/PathPlanning" />
+    <property name="last_opened_file_path" value="$PROJECT_DIR$" />
     <property name="restartRequiresConfirmation" value="false" />
     <property name="settings.editor.selected.configurable" value="com.jetbrains.python.configuration.PyActiveSdkModuleConfigurable" />
   </component>
@@ -61,20 +65,8 @@
       <recent name="C:\Users\Huiming Zhou\Desktop\path planning algorithms\Search-based Planning\Search_2D" />
     </key>
   </component>
-  <component name="RunDashboard">
-    <option name="ruleStates">
-      <list>
-        <RuleState>
-          <option name="name" value="ConfigurationTypeDashboardGroupingRule" />
-        </RuleState>
-        <RuleState>
-          <option name="name" value="StatusDashboardGroupingRule" />
-        </RuleState>
-      </list>
-    </option>
-  </component>
   <component name="RunManager" selected="Python.D_star_Lite">
-    <configuration name="D_star_Lite" type="PythonConfigurationType" factoryName="Python" temporary="true">
+    <configuration name="D_star" type="PythonConfigurationType" factoryName="Python" temporary="true" nameIsGenerated="true">
       <module name="Search-based Planning" />
       <option name="INTERPRETER_OPTIONS" value="" />
       <option name="PARENT_ENVS" value="true" />
@@ -86,7 +78,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/D_star_Lite.py" />
+      <option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_2D/D_star.py" />
       <option name="PARAMETERS" value="" />
       <option name="SHOW_COMMAND_LINE" value="false" />
       <option name="EMULATE_TERMINAL" value="false" />
@@ -95,7 +87,7 @@
       <option name="INPUT_FILE" value="" />
       <method v="2" />
     </configuration>
-    <configuration name="LPAstar" type="PythonConfigurationType" factoryName="Python" temporary="true">
+    <configuration name="D_star_Lite" type="PythonConfigurationType" factoryName="Python" temporary="true">
       <module name="Search-based Planning" />
       <option name="INTERPRETER_OPTIONS" value="" />
       <option name="PARENT_ENVS" value="true" />
@@ -107,7 +99,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/LPAstar.py" />
+      <option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_2D/D_star_Lite.py" />
       <option name="PARAMETERS" value="" />
       <option name="SHOW_COMMAND_LINE" value="false" />
       <option name="EMULATE_TERMINAL" value="false" />
@@ -116,7 +108,7 @@
       <option name="INPUT_FILE" value="" />
       <method v="2" />
     </configuration>
-    <configuration name="LRTAstar" type="PythonConfigurationType" factoryName="Python" temporary="true">
+    <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" />
@@ -128,7 +120,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/LRTAstar.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" />
@@ -137,7 +129,7 @@
       <option name="INPUT_FILE" value="" />
       <method v="2" />
     </configuration>
-    <configuration name="LRT_Astar3D" type="PythonConfigurationType" factoryName="Python" temporary="true">
+    <configuration name="RTAAstar" type="PythonConfigurationType" factoryName="Python" temporary="true">
       <module name="Search-based Planning" />
       <option name="INTERPRETER_OPTIONS" value="" />
       <option name="PARENT_ENVS" value="true" />
@@ -145,11 +137,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/RTAAstar.py" />
       <option name="PARAMETERS" value="" />
       <option name="SHOW_COMMAND_LINE" value="false" />
       <option name="EMULATE_TERMINAL" value="false" />
@@ -158,7 +150,7 @@
       <option name="INPUT_FILE" value="" />
       <method v="2" />
     </configuration>
-    <configuration name="RTAAstar" type="PythonConfigurationType" factoryName="Python" temporary="true">
+    <configuration name="astar" type="PythonConfigurationType" factoryName="Python" temporary="true" nameIsGenerated="true">
       <module name="Search-based Planning" />
       <option name="INTERPRETER_OPTIONS" value="" />
       <option name="PARENT_ENVS" value="true" />
@@ -170,7 +162,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/astar.py" />
       <option name="PARAMETERS" value="" />
       <option name="SHOW_COMMAND_LINE" value="false" />
       <option name="EMULATE_TERMINAL" value="false" />
@@ -201,20 +193,20 @@
       <method v="2" />
     </configuration>
     <list>
-      <item itemvalue="Python.dijkstra" />
+      <item itemvalue="Python.D_star" />
+      <item itemvalue="Python.D_star_Lite" />
       <item itemvalue="Python.LPAstar" />
-      <item itemvalue="Python.LRTAstar" />
       <item itemvalue="Python.RTAAstar" />
-      <item itemvalue="Python.LRT_Astar3D" />
-      <item itemvalue="Python.D_star_Lite" />
+      <item itemvalue="Python.astar" />
+      <item itemvalue="Python.dijkstra" />
     </list>
     <recent_temporary>
       <list>
         <item itemvalue="Python.D_star_Lite" />
+        <item itemvalue="Python.D_star" />
+        <item itemvalue="Python.astar" />
         <item itemvalue="Python.LPAstar" />
         <item itemvalue="Python.RTAAstar" />
-        <item itemvalue="Python.LRTAstar" />
-        <item itemvalue="Python.LRT_Astar3D" />
       </list>
     </recent_temporary>
   </component>
@@ -236,40 +228,39 @@
       <map>
         <entry key="MAIN">
           <value>
-            <State>
-              <option name="COLUMN_ORDER" />
-            </State>
+            <State />
           </value>
         </entry>
       </map>
     </option>
+    <option name="oldMeFiltersMigrated" value="true" />
   </component>
   <component name="WindowStateProjectService">
     <state x="2377" y="197" key="#com.intellij.execution.impl.EditConfigurationsDialog" timestamp="1593282711181">
       <screen x="1920" y="0" width="1920" height="1080" />
     </state>
     <state x="2377" y="197" key="#com.intellij.execution.impl.EditConfigurationsDialog/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1593282711181" />
-    <state x="2700" y="297" width="424" height="482" key="FileChooserDialogImpl" timestamp="1593282670147">
+    <state x="2700" y="297" width="424" height="482" key="FileChooserDialogImpl" timestamp="1593631602696">
       <screen x="1920" y="0" width="1920" height="1080" />
     </state>
-    <state x="2700" y="297" width="424" height="482" key="FileChooserDialogImpl/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1593282670147" />
+    <state x="2700" y="297" width="424" height="482" key="FileChooserDialogImpl/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1593631602696" />
     <state x="819" y="314" key="FileChooserDialogImpl/65.24.1855.1056/1920.0.1920.1080@65.24.1855.1056" timestamp="1592933974409" />
-    <state width="1832" height="206" key="GridCell.Tab.0.bottom" timestamp="1593282755222">
+    <state width="1832" height="125" key="GridCell.Tab.0.bottom" timestamp="1593677260010">
       <screen x="1920" y="0" width="1920" height="1080" />
     </state>
-    <state width="1832" height="206" key="GridCell.Tab.0.bottom/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1593282755222" />
-    <state width="1832" height="206" key="GridCell.Tab.0.center" timestamp="1593282755222">
+    <state width="1832" height="125" key="GridCell.Tab.0.bottom/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1593677260010" />
+    <state width="1832" height="125" key="GridCell.Tab.0.center" timestamp="1593677260010">
       <screen x="1920" y="0" width="1920" height="1080" />
     </state>
-    <state width="1832" height="206" key="GridCell.Tab.0.center/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1593282755222" />
-    <state width="1832" height="206" key="GridCell.Tab.0.left" timestamp="1593282755222">
+    <state width="1832" height="125" key="GridCell.Tab.0.center/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1593677260010" />
+    <state width="1832" height="125" key="GridCell.Tab.0.left" timestamp="1593677260010">
       <screen x="1920" y="0" width="1920" height="1080" />
     </state>
-    <state width="1832" height="206" key="GridCell.Tab.0.left/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1593282755222" />
-    <state width="1832" height="206" key="GridCell.Tab.0.right" timestamp="1593282755222">
+    <state width="1832" height="125" key="GridCell.Tab.0.left/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1593677260010" />
+    <state width="1832" height="125" key="GridCell.Tab.0.right" timestamp="1593677260010">
       <screen x="1920" y="0" width="1920" height="1080" />
     </state>
-    <state width="1832" height="206" key="GridCell.Tab.0.right/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1593282755222" />
+    <state width="1832" height="125" key="GridCell.Tab.0.right/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1593677260010" />
     <state x="2406" y="174" key="SettingsEditor" timestamp="1593282573348">
       <screen x="1920" y="0" width="1920" height="1080" />
     </state>

Search-based Planning/Search_2D/D_star.py

@@ -0,0 +1,186 @@
+"""
+D_star 2D
+@author: huiming zhou
+"""
+
+import os
+import sys
+import matplotlib.pyplot as plt
+
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
+                "/../../Search-based Planning/")
+
+from Search_2D import plotting
+from Search_2D import env
+
+
+class Dstar:
+    def __init__(self, x_start, x_goal):
+        self.xI, self.xG = x_start, x_goal
+
+        self.Env = env.Env()
+        self.Plot = plotting.Plotting(self.xI, self.xG)
+
+        self.u_set = self.Env.motions
+        self.obs = self.Env.obs
+        self.x = self.Env.x_range
+        self.y = self.Env.y_range
+
+        self.fig = plt.figure()
+        self.OPEN = set()
+        self.t = {}
+        self.PARENT = {}
+        self.h = {self.xG: 0}
+        self.k = {}
+        self.path = []
+
+        for i in range(self.Env.x_range):
+            for j in range(self.Env.y_range):
+                self.t[(i, j)] = 'NEW'
+                self.k[(i, j)] = 0
+                self.PARENT[(i, j)] = None
+
+    def run(self, s_start, s_end):
+        self.insert(s_end, 0)
+        while True:
+            self.process_state()
+            if self.t[s_start] == 'CLOSED':
+                break
+        self.path = self.extract_path(s_start, s_end)
+        self.Plot.plot_grid("Dynamic A* (D*)")
+        self.plot_path(self.path)
+        self.fig.canvas.mpl_connect('button_press_event', self.on_press)
+        plt.show()
+
+    def on_press(self, event):
+        x, y = event.xdata, event.ydata
+        if x < 0 or x > self.x - 1 or y < 0 or y > self.y - 1:
+            print("Please choose right area!")
+        else:
+            x, y = int(x), int(y)
+            print("Add obstacle at: x =", x, ",", "y =", y)
+            self.obs.add((x, y))
+            plt.plot(x, y, 'sk')
+            if (x, y) in self.path:
+                s = self.xI
+                while s != self.xG:
+                    if self.PARENT[s] in self.obs:
+                        self.modify(s)
+                        continue
+                    s = self.PARENT[s]
+                self.path = self.extract_path(self.xI, self.xG)
+                self.plot_path(self.path)
+            self.fig.canvas.draw_idle()
+
+    def extract_path(self, s_start, s_end):
+        path = []
+        s = s_start
+        while True:
+            s = self.PARENT[s]
+            if s == s_end:
+                return path
+            path.append(s)
+
+    def process_state(self):
+        s = self.min_state()
+        if s is None:
+            return -1
+        k_old = self.get_k_min()
+        self.delete(s)
+
+        if k_old < self.h[s]:
+            for s_n in self.get_neighbor(s):
+                if self.h[s_n] <= k_old and self.h[s] > self.h[s_n] + self.cost(s_n, s):
+                    self.PARENT[s] = s_n
+                    self.h[s] = self.h[s_n] + self.cost(s_n, s)
+        if k_old == self.h[s]:
+            for s_n in self.get_neighbor(s):
+                if self.t[s_n] == 'NEW' or \
+                        (self.PARENT[s_n] == s and self.h[s_n] != self.h[s] + self.cost(s, s_n)) or \
+                        (self.PARENT[s_n] != s and self.h[s_n] > self.h[s] + self.cost(s, s_n)):
+                    self.PARENT[s_n] = s
+                    self.insert(s_n, self.h[s] + self.cost(s, s_n))
+        else:
+            for s_n in self.get_neighbor(s):
+                if self.t[s_n] == 'NEW' or \
+                        (self.PARENT[s_n] == s and self.h[s_n] != self.h[s] + self.cost(s, s_n)):
+                    self.PARENT[s_n] = s
+                    self.insert(s_n, self.h[s] + self.cost(s, s_n))
+                else:
+                    if self.PARENT[s_n] != s and self.h[s_n] > self.h[s] + self.cost(s, s_n):
+                        self.insert(s, self.h[s])
+                    else:
+                        if self.PARENT[s_n] != s and \
+                                self.h[s] > self.h[s_n] + self.cost(s_n, s) and \
+                                self.t[s_n] == 'CLOSED' and \
+                                self.h[s_n] > k_old:
+                            self.insert(s_n, self.h[s_n])
+        return self.get_k_min()
+
+    def min_state(self):
+        if not self.OPEN:
+            return None
+        return min(self.OPEN, key=lambda x: self.k[x])
+
+    def get_k_min(self):
+        if not self.OPEN:
+            return -1
+        return min([self.k[x] for x in self.OPEN])
+
+    def insert(self, s, h_new):
+        if self.t[s] == 'NEW':
+            self.k[s] = h_new
+        elif self.t[s] == 'OPEN':
+            self.k[s] = min(self.k[s], h_new)
+        elif self.t[s] == 'CLOSED':
+            self.k[s] = min(self.h[s], h_new)
+        self.h[s] = h_new
+        self.t[s] = 'OPEN'
+        self.OPEN.add(s)
+
+    def delete(self, s):
+        if self.t[s] == 'OPEN':
+            self.t[s] = 'CLOSED'
+        self.OPEN.remove(s)
+
+    def modify(self, s):
+        self.modify_cost(s)
+        while True:
+            k_min = self.process_state()
+            if k_min >= self.h[s]:
+                break
+
+    def modify_cost(self, s):
+        if self.t[s] == 'CLOSED':
+            self.insert(s, self.h[self.PARENT[s]] + self.cost(s, self.PARENT[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 cost(self, s_start, s_end):
+        if s_start in self.obs or s_end in self.obs:
+            return float("inf")
+        return 1
+
+    @staticmethod
+    def plot_path(path):
+        px = [x[0] for x in path]
+        py = [x[1] for x in path]
+        plt.plot(px, py, marker='o')
+
+
+def main():
+    s_start = (5, 5)
+    s_goal = (45, 25)
+    dstar = Dstar(s_start, s_goal)
+    dstar.run(s_start, s_goal)
+
+
+if __name__ == '__main__':
+    main()

Search-based Planning/Search_2D/D_star_Lite.py

@@ -16,7 +16,7 @@ from Search_2D import plotting
 from Search_2D import env
 
 
-class LpaStar:
+class DStar:
     def __init__(self, x_start, x_goal, heuristic_type):
         self.xI, self.xG = x_start, x_goal
         self.heuristic_type = heuristic_type
@@ -42,14 +42,11 @@ class LpaStar:
         self.U.put(self.xG, self.Key(self.xG))
         self.fig = plt.figure()
 
-    def searching(self):
-        self.Plot.plot_grid("Lifelong Planning A*")
-
+    def run(self):
+        self.Plot.plot_grid("Dynamic A* (D*)")
         self.ComputePath()
-        self.plot_path(self.extract_path_test())
-
-        # self.fig.canvas.mpl_connect('button_press_event', self.on_press)
-
+        self.plot_path(self.extract_path())
+        self.fig.canvas.mpl_connect('button_press_event', self.on_press)
         plt.show()
 
     def on_press(self, event):
@@ -59,20 +56,37 @@ class LpaStar:
         else:
             x, y = int(x), int(y)
             print("Change position: x =", x, ",", "y =", y)
-            if (x, y) not in self.obs:
-                self.obs.add((x, y))
-                plt.plot(x, y, 'sk')
-                self.rhs[(x, y)] = float("inf")
-                self.g[(x, y)] = float("inf")
-                for node in self.getSucc((x, y)):
-                    self.UpdateVertex(node)
-            else:
-                self.obs.remove((x, y))
-                plt.plot(x, y, marker='s', color='white')
-                self.UpdateVertex((x, y))
-            self.ComputePath()
-            self.plot_path(self.extract_path_test())
-            self.fig.canvas.draw_idle()
+
+            s_curr = self.xI
+            s_last = self.xI
+            i = 0
+            path = []
+
+            while s_curr != self.xG:
+                s_list = {}
+                for s in self.get_neighbor(s_curr):
+                    s_list[s] = self.g[s] + self.get_cost(s_curr, s)
+                s_curr = min(s_list, key=s_list.get)
+                path.append(s_curr)
+
+                if i < 1:
+                    self.km += self.h(s_last, s_curr)
+                    s_last = s_curr
+                    if (x, y) not in self.obs:
+                        self.obs.add((x, y))
+                        plt.plot(x, y, 'sk')
+                        self.g[(x, y)] = float("inf")
+                        self.rhs[(x, y)] = float("inf")
+                    else:
+                        self.obs.remove((x, y))
+                        plt.plot(x, y, marker='s', color='white')
+                        self.UpdateVertex((x, y))
+                    for s in self.get_neighbor((x, y)):
+                        self.UpdateVertex(s)
+                    i += 1
+                self.ComputePath()
+                self.plot_path(path)
+                self.fig.canvas.draw_idle()
 
     @staticmethod
     def plot_path(path):
@@ -81,88 +95,46 @@ class LpaStar:
         plt.plot(px, py, marker='o')
 
     def ComputePath(self):
-        count = 0
         while self.U.top_key() < self.Key(self.xI) or \
                 self.rhs[self.xI] != self.g[self.xI]:
-            count += 1
-            print(count)
             k_old = self.U.top_key()
             s = self.U.get()
             if k_old < self.Key(s):
                 self.U.put(s, self.Key(s))
             elif self.g[s] > self.rhs[s]:
                 self.g[s] = self.rhs[s]
-                for x in self.getPred(s):
+                for x in self.get_neighbor(s):
                     self.UpdateVertex(x)
             else:
                 self.g[s] = float("inf")
                 self.UpdateVertex(s)
-                for x in self.getPred(s):
+                for x in self.get_neighbor(s):
                     self.UpdateVertex(x)
 
-    def getSucc(self, s):
-        nei_list = set()
-        for u in self.u_set:
-            s_next = tuple([s[i] + u[i] for i in range(2)])
-            if s_next not in self.obs and self.g[s_next] >= self.g[s]:
-                nei_list.add(s_next)
-        return nei_list
-
-    def getPred(self, s):
-        nei_list = set()
-        for u in self.u_set:
-            s_next = tuple([s[i] + u[i] for i in range(2)])
-            if s_next not in self.obs and self.g[s_next] <= self.g[s]:
-                nei_list.add(s_next)
-        return nei_list
-
     def UpdateVertex(self, s):
         if s != self.xG:
             self.rhs[s] = float("inf")
-            for x in self.getSucc(s):
+            for x in self.get_neighbor(s):
                 self.rhs[s] = min(self.rhs[s], self.g[x] + self.get_cost(s, x))
         self.U.remove(s)
         if self.g[s] != self.rhs[s]:
             self.U.put(s, self.Key(s))
 
-    def extract_path_test(self):
-        path = []
-        s = self.xG
-
-        for k in range(100):
-            g_list = {}
-            for x in self.get_neighbor(s):
-                g_list[x] = self.g[x]
-            s = min(g_list, key=g_list.get)
-            if s == self.xI:
-                return list(reversed(path))
-            path.append(s)
-        return list(reversed(path))
-
     def Key(self, s):
-        return [min(self.g[s], self.rhs[s]) + self.h(s) + self.km,
+        return [min(self.g[s], self.rhs[s]) + self.h(self.xI, s) + self.km,
                 min(self.g[s], self.rhs[s])]
 
-    def h(self, s):
+    def h(self, s_start, s_goal):
         heuristic_type = self.heuristic_type            # heuristic type
-        s_start = self.xI                               # goal node
 
         if heuristic_type == "manhattan":
-            return abs(s[0] - s_start[0]) + abs(s[1] - s_start[1])
+            return abs(s_goal[0] - s_start[0]) + abs(s_goal[1] - s_start[1])
         else:
-            return math.hypot(s[0] - s_start[0], s[1] - s_start[1])
-
-    @staticmethod
-    def get_cost(s_start, s_end):
-        """
-        Calculate cost for this motion
-
-        :param s_start:
-        :param s_end:
-        :return:  cost for this motion
-        :note: cost function could be more complicate!
-        """
+            return math.hypot(s_goal[0] - s_start[0], s_goal[1] - s_start[1])
 
+    def get_cost(self, s_start, s_end):
+        if s_start in self.obs or s_end in self.obs:
+            return float("inf")
         return 1
 
     def get_neighbor(self, s):
@@ -176,14 +148,15 @@ class LpaStar:
 
     def extract_path(self):
         path = []
-        s = self.xG
-
+        s = self.xI
+        count = 0
         while True:
+            count += 1
             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:
+            if s == self.xG or count > 100:
                 return list(reversed(path))
             path.append(s)
 
@@ -207,8 +180,8 @@ def main():
     x_start = (5, 5)
     x_goal = (45, 25)
 
-    lpastar = LpaStar(x_start, x_goal, "euclidean")
-    lpastar.searching()
+    dstar = DStar(x_start, x_goal, "euclidean")
+    dstar.run()
 
 
 if __name__ == '__main__':

Search-based Planning/Search_2D/LPAstar.py

@@ -21,15 +21,15 @@ class LpaStar:
         self.xI, self.xG = x_start, x_goal
         self.heuristic_type = heuristic_type
 
-        self.Env = env.Env()  # class Env
+        self.Env = env.Env()
         self.Plot = plotting.Plotting(x_start, x_goal)
 
-        self.u_set = self.Env.motions  # feasible input set
-        self.obs = self.Env.obs  # position of obstacles
+        self.u_set = self.Env.motions
+        self.obs = self.Env.obs
         self.x = self.Env.x_range
         self.y = self.Env.y_range
 
-        self.U = queue.QueuePrior()  # priority queue / U set
+        self.U = queue.QueuePrior()
         self.g, self.rhs = {}, {}
 
         for i in range(self.Env.x_range):
@@ -39,9 +39,9 @@ class LpaStar:
 
         self.rhs[self.xI] = 0
         self.U.put(self.xI, self.Key(self.xI))
-
-    def searching(self):
         self.fig = plt.figure()
+
+    def run(self):
         self.Plot.plot_grid("Lifelong Planning A*")
 
         self.ComputePath()
@@ -62,9 +62,10 @@ class LpaStar:
             if (x, y) not in self.obs:
                 self.obs.add((x, y))
                 plt.plot(x, y, 'sk')
+                plt.pause(0.001)
                 self.rhs[(x, y)] = float("inf")
                 self.g[(x, y)] = float("inf")
-                for node in self.getSucc((x, y)):
+                for node in self.get_neighbor((x, y)):
                     self.UpdateVertex(node)
             else:
                 self.obs.remove((x, y))
@@ -86,54 +87,48 @@ class LpaStar:
             s = self.U.get()
             if self.g[s] > self.rhs[s]:
                 self.g[s] = self.rhs[s]
-                for x in self.getSucc(s):
-                    self.UpdateVertex(x)
             else:
                 self.g[s] = float("inf")
                 self.UpdateVertex(s)
-                for x in self.getSucc(s):
-                    self.UpdateVertex(x)
-
-    def getSucc(self, s):
-        nei_list = set()
-        for u in self.u_set:
-            s_next = tuple([s[i] + u[i] for i in range(2)])
-            if s_next not in self.obs and self.g[s_next] > self.g[s]:
-                nei_list.add(s_next)
-        return nei_list
-
-    def getPred(self, s):
-        nei_list = set()
-        for u in self.u_set:
-            s_next = tuple([s[i] + u[i] for i in range(2)])
-            if s_next not in self.obs and self.g[s_next] < self.g[s]:
-                nei_list.add(s_next)
-        return nei_list
+            for x in self.get_neighbor(s):
+                self.UpdateVertex(x)
 
     def UpdateVertex(self, s):
         if s != self.xI:
             u_min = float("inf")
-            for x in self.getPred(s):
-                u_min = min(u_min, self.g[x] + self.get_cost(x, s))
+            for x in self.get_neighbor(s):
+                u_min = min(u_min, self.g[x] + self.cost(x, s))
             self.rhs[s] = u_min
         self.U.remove(s)
         if self.g[s] != self.rhs[s]:
             self.U.put(s, self.Key(s))
 
-    def print_g(self):
-        print("he")
-        for k in range(self.Env.y_range):
-            j = self.Env.y_range - k - 1
-            string = ""
-            for i in range(self.Env.x_range):
-                if self.g[(i, j)] == float("inf"):
-                    string += ("00" + ', ')
-                else:
-                    if self.g[(i, j)] // 10 == 0:
-                        string += ("0" + str(self.g[(i, j)]) + ', ')
-                    else:
-                        string += (str(self.g[(i, j)]) + ', ')
-            print(string)
+    def get_neighbor(self, s):
+        nei_list = set()
+        for u in self.u_set:
+            s_next = tuple([s[i] + u[i] for i in range(2)])
+            if s_next not in self.obs:
+                nei_list.add(s_next)
+
+        return nei_list
+
+    def Key(self, s):
+        return [min(self.g[s], self.rhs[s]) + self.h(s),
+                min(self.g[s], self.rhs[s])]
+
+    def h(self, s):
+        heuristic_type = self.heuristic_type  # heuristic type
+        goal = self.xG  # goal node
+
+        if heuristic_type == "manhattan":
+            return abs(goal[0] - s[0]) + abs(goal[1] - s[1])
+        else:
+            return math.hypot(goal[0] - s[0], goal[1] - s[1])
+
+    def cost(self, s_start, s_end):
+        if s_start in self.obs or s_end in self.obs:
+            return float("inf")
+        return 1
 
     def extract_path(self):
         path = []
@@ -162,48 +157,28 @@ class LpaStar:
             path.append(s)
         return list(reversed(path))
 
-    def get_neighbor(self, s):
-        nei_list = set()
-        for u in self.u_set:
-            s_next = tuple([s[i] + u[i] for i in range(2)])
-            if s_next not in self.obs:
-                nei_list.add(s_next)
-
-        return nei_list
-
-    def Key(self, s):
-        return [min(self.g[s], self.rhs[s]) + self.h(s),
-                min(self.g[s], self.rhs[s])]
-
-    def h(self, s):
-        heuristic_type = self.heuristic_type  # heuristic type
-        goal = self.xG  # goal node
-
-        if heuristic_type == "manhattan":
-            return abs(goal[0] - s[0]) + abs(goal[1] - s[1])
-        else:
-            return math.hypot(goal[0] - s[0], goal[1] - s[1])
-
-    @staticmethod
-    def get_cost(s_start, s_end):
-        """
-        Calculate cost for this motion
-
-        :param s_start:
-        :param s_end:
-        :return:  cost for this motion
-        :note: cost function could be more complicate!
-        """
-
-        return 1
+    def print_g(self):
+        print("he")
+        for k in range(self.Env.y_range):
+            j = self.Env.y_range - k - 1
+            string = ""
+            for i in range(self.Env.x_range):
+                if self.g[(i, j)] == float("inf"):
+                    string += ("00" + ', ')
+                else:
+                    if self.g[(i, j)] // 10 == 0:
+                        string += ("0" + str(self.g[(i, j)]) + ', ')
+                    else:
+                        string += (str(self.g[(i, j)]) + ', ')
+            print(string)
 
 
 def main():
     x_start = (5, 5)
     x_goal = (45, 25)
 
-    lpastar = LpaStar(x_start, x_goal, "euclidean")
-    lpastar.searching()
+    lpastar = LpaStar(x_start, x_goal, "manhattan")
+    lpastar.run()
 
 
 if __name__ == '__main__':

Search-based Planning/Search_2D/plotting.py

@@ -96,10 +96,10 @@ class Plotting:
 
     def plot_path(self, path, cl='r', flag=False):
         if self.xI in path:
-            path.remove(self.xI)
+            path.delete(self.xI)
 
         if self.xG in path:
-            path.remove(self.xG)
+            path.delete(self.xG)
 
         path_x = [path[i][0] for i in range(len(path))]
         path_y = [path[i][1] for i in range(len(path))]
@@ -113,10 +113,10 @@ class Plotting:
 
     def plot_visited_bi(self, v_fore, v_back):
         if self.xI in v_fore:
-            v_fore.remove(self.xI)
+            v_fore.delete(self.xI)
 
         if self.xG in v_back:
-            v_back.remove(self.xG)
+            v_back.delete(self.xG)
 
         len_fore, len_back = len(v_fore), len(v_back)
 

Search-based Planning/Search_2D/queue.py

@@ -53,16 +53,15 @@ class QueuePrior:
         return len(self.queue) == 0
 
     def put(self, item, priority):
-        flag = 0
+        heapq.heappush(self.queue, (priority, item))  # reorder x using priority
+
+    def update(self, item, priority):
         count = 0
         for (p, x) in self.queue:
             if x == item:
                 self.queue[count] = (priority, item)
-                flag = 1
                 break
             count += 1
-        if flag == 0:
-            heapq.heappush(self.queue, (priority, item))  # reorder x using priority
 
     def get(self):
         return heapq.heappop(self.queue)[1]  # pop out the smallest item