reformat

Search-based Planning/.idea/workspace.xml

@@ -22,9 +22,7 @@
     <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$/a_star.py" beforeDir="false" afterPath="$PROJECT_DIR$/a_star.py" afterDir="false" />
-      <change beforePath="$PROJECT_DIR$/bfs.py" beforeDir="false" afterPath="$PROJECT_DIR$/bfs.py" afterDir="false" />
-      <change beforePath="$PROJECT_DIR$/dfs.py" beforeDir="false" afterPath="$PROJECT_DIR$/dfs.py" afterDir="false" />
-      <change beforePath="$PROJECT_DIR$/dijkstra.py" beforeDir="false" afterPath="$PROJECT_DIR$/dijkstra.py" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/ara_star.py" beforeDir="false" afterPath="$PROJECT_DIR$/ara_star.py" afterDir="false" />
       <change beforePath="$PROJECT_DIR$/plotting.py" beforeDir="false" afterPath="$PROJECT_DIR$/plotting.py" afterDir="false" />
     </list>
     <option name="SHOW_DIALOG" value="false" />
@@ -205,22 +203,22 @@
     </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="1592802293738" />
     <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="296" key="GridCell.Tab.0.bottom" timestamp="1592801972746">
+    <state width="1832" height="296" key="GridCell.Tab.0.bottom" timestamp="1593123745200">
       <screen x="1920" y="0" width="1920" height="1080" />
     </state>
-    <state width="1832" height="296" key="GridCell.Tab.0.bottom/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1592801972746" />
-    <state width="1832" height="296" key="GridCell.Tab.0.center" timestamp="1592801972746">
+    <state width="1832" height="296" key="GridCell.Tab.0.bottom/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1593123745200" />
+    <state width="1832" height="296" key="GridCell.Tab.0.center" timestamp="1593123745200">
       <screen x="1920" y="0" width="1920" height="1080" />
     </state>
-    <state width="1832" height="296" key="GridCell.Tab.0.center/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1592801972746" />
-    <state width="1832" height="296" key="GridCell.Tab.0.left" timestamp="1592801972746">
+    <state width="1832" height="296" key="GridCell.Tab.0.center/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1593123745200" />
+    <state width="1832" height="296" key="GridCell.Tab.0.left" timestamp="1593123745199">
       <screen x="1920" y="0" width="1920" height="1080" />
     </state>
-    <state width="1832" height="296" key="GridCell.Tab.0.left/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1592801972746" />
-    <state width="1832" height="296" key="GridCell.Tab.0.right" timestamp="1592801972746">
+    <state width="1832" height="296" key="GridCell.Tab.0.left/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1593123745199" />
+    <state width="1832" height="296" key="GridCell.Tab.0.right" timestamp="1593123745200">
       <screen x="1920" y="0" width="1920" height="1080" />
     </state>
-    <state width="1832" height="296" key="GridCell.Tab.0.right/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1592801972746" />
+    <state width="1832" height="296" key="GridCell.Tab.0.right/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1593123745200" />
     <state x="2406" y="174" key="SettingsEditor" timestamp="1592801555194">
       <screen x="1920" y="0" width="1920" height="1080" />
     </state>

Search-based Planning/a_star.py

@@ -4,29 +4,30 @@ import env
 
 
 class Astar:
-    def __init__(self, x_start, x_goal, heuristic_type):
+    def __init__(self, x_start, x_goal, e, heuristic_type):
         self.xI, self.xG = x_start, x_goal
+        self.e = e
+        self.heuristic_type = heuristic_type
 
         self.Env = env.Env()  # class Env
-        self.plotting = plotting.Plotting(self.xI, self.xG)  # class Plotting
 
         self.u_set = self.Env.motions  # feasible input set
         self.obs = self.Env.obs  # position of obstacles
 
-        self.path, self.policy, self.visited = self.searching(self.xI, self.xG, heuristic_type)
-
-        self.fig_name = "A* Algorithm"
-        self.plotting.animation(self.path, self.visited, self.fig_name)  # animation generate
-
-    def searching(self, xI, xG, heuristic_type):
+    def searching(self):
         """
         Searching using A_star.
 
         :return: planning path, action in each node, visited nodes in the planning process
         """
 
-        q_astar = queue.QueuePrior()  # priority queue
-        q_astar.put(xI, 0)
+        xI = self.xI
+        xG = self.xG
+        heuristic_type = self.heuristic_type
+        e = self.e
+
+        q_astar = queue.QueuePrior()  # priority queue / OPEN
+        q_astar.put(xI, e * self.Heuristic(xI, xG, heuristic_type))
         parent = {xI: xI}  # record parents of nodes
         action = {xI: (0, 0)}  # record actions of nodes
         visited = []
@@ -43,7 +44,7 @@ class Astar:
                     new_cost = cost[x_current] + self.get_cost(x_current, u_next)
                     if x_next not in cost or new_cost < cost[x_next]:  # conditions for updating cost
                         cost[x_next] = new_cost
-                        priority = new_cost + self.Heuristic(x_next, xG, heuristic_type)
+                        priority = new_cost + e * self.Heuristic(x_next, xG, heuristic_type)
                         q_astar.put(x_next, priority)  # put node into queue using priority "f+h"
                         parent[x_next], action[x_next] = x_current, u_next
 
@@ -108,8 +109,18 @@ class Astar:
             print("Please choose right heuristic type!")
 
 
-if __name__ == '__main__':
-    x_Start = (5, 5)  # Starting node
-    x_Goal = (49, 5)  # Goal node
+def main():
+    x_start = (5, 5)  # Starting node
+    x_goal = (49, 5)  # Goal node
+
+    astar = Astar(x_start, x_goal, 1, "manhattan")
+    plot = plotting.Plotting(x_start, x_goal)  # class Plotting
 
-    astar = Astar(x_Start, x_Goal, "manhattan")
+    fig_name = "A* Algorithm"
+    path, policy, visited = astar.searching()
+
+    plot.animation(path, visited, fig_name)  # animation generate
+
+
+if __name__ == '__main__':
+    main()

Search-based Planning/ara_star.py

@@ -0,0 +1,21 @@
+import queue
+import plotting
+import env
+
+
+class AraStar:
+    def __init__(self, x_start, x_goal):
+        self.xI, self.xG = x_start, x_goal
+
+        self.Env = env.Env()  # class Env
+
+        self.u_set = self.Env.motions  # feasible input set
+        self.obs = self.Env.obs  # position of obstacles
+
+
+
+def main():
+    x_start = (5, 5)  # Starting node
+    x_goal = (49, 5)  # Goal node
+
+    arastar = AraStar(x_start, x_goal)

Search-based Planning/plotting.py

@@ -30,8 +30,8 @@ class Plotting:
         for x in visited:
             count += 1
             plt.plot(x[0], x[1], linewidth='3', color='#808080', marker='o')
-            plt.gcf().canvas.mpl_connect('key_release_event', lambda event:
-            [exit(0) if event.key == 'escape' else None])
+            plt.gcf().canvas.mpl_connect('key_release_event',
+                                         lambda event: [exit(0) if event.key == 'escape' else None])
 
             if count < len(visited) / 3:
                 length = 15