From 003a01ac9872c1e39fe3b3279027403eeca9fe80 Mon Sep 17 00:00:00 2001
From: zhm-real <zhou.hm0420@gmail.com>
Date: Thu, 2 Jul 2020 10:24:23 -0700
Subject: [PATCH] update

---
 .../.idea/Search-based Planning.iml           |   2 +-
 Search-based Planning/.idea/misc.xml          |   2 +-
 Search-based Planning/.idea/workspace.xml     | 145 +++++++-------
 Search-based Planning/Search_2D/D_star.py     | 186 ++++++++++++++++++
 .../Search_2D/D_star_Lite.py                  | 131 +++++-------
 Search-based Planning/Search_2D/LPAstar.py    | 133 +++++--------
 .../Search_2D/__pycache__/env.cpython-37.pyc  | Bin 1213 -> 1240 bytes
 .../__pycache__/plotting.cpython-37.pyc       | Bin 5216 -> 5208 bytes
 .../__pycache__/queue.cpython-37.pyc          | Bin 3103 -> 3187 bytes
 Search-based Planning/Search_2D/plotting.py   |   8 +-
 Search-based Planning/Search_2D/queue.py      |   7 +-
 11 files changed, 369 insertions(+), 245 deletions(-)
 create mode 100644 Search-based Planning/Search_2D/D_star.py

diff --git a/Search-based Planning/.idea/Search-based Planning.iml b/Search-based Planning/.idea/Search-based Planning.iml
index c444878..5965bde 100644
--- a/Search-based Planning/.idea/Search-based Planning.iml	
+++ b/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>
\ No newline at end of file
diff --git a/Search-based Planning/.idea/misc.xml b/Search-based Planning/.idea/misc.xml
index a2e120d..0e7ac62 100644
--- a/Search-based Planning/.idea/misc.xml	
+++ b/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>
\ No newline at end of file
diff --git a/Search-based Planning/.idea/workspace.xml b/Search-based Planning/.idea/workspace.xml
index e82275b..66da253 100644
--- a/Search-based Planning/.idea/workspace.xml	
+++ b/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,19 +65,28 @@
       <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" type="PythonConfigurationType" factoryName="Python" temporary="true" nameIsGenerated="true">
+      <module name="Search-based Planning" />
+      <option name="INTERPRETER_OPTIONS" value="" />
+      <option name="PARENT_ENVS" value="true" />
+      <envs>
+        <env name="PYTHONUNBUFFERED" value="1" />
+      </envs>
+      <option name="SDK_HOME" value="" />
+      <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_2D/D_star.py" />
+      <option name="PARAMETERS" value="" />
+      <option name="SHOW_COMMAND_LINE" value="false" />
+      <option name="EMULATE_TERMINAL" value="false" />
+      <option name="MODULE_MODE" value="false" />
+      <option name="REDIRECT_INPUT" value="false" />
+      <option name="INPUT_FILE" value="" />
+      <method v="2" />
+    </configuration>
     <configuration name="D_star_Lite" type="PythonConfigurationType" factoryName="Python" temporary="true">
       <module name="Search-based Planning" />
       <option name="INTERPRETER_OPTIONS" value="" />
@@ -116,48 +129,6 @@
       <option name="INPUT_FILE" value="" />
       <method v="2" />
     </configuration>
-    <configuration name="LRTAstar" type="PythonConfigurationType" factoryName="Python" temporary="true">
-      <module name="Search-based Planning" />
-      <option name="INTERPRETER_OPTIONS" value="" />
-      <option name="PARENT_ENVS" value="true" />
-      <envs>
-        <env name="PYTHONUNBUFFERED" value="1" />
-      </envs>
-      <option name="SDK_HOME" value="" />
-      <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_2D/LRTAstar.py" />
-      <option name="PARAMETERS" value="" />
-      <option name="SHOW_COMMAND_LINE" value="false" />
-      <option name="EMULATE_TERMINAL" value="false" />
-      <option name="MODULE_MODE" value="false" />
-      <option name="REDIRECT_INPUT" value="false" />
-      <option name="INPUT_FILE" value="" />
-      <method v="2" />
-    </configuration>
-    <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" />
-      <envs>
-        <env name="PYTHONUNBUFFERED" value="1" />
-      </envs>
-      <option name="SDK_HOME" value="" />
-      <option name="WORKING_DIRECTORY" value="$PROJECT_DIR$/Search_3D" />
-      <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="PARAMETERS" value="" />
-      <option name="SHOW_COMMAND_LINE" value="false" />
-      <option name="EMULATE_TERMINAL" value="false" />
-      <option name="MODULE_MODE" value="false" />
-      <option name="REDIRECT_INPUT" value="false" />
-      <option name="INPUT_FILE" value="" />
-      <method v="2" />
-    </configuration>
     <configuration name="RTAAstar" type="PythonConfigurationType" factoryName="Python" temporary="true">
       <module name="Search-based Planning" />
       <option name="INTERPRETER_OPTIONS" value="" />
@@ -179,6 +150,27 @@
       <option name="INPUT_FILE" value="" />
       <method v="2" />
     </configuration>
+    <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" />
+      <envs>
+        <env name="PYTHONUNBUFFERED" value="1" />
+      </envs>
+      <option name="SDK_HOME" value="" />
+      <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_2D/astar.py" />
+      <option name="PARAMETERS" value="" />
+      <option name="SHOW_COMMAND_LINE" value="false" />
+      <option name="EMULATE_TERMINAL" value="false" />
+      <option name="MODULE_MODE" value="false" />
+      <option name="REDIRECT_INPUT" value="false" />
+      <option name="INPUT_FILE" value="" />
+      <method v="2" />
+    </configuration>
     <configuration name="dijkstra" type="PythonConfigurationType" factoryName="Python" nameIsGenerated="true">
       <module name="Search-based Planning" />
       <option name="INTERPRETER_OPTIONS" value="" />
@@ -201,20 +193,20 @@
       <method v="2" />
     </configuration>
     <list>
-      <item itemvalue="Python.dijkstra" />
-      <item itemvalue="Python.LPAstar" />
-      <item itemvalue="Python.LRTAstar" />
-      <item itemvalue="Python.RTAAstar" />
-      <item itemvalue="Python.LRT_Astar3D" />
+      <item itemvalue="Python.D_star" />
       <item itemvalue="Python.D_star_Lite" />
+      <item itemvalue="Python.LPAstar" />
+      <item itemvalue="Python.RTAAstar" />
+      <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>
diff --git a/Search-based Planning/Search_2D/D_star.py b/Search-based Planning/Search_2D/D_star.py
new file mode 100644
index 0000000..c8ddbcd
--- /dev/null
+++ b/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()
diff --git a/Search-based Planning/Search_2D/D_star_Lite.py b/Search-based Planning/Search_2D/D_star_Lite.py
index 15a9493..df67935 100644
--- a/Search-based Planning/Search_2D/D_star_Lite.py	
+++ b/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__':
diff --git a/Search-based Planning/Search_2D/LPAstar.py b/Search-based Planning/Search_2D/LPAstar.py
index 4ef9d51..fd6310b 100644
--- a/Search-based Planning/Search_2D/LPAstar.py	
+++ b/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__':
diff --git a/Search-based Planning/Search_2D/__pycache__/env.cpython-37.pyc b/Search-based Planning/Search_2D/__pycache__/env.cpython-37.pyc
index 82832455a65a8f922223c62697da1c9b6e904b77..ff2b6864817796bee8af39576e6b79d2f97437fc 100644
GIT binary patch
delta 111
zcmdnXd4rSJiI<m)0SK-<{1Z28Bd-smZiRkEer~FMRYrcPen3%vdQoDoenDbMhHgPl
zVqRWmUb=2#PI`V(W=TeFvHs+VjG}`2!KsNw$r<rRF8X?UAOsTSMiSk;k&%ZP05pjt
AY5)KL

delta 116
zcmcb?xtEjIiI<m)0SK1&{f;}nk=KVYBG%a|CbT%Us5r)>G&46dFI^!jBfm7pCAB!a
zB)=f0Ah9Gvp&%zQFApS@n3JAglv$FITO1Rdnpl*ap_`OgoSLE#0961LjW=?M*?fkP
GhZz7#bScFE

diff --git a/Search-based Planning/Search_2D/__pycache__/plotting.cpython-37.pyc b/Search-based Planning/Search_2D/__pycache__/plotting.cpython-37.pyc
index 91533dfa0ea087a1918e9271ff2c52f8913a3315..a39f99c9f024d8c809f2337b4361ab3534dd315b 100644
GIT binary patch
delta 600
zcmZvZPixdb7{;5pjmfszKekvA1TR8EcN<%^>Ol)@{RbkV1#u3R*=9Nkon+#h2{rXl
z5O1DjZsNgDAk_T;`VqVcvUso;zlGqu>Eg*ie)Hs=_vLwJ@@w(OVr$_0%_;LNz4-I|
zd-t&Q+v+_HW0|sWKbB?q2qXuZhB;MnKNmF1l8pDM;1UuQr^UN33(Y5t!gjp1x)F{Z
z*v}lF*u#R@+`Ppu(VyMGBBP&fX<NbQ@ys>bx~1Rx7Y=&H6b6XCL2v?=QHO|&2K?3h
zr@Ke{Z`K1B8s>ZKL`*QnU>ZA4BNh;CM1VMB5IEqX?jp__kj9RnoH&?&>*Zvvz?&NN
zTAS8N9byTa{~Kc5+Ln;*2)V~#C`tw^`mlX{Z50PBgTRJMs4t_o;0kK*AMjpb&EPeP
ztNMKBp|zo9=Uk`GdjqRC5$pOx=i41Jy%#g8$gD_stcu+V9mgKXiVBp(D+E={{EiAR
zGI1o(j59J5F_usI&3$*@Z{EXguTYE~Po9jDw4S?-8_h^Yd~%@sSZL&;Oi4RsQBtP0
sN$od#T*ynv1ta?7DdwWDIrKpLb6@94H>D~UQi)`^oL8ubKAJoD3r$R<IsgCw

delta 558
zcmZXQO-~a+7{}ek)@{o!Ed?n?6FflZ0V6M!@^S&1l#pml)PrUNnX=>Vrn9?~XLf@P
z2Mu3gJQ%(J2{+O&0N+4wFegrY2`|jE8ZS;Vzx?xm=6Rdz`P+GC+qTOO^l^@V?|&J;
zaPEwDZ)e^8ASeiTHw&XM_Gdl?N#=H_=%)#HIZ1;V&PW_%h%i5aFbyK%%ID@}#eGK!
z90mtV9e228ek|I`><MN1`ij1iXI9ft!(Ucr8Vkd2rEMC!@|!*TbWx80UBr?`a|D)A
zuOM3T%C5Iy8g&8j7%_zyLo6T~8qFeDsB4HP8frM|Fk>aJ{SwXpYvU{DFoF0pOO7b?
zSVrNMHYF*yR$&#B42`A<ZPaV14OmBA`Um_#*w8pZu_+tXH^#K|tIw+WY}@GWAUg7U
z^{1r@M*$_NlC0L4QzgMjN_`*$z!_CB0TTOERXj?$=nrrtCC_W8?_XoMBvMLQZa(tD
zD9_u&K-o(?|L+EG(Rj#N7OAm_dSMpjP4cYKKL*c$GhO({IJK~^-mohR^~(uWi%7~@
QlCrS(f)7wBc~k#-59Y3%J^%m!

diff --git a/Search-based Planning/Search_2D/__pycache__/queue.cpython-37.pyc b/Search-based Planning/Search_2D/__pycache__/queue.cpython-37.pyc
index 324b066eec60223e4ce37a6f22a4a19dd28420a3..9185c918610c0e661d2d69a6cf1c28cb5efae5f9 100644
GIT binary patch
delta 587
zcmYjN&ui2`6rMMeNwb^n+8?0n4{(b;80<D!5J7rrt>UF6(t=d9CEbjht<5GoNwh9m
zya@gQbMdDB3DSdqfmg3W4&D`SJqy0c?!g(}H^ckho9~<Vv31^RZqClu39h;QzXxB<
zFU=ur@3~<d33n96x$DWeC;iAx{4DGwC;nj2AM`r@NiUZDER0fjSNO6Uc8>g11Z{6x
zFwGvWZMkPZ;I{#u`onIkbF<Y5=HHR0cCfHZoW-*XGkdVlP@QAk$4D<CWaJYR<d_z;
zfEV;L1riV}haHENJQRL1EG>N`c^XRH=`d;HQ5yYBM5UF;zIOLUE!8f+rAQDMH6Q09
z7oKKTlRWEcQN47Zfl-OC<0XJGJ6%J0vPGWXd`8E{m>0Z&msGAy3~8=l>%>D0=v^{q
z!^V?|MYbaW-}t4$0qyO8IA&!|UcsEaics=yoDZ@Rk`hkkj80czY0k`(z|X`zyr&HW
z0tU>11x=tZn*Z+;lR9-&TeYrX8IGW(KGarVQ+=xmc%W|8kC!TfRwF7+DWdq5n2u`V
z=DOP8i~I(*KT_Z8uVGs~wA;{7J$o72>b-quc>y`gFsQ|<ZB@wSC9TM%9wN&Av^V|%
D(k_Ru

delta 558
zcmYjOO>fgc5Zzg?9XCm>P>>qbOK47(LM19hrHTtAf^vz13IbK8k*T+_n-4p?Hl&rU
z{)qhnz?lO={TqlM;2TItz?CZpX44+n)x3F{dAsvg`=|M?={#IrtrA=x+kX$+mCw!`
zUJkpv-gC)?^!Bo78qG$ngD}p#9+%@ZPQ1iV!&Wl!XEWXMC!<(IX*iYMKKI2@c>A?4
zdC(eM39hQc?Vfk>1%5NwQ{Rj`>ZjFY`^3F=QLj9M^9mX;F}fJ?5FvoVz+j7eIXNLc
z@~Y#}!Yr)Z%3(~!-JIrdOm#13_enqtV}LB6JEUOq#tV|;Y1AF0@cvS@)PJBr0)k_W
zz5tx^OM`2Ob3MB>L+&T@($*=-Waw6erRyw8`LwhX5$T7Xmh5=qk4p0>&Sq%|NeK&~
z{a=EunS-!{A#WfMV7MD#gM*nu9od=lG~*c`=qtC93{v%JWy@}&7~R}dKUO->RbTCO
z<yQOfNG+--x0X#RmQF3fr|}z(T5V8lXlSXqb=_<ueWEsMZ#VnMBt;-_w)WpN{K4Uv
mpZ49FdThDsd#$tCK#|QDwGG_$C7NO#i>@gQOz3F4{@@?URf&NB

diff --git a/Search-based Planning/Search_2D/plotting.py b/Search-based Planning/Search_2D/plotting.py
index 1121d06..fc6ea9b 100644
--- a/Search-based Planning/Search_2D/plotting.py	
+++ b/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)
 
diff --git a/Search-based Planning/Search_2D/queue.py b/Search-based Planning/Search_2D/queue.py
index 2721920..2a9fb3b 100644
--- a/Search-based Planning/Search_2D/queue.py	
+++ b/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