add repeated A*

Search-based Planning/.idea/workspace.xml

@@ -19,7 +19,13 @@
     <select />
   </component>
   <component name="ChangeListManager">
-    <list default="true" id="025aff36-a6aa-4945-ab7e-b2c625055f47" name="Default Changelist" comment="" />
+    <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/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" />
     <option name="HIGHLIGHT_CONFLICTS" value="true" />
@@ -64,7 +70,7 @@
       </list>
     </option>
   </component>
-  <component name="RunManager" selected="Python.ARAstar">
+  <component name="RunManager" selected="Python.astar">
     <configuration name="ARAstar" type="PythonConfigurationType" factoryName="Python" temporary="true">
       <module name="Search-based Planning" />
       <option name="INTERPRETER_OPTIONS" value="" />
@@ -149,7 +155,7 @@
       <option name="INPUT_FILE" value="" />
       <method v="2" />
     </configuration>
-    <configuration name="dfs" type="PythonConfigurationType" factoryName="Python" temporary="true">
+    <configuration name="astar" type="PythonConfigurationType" factoryName="Python" temporary="true">
       <module name="Search-based Planning" />
       <option name="INTERPRETER_OPTIONS" value="" />
       <option name="PARENT_ENVS" value="true" />
@@ -161,7 +167,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/dfs.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" />
@@ -193,19 +199,19 @@
     </configuration>
     <list>
       <item itemvalue="Python.dijkstra" />
-      <item itemvalue="Python.dfs" />
       <item itemvalue="Python.LRTA_star" />
       <item itemvalue="Python.LRTAstar" />
       <item itemvalue="Python.RTAAstar" />
       <item itemvalue="Python.ARAstar" />
+      <item itemvalue="Python.astar" />
     </list>
     <recent_temporary>
       <list>
+        <item itemvalue="Python.astar" />
         <item itemvalue="Python.ARAstar" />
         <item itemvalue="Python.RTAAstar" />
         <item itemvalue="Python.LRTAstar" />
         <item itemvalue="Python.LRTA_star" />
-        <item itemvalue="Python.dfs" />
       </list>
     </recent_temporary>
   </component>

Search-based Planning/Search_2D/ARAstar.py

@@ -152,9 +152,9 @@ class AraStar:
 
 def main():
     x_start = (5, 5)  # Starting node
-    x_goal = (45, 5)  # Goal node
+    x_goal = (45, 25)  # Goal node
 
-    arastar = AraStar(x_start, x_goal, 2.5, "euclidean")
+    arastar = AraStar(x_start, x_goal, 2.5, "manhattan")
     plot = plotting.Plotting(x_start, x_goal)
 
     fig_name = "Anytime Repairing A* (ARA*)"

Search-based Planning/Search_2D/astar.py

@@ -19,17 +19,17 @@ class Astar:
         self.xI, self.xG = x_start, x_goal
         self.heuristic_type = heuristic_type
 
-        self.Env = env.Env()                                # class Env
+        self.Env = env.Env()  # class Env
 
-        self.e = e                                          # weighted A*: e >= 1
-        self.u_set = self.Env.motions                       # feasible input set
-        self.obs = self.Env.obs                             # position of obstacles
+        self.e = e  # weighted A*: e >= 1
+        self.u_set = self.Env.motions  # feasible input set
+        self.obs = self.Env.obs  # position of obstacles
 
-        self.g = {self.xI: 0, self.xG: float("inf")}        # cost to come
-        self.OPEN = queue.QueuePrior()                      # priority queue / OPEN set
+        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.PARENT = {self.xI: self.xI}                    # relations
+        self.CLOSED = []  # closed set & visited
+        self.PARENT = {self.xI: self.xI}  # relations
 
     def searching(self):
         """
@@ -42,10 +42,10 @@ class Astar:
             s = self.OPEN.get()
             self.CLOSED.append(s)
 
-            if s == self.xG:                                                # stop condition
+            if s == self.xG:  # stop condition
                 break
 
-            for u in self.u_set:                                       # explore neighborhoods of current node
+            for u in self.u_set:  # explore neighborhoods of current node
                 s_next = tuple([s[i] + u[i] for i in range(2)])
                 if s_next not in self.obs and s_next not in self.CLOSED:
                     new_cost = self.g[s] + self.get_cost(s, u)
@@ -56,18 +56,58 @@ class Astar:
                         self.PARENT[s_next] = s
                         self.OPEN.put(s_next, self.fvalue(s_next))
 
-        return self.extract_path(), self.CLOSED
+        return self.extract_path(self.PARENT), self.CLOSED
 
-    def fvalue(self, x):
+    def repeated_Searching(self, xI, xG, e):
+        path, visited = [], []
+
+        while e >= 1:
+            p_k, v_k = self.repeated_Astar(xI, xG, e)
+            path.append(p_k)
+            visited.append(v_k)
+            e -= 0.5
+
+        return path, visited
+
+    def repeated_Astar(self, xI, xG, e):
+        g = {xI: 0, xG: float("inf")}
+        OPEN = queue.QueuePrior()
+        OPEN.put(xI, g[xI] + e * self.Heuristic(xI))
+        CLOSED = set()
+        PARENT = {xI: xI}
+        VISITED = []
+
+        while OPEN:
+            s = OPEN.get()
+            CLOSED.add(s)
+            VISITED.append(s)
+
+            if s == xG:
+                break
+
+            for u in self.u_set:  # explore neighborhoods of current node
+                s_next = tuple([s[i] + u[i] for i in range(2)])
+                if s_next not in self.obs and s_next not in CLOSED:
+                    new_cost = g[s] + self.get_cost(s, u)
+                    if s_next not in g:
+                        g[s_next] = float("inf")
+                    if new_cost < g[s_next]:  # conditions for updating cost
+                        g[s_next] = new_cost
+                        PARENT[s_next] = s
+                        OPEN.put(s_next, g[s_next] + e * self.Heuristic(s_next))
+
+        return self.extract_path(PARENT), VISITED
+
+    def fvalue(self, x, e=1):
         """
         f = g + h. (g: cost to come, h: heuristic function)
         :param x: current state
         :return: f
         """
 
-        return self.g[x] + self.e * self.Heuristic(x)
+        return self.g[x] + e * self.Heuristic(x)
 
-    def extract_path(self):
+    def extract_path(self, PARENT):
         """
         Extract the path based on the relationship of nodes.
 
@@ -78,7 +118,7 @@ class Astar:
         x_current = self.xG
 
         while True:
-            x_current = self.PARENT[x_current]
+            x_current = PARENT[x_current]
             path_back.append(x_current)
 
             if x_current == self.xI:
@@ -107,8 +147,8 @@ class Astar:
         :return: heuristic function value
         """
 
-        heuristic_type = self.heuristic_type                    # heuristic type
-        goal = self.xG                                          # goal node
+        heuristic_type = self.heuristic_type  # heuristic type
+        goal = self.xG  # goal node
 
         if heuristic_type == "manhattan":
             return abs(goal[0] - state[0]) + abs(goal[1] - state[1])
@@ -122,12 +162,16 @@ def main():
     x_start = (5, 5)
     x_goal = (45, 25)
 
-    astar = Astar(x_start, x_goal, 1, "euclidean")              # weight e = 1
-    plot = plotting.Plotting(x_start, x_goal)                   # class Plotting
+    astar = Astar(x_start, x_goal, 1, "manhattan")  # 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 = "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__':

Search-based Planning/Search_2D/plotting.py

@@ -138,7 +138,7 @@ class Plotting:
         cl_v = ['silver',
                 'wheat',
                 'lightskyblue',
-                'plum',
+                'royalblue',
                 'slategray']
         cl_p = ['gray',
                 'orange',