update

Search-based Planning/.idea/workspace.xml

@@ -21,6 +21,12 @@
   <component name="ChangeListManager">
     <list default="true" id="025aff36-a6aa-4945-ab7e-b2c625055f47" name="Default Changelist" comment="">
       <change beforePath="$PROJECT_DIR$/.idea/workspace.xml" beforeDir="false" afterPath="$PROJECT_DIR$/.idea/workspace.xml" 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/LRTAstar.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/LRTAstar.py" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/Search_2D/RTAAstar.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/RTAAstar.py" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/Search_2D/env.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/env.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/test.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/test.py" afterDir="false" />
     </list>
     <option name="EXCLUDED_CONVERTED_TO_IGNORED" value="true" />
     <option name="SHOW_DIALOG" value="false" />
@@ -66,7 +72,7 @@
       </list>
     </option>
   </component>
-  <component name="RunManager" selected="Python.bfs">
+  <component name="RunManager" selected="Python.RTAAstar">
     <configuration name="LPAstar" type="PythonConfigurationType" factoryName="Python" temporary="true">
       <module name="Search-based Planning" />
       <option name="INTERPRETER_OPTIONS" value="" />
@@ -88,7 +94,7 @@
       <option name="INPUT_FILE" value="" />
       <method v="2" />
     </configuration>
-    <configuration name="LRT_Astar3D" type="PythonConfigurationType" factoryName="Python" temporary="true">
+    <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" />
@@ -96,11 +102,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/LRTAstar.py" />
       <option name="PARAMETERS" value="" />
       <option name="SHOW_COMMAND_LINE" value="false" />
       <option name="EMULATE_TERMINAL" value="false" />
@@ -109,7 +115,7 @@
       <option name="INPUT_FILE" value="" />
       <method v="2" />
     </configuration>
-    <configuration name="astar" 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" />
@@ -121,7 +127,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/RTAAstar.py" />
       <option name="PARAMETERS" value="" />
       <option name="SHOW_COMMAND_LINE" value="false" />
       <option name="EMULATE_TERMINAL" value="false" />
@@ -195,19 +201,19 @@
     </configuration>
     <list>
       <item itemvalue="Python.dijkstra" />
-      <item itemvalue="Python.astar" />
-      <item itemvalue="Python.LRT_Astar3D" />
       <item itemvalue="Python.test" />
       <item itemvalue="Python.LPAstar" />
       <item itemvalue="Python.bfs" />
+      <item itemvalue="Python.LRTAstar" />
+      <item itemvalue="Python.RTAAstar" />
     </list>
     <recent_temporary>
       <list>
-        <item itemvalue="Python.bfs" />
+        <item itemvalue="Python.RTAAstar" />
+        <item itemvalue="Python.LRTAstar" />
         <item itemvalue="Python.LPAstar" />
+        <item itemvalue="Python.bfs" />
         <item itemvalue="Python.test" />
-        <item itemvalue="Python.astar" />
-        <item itemvalue="Python.LRT_Astar3D" />
       </list>
     </recent_temporary>
   </component>

Search-based Planning/Search_2D/LPAstar.py

@@ -34,12 +34,38 @@ class LpaStar:
                 self.g[(i, j)] = float("inf")
 
         self.rhs[self.xI] = 0
-        self.U.put(self.xI, [self.h(self.xI), 0])
+        self.U.put(self.xI, self.CalculateKey(self.xI))
 
     def searching(self):
         self.computePath()
-        path = self.extract_path()
-        return path
+        path = [self.extract_path()]
+
+        obs_change = set()
+        for j in range(14, 15):
+            self.obs.add((30, j))
+            obs_change.add((30, j))
+        for s in obs_change:
+            self.rhs[s] = float("inf")
+            self.g[s] = float("inf")
+            for x in self.get_neighbor(s):
+                self.UpdateVertex(x)
+        # for x in obs_change:
+        #     self.obs.remove(x)
+        # for x in obs_change:
+        #     self.UpdateVertex(x)
+        print(self.g[(29, 15)])
+        print(self.g[(29, 14)])
+        print(self.g[(29, 13)])
+        print(self.g[(30, 13)])
+        print(self.g[(31, 13)])
+        print(self.g[(32, 13)])
+        print(self.g[(33, 13)])
+        print(self.g[(34, 13)])
+
+        self.computePath()
+        path.append(self.extract_path_test())
+
+        return path, obs_change
 
     def computePath(self):
         while self.U.top_key() < self.CalculateKey(self.xG) \
@@ -47,13 +73,12 @@ class LpaStar:
             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)
+            for x in self.get_neighbor(s):
+                self.UpdateVertex(x)
+        # return self.extract_path()
 
     def extract_path(self):
         path = []
@@ -68,6 +93,18 @@ class LpaStar:
                 return list(reversed(path))
             path.append(s)
 
+    def extract_path_test(self):
+        path = []
+        s = self.xG
+
+        for k in range(30):
+            g_list = {}
+            for x in self.get_neighbor(s):
+                g_list[x] = self.g[x]
+            s = min(g_list, key=g_list.get)
+            path.append(s)
+        return list(reversed(path))
+
     def get_neighbor(self, s):
         nei_list = set()
         for u in self.u_set:
@@ -85,7 +122,7 @@ class LpaStar:
         if u != self.xI:
             u_min = float("inf")
             for x in self.get_neighbor(u):
-                u_min = min(u_min, self.g[x] + 1)
+                u_min = min(u_min, self.g[x] + self.get_cost(u, x))
             self.rhs[u] = u_min
         self.U.check_remove(u)
         if self.g[u] != self.rhs[u]:
@@ -102,32 +139,45 @@ class LpaStar:
         else:
             print("Please choose right heuristic type!")
 
-    @staticmethod
-    def get_cost(x, u):
+    def get_cost(self, s_start, s_end):
         """
         Calculate cost for this motion
 
-        :param x: current node
-        :param u: current input
+        :param s_start:
+        :param s_end:
         :return:  cost for this motion
         :note: cost function could be more complicate!
         """
 
-        return 1
+        if s_start not in self.obs:
+            if s_end not in self.obs:
+                return 1
+            else:
+                return float("inf")
+        return float("inf")
 
 
 def main():
     x_start = (5, 5)
     x_goal = (45, 25)
 
-    lpastar = LpaStar(x_start, x_goal, "manhattan")
+    lpastar = LpaStar(x_start, x_goal, "euclidean")
     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')
+    path, obs = lpastar.searching()
+
+    plot.plot_grid("Lifelong Planning A*")
+    p = path[0]
+    px = [x[0] for x in p]
+    py = [x[1] for x in p]
+    plt.plot(px, py, marker='o')
+    plt.pause(0.5)
+
+    p = path[1]
+    px = [x[0] for x in p]
+    py = [x[1] for x in p]
+    plt.plot(px, py, marker='o')
+    plt.pause(0.01)
     plt.show()
 
 

Search-based Planning/Search_2D/LRTAstar.py

@@ -29,6 +29,11 @@ class LrtAstarN:
         self.N = N                                      # number of expand nodes each iteration
         self.visited = []                               # order of visited nodes in planning
         self.path = []                                  # path of each iteration
+        self.h_table = {}
+
+        for i in range(self.Env.x_range):
+            for j in range(self.Env.y_range):
+                self.h_table[(i, j)] = self.h((i, j))   # initialize h_value
 
     def searching(self):
         s_start = self.xI                               # initialize start node
@@ -41,6 +46,10 @@ class LrtAstarN:
                 break
 
             h_value = self.iteration(CLOSED)            # h_value table of CLOSED nodes
+
+            for x in h_value:
+                self.h_table[x] = h_value[x]
+
             s_start, path_k = self.extract_path_in_CLOSE(s_start, h_value)      # s_start -> expected node in OPEN set
             self.path.append(path_k)
 
@@ -56,7 +65,7 @@ class LrtAstarN:
                     if s_next in h_value:
                         h_list[s_next] = h_value[s_next]
                     else:
-                        h_list[s_next] = self.h(s_next)
+                        h_list[s_next] = self.h_table[s_next]
             s_key = min(h_list, key=h_list.get)                 # move to the smallest node with min h_value
             path.append(s_key)                                  # generate path
             s = s_key                                           # use end of this iteration as the start of next
@@ -78,7 +87,7 @@ class LrtAstarN:
                     s_next = tuple([s[i] + u[i] for i in range(2)])
                     if s_next not in self.obs:
                         if s_next not in CLOSED:
-                            h_list.append(self.get_cost(s, s_next) + self.h(s_next))
+                            h_list.append(self.get_cost(s, s_next) + self.h_table[s_next])
                         else:
                             h_list.append(self.get_cost(s, s_next) + h_value[s_next])
                 h_value[s] = min(h_list)                        # update h_value of current node
@@ -114,7 +123,7 @@ class LrtAstarN:
                     if new_cost < g_table[s_next]:                           # conditions for updating cost
                         g_table[s_next] = new_cost
                         PARENT[s_next] = s
-                        OPEN.put(s_next, g_table[s_next] + self.h(s_next))
+                        OPEN.put(s_next, g_table[s_next] + self.h_table[s_next])
 
             if count == N:                                                   # expand needed CLOSED nodes
                 break
@@ -171,7 +180,7 @@ def main():
     x_start = (10, 5)
     x_goal = (45, 25)
 
-    lrta = LrtAstarN(x_start, x_goal, 220, "euclidean")
+    lrta = LrtAstarN(x_start, x_goal, 100, "euclidean")
     plot = plotting.Plotting(x_start, x_goal)
     fig_name = "Learning Real-time A* (LRTA*)"
 

Search-based Planning/Search_2D/RTAAstar.py

@@ -29,6 +29,11 @@ class RtaAstar:
         self.N = N                                      # number of expand nodes each iteration
         self.visited = []                               # order of visited nodes in planning
         self.path = []                                  # path of each iteration
+        self.h_table = {}
+
+        for i in range(self.Env.x_range):
+            for j in range(self.Env.y_range):
+                self.h_table[(i, j)] = self.h((i, j))   # initialize h_value
 
     def searching(self):
         s_start = self.xI                               # initialize start node
@@ -42,6 +47,10 @@ class RtaAstar:
                 break
 
             s_next, h_value = self.cal_h_value(OPEN, CLOSED, g_table, PARENT)
+
+            for x in h_value:
+                self.h_table[x] = h_value[x]
+
             s_start, path_k = self.extract_path_in_CLOSE(s_start, s_next, h_value)
             self.path.append(path_k)
 
@@ -49,7 +58,7 @@ class RtaAstar:
         v_open = {}
         h_value = {}
         for (_, x) in OPEN.enumerate():
-            v_open[x] = g_table[PARENT[x]] + 1 + self.h(x)
+            v_open[x] = g_table[PARENT[x]] + 1 + self.h_table[x]
         s_open = min(v_open, key=v_open.get)
         f_min = min(v_open.values())
         for x in CLOSED:
@@ -88,7 +97,7 @@ class RtaAstar:
                     s_next = tuple([s[i] + u[i] for i in range(2)])
                     if s_next not in self.obs:
                         if s_next not in CLOSED:
-                            h_list.append(self.get_cost(s, s_next) + self.h(s_next))
+                            h_list.append(self.get_cost(s, s_next) + self.h_table[s_next])
                         else:
                             h_list.append(self.get_cost(s, s_next) + h_value[s_next])
                 h_value[s] = min(h_list)                        # update h_value of current node
@@ -124,7 +133,7 @@ class RtaAstar:
                     if new_cost < g_table[s_next]:                           # conditions for updating cost
                         g_table[s_next] = new_cost
                         PARENT[s_next] = s
-                        OPEN.put(s_next, g_table[s_next] + self.h(s_next))
+                        OPEN.put(s_next, g_table[s_next] + self.h_table[s_next])
 
             if count == N:                                                   # expand needed CLOSED nodes
                 break

Search-based Planning/Search_2D/env.py

@@ -20,26 +20,26 @@ class Env:
 
         x = self.x_range
         y = self.y_range
-        obs = []
+        obs = set()
 
         for i in range(x):
-            obs.append((i, 0))
+            obs.add((i, 0))
         for i in range(x):
-            obs.append((i, y - 1))
+            obs.add((i, y - 1))
 
         for i in range(y):
-            obs.append((0, i))
+            obs.add((0, i))
         for i in range(y):
-            obs.append((x - 1, i))
+            obs.add((x - 1, i))
 
         for i in range(10, 21):
-            obs.append((i, 15))
+            obs.add((i, 15))
         for i in range(15):
-            obs.append((20, i))
+            obs.add((20, i))
 
         for i in range(15, 30):
-            obs.append((30, i))
+            obs.add((30, i))
         for i in range(16):
-            obs.append((40, i))
+            obs.add((40, i))
 
         return obs

Search-based Planning/Search_2D/plotting.py

@@ -59,8 +59,8 @@ class Plotting:
         plt.show()
 
     def plot_grid(self, name):
-        obs_x = [self.obs[i][0] for i in range(len(self.obs))]
-        obs_y = [self.obs[i][1] for i in range(len(self.obs))]
+        obs_x = [x[0] for x in self.obs]
+        obs_y = [x[1] for x in self.obs]
 
         plt.plot(self.xI[0], self.xI[1], "bs")
         plt.plot(self.xG[0], self.xG[1], "gs")

Search-based Planning/Search_2D/test.py

@@ -17,4 +17,4 @@ from Search_2D import env
 U = queue.QueuePrior()
 U.put((1, 2), [2, 3])
 U.put((2, 3), [1, 5])
-print(U.get())
+print(U.get())