update

Search-based Planning/.idea/shelf/Uncommitted_changes_before_Update_at_6_27_20__12_26_AM__Default_Changelist_.xml

@@ -1,4 +0,0 @@
-<changelist name="Uncommitted_changes_before_Update_at_6_27_20,_12_26_AM_[Default_Changelist]" date="1593242806212" recycled="true" deleted="true">
-  <option name="PATH" value="$PROJECT_DIR$/.idea/shelf/Uncommitted_changes_before_Update_at_6_27_20,_12_26_AM_[Default_Changelist]/shelved.patch" />
-  <option name="DESCRIPTION" value="Uncommitted changes before Update at 6/27/20, 12:26 AM [Default Changelist]" />
-</changelist>

Search-based Planning/.idea/workspace.xml

@@ -20,7 +20,31 @@
   </component>
   <component name="ChangeListManager">
     <list default="true" id="025aff36-a6aa-4945-ab7e-b2c625055f47" name="Default Changelist" comment="">
+      <change afterPath="$PROJECT_DIR$/Search_2D/FieldD_star.py" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/.idea/shelf/Uncommitted_changes_before_Update_at_6_27_20,_12_26_AM_[Default_Changelist]/shelved.patch" beforeDir="false" />
+      <change beforePath="$PROJECT_DIR$/.idea/shelf/Uncommitted_changes_before_Update_at_6_27_20__12_26_AM__Default_Changelist_.xml" beforeDir="false" />
       <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/D_star.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/D_star.py" 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/astar.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/astar.py" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/Search_2D/bfs.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/bfs.py" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/Search_2D/bidirectional_a_star.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/bidirectional_a_star.py" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/Search_2D/dfs.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/dfs.py" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/Search_2D/dijkstra.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/dijkstra.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$/gif/ARA_star.gif" beforeDir="false" afterPath="$PROJECT_DIR$/gif/ARA_star.gif" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/gif/Astar.gif" beforeDir="false" afterPath="$PROJECT_DIR$/gif/Astar.gif" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/gif/BFS.gif" beforeDir="false" afterPath="$PROJECT_DIR$/gif/BFS.gif" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/gif/Bi-Astar.gif" beforeDir="false" afterPath="$PROJECT_DIR$/gif/Bi-Astar.gif" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/gif/DFS.gif" beforeDir="false" afterPath="$PROJECT_DIR$/gif/DFS.gif" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/gif/Dijkstra.gif" beforeDir="false" afterPath="$PROJECT_DIR$/gif/Dijkstra.gif" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/gif/LRTA_star.gif" beforeDir="false" afterPath="$PROJECT_DIR$/gif/LRTA_star.gif" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/gif/RTAA_star.gif" beforeDir="false" />
+      <change beforePath="$PROJECT_DIR$/gif/RepeatedA_star.gif" beforeDir="false" afterPath="$PROJECT_DIR$/gif/RepeatedA_star.gif" afterDir="false" />
     </list>
     <option name="EXCLUDED_CONVERTED_TO_IGNORED" value="true" />
     <option name="SHOW_DIALOG" value="false" />
@@ -50,7 +74,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="run.code.analysis.last.selected.profile" value="aDefault" />
     <property name="settings.editor.selected.configurable" value="com.jetbrains.python.configuration.PyActiveSdkModuleConfigurable" />
@@ -72,7 +96,7 @@
       </list>
     </option>
   </component>
-  <component name="RunManager" selected="Python.D_star">
+  <component name="RunManager" selected="Python.LPAstar">
     <configuration name="ARAstar" type="PythonConfigurationType" factoryName="Python" temporary="true">
       <module name="Search-based Planning" />
       <option name="INTERPRETER_OPTIONS" value="" />
@@ -136,7 +160,7 @@
       <option name="INPUT_FILE" value="" />
       <method v="2" />
     </configuration>
-    <configuration name="RTAAstar" 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" />
@@ -148,7 +172,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/LRTAstar.py" />
       <option name="PARAMETERS" value="" />
       <option name="SHOW_COMMAND_LINE" value="false" />
       <option name="EMULATE_TERMINAL" value="false" />
@@ -157,7 +181,7 @@
       <option name="INPUT_FILE" value="" />
       <method v="2" />
     </configuration>
-    <configuration name="bidirectional_a_star" 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" />
@@ -169,7 +193,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/bidirectional_a_star.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" />
@@ -201,19 +225,19 @@
     </configuration>
     <list>
       <item itemvalue="Python.dijkstra" />
-      <item itemvalue="Python.ARAstar" />
-      <item itemvalue="Python.LPAstar" />
+      <item itemvalue="Python.LRTAstar" />
       <item itemvalue="Python.RTAAstar" />
-      <item itemvalue="Python.bidirectional_a_star" />
+      <item itemvalue="Python.ARAstar" />
       <item itemvalue="Python.D_star" />
+      <item itemvalue="Python.LPAstar" />
     </list>
     <recent_temporary>
       <list>
+        <item itemvalue="Python.LPAstar" />
         <item itemvalue="Python.D_star" />
-        <item itemvalue="Python.bidirectional_a_star" />
         <item itemvalue="Python.ARAstar" />
-        <item itemvalue="Python.LPAstar" />
         <item itemvalue="Python.RTAAstar" />
+        <item itemvalue="Python.LRTAstar" />
       </list>
     </recent_temporary>
   </component>

Search-based Planning/Search_2D/ARAstar.py

@@ -153,8 +153,7 @@ class AraStar:
         else:
             return math.hypot(goal[0] - s[0], goal[1] - s[1])
 
-    @staticmethod
-    def cost(s_start, s_goal):
+    def cost(self, s_start, s_goal):
         """
         Calculate cost for this motion
         :param s_start: starting node
@@ -163,7 +162,27 @@ class AraStar:
         :note: cost function could be more complicate!
         """
 
-        return 1
+        if self.is_collision(s_start, s_goal):
+            return float("inf")
+
+        return math.hypot(s_goal[0] - s_start[0], s_goal[1] - s_start[1])
+
+    def is_collision(self, s_start, s_end):
+        if s_start in self.obs or s_end in self.obs:
+            return True
+
+        if s_start[0] != s_end[0] and s_start[1] != s_end[1]:
+            if s_end[0] - s_start[0] == s_start[1] - s_end[1]:
+                s1 = (min(s_start[0], s_end[0]), min(s_start[1], s_end[1]))
+                s2 = (max(s_start[0], s_end[0]), max(s_start[1], s_end[1]))
+            else:
+                s1 = (min(s_start[0], s_end[0]), max(s_start[1], s_end[1]))
+                s2 = (max(s_start[0], s_end[0]), min(s_start[1], s_end[1]))
+
+            if s1 in self.obs or s2 in self.obs:
+                return True
+
+        return False
 
 
 def main():

Search-based Planning/Search_2D/D_star.py

@@ -5,6 +5,7 @@ D_star 2D
 
 import os
 import sys
+import math
 import matplotlib.pyplot as plt
 
 sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
@@ -15,11 +16,11 @@ from Search_2D import env
 
 
 class Dstar:
-    def __init__(self, x_start, x_goal):
-        self.xI, self.xG = x_start, x_goal
+    def __init__(self, s_start, s_goal):
+        self.s_start, self.s_goal = s_start, s_goal
 
         self.Env = env.Env()
-        self.Plot = plotting.Plotting(self.xI, self.xG)
+        self.Plot = plotting.Plotting(self.s_start, self.s_goal)
 
         self.u_set = self.Env.motions
         self.obs = self.Env.obs
@@ -30,16 +31,21 @@ class Dstar:
         self.OPEN = set()
         self.t = {}
         self.PARENT = {}
-        self.h = {self.xG: 0}
+        self.h = {}
         self.k = {}
         self.path = []
+        self.visited = []
+        self.count = 0
 
         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.k[(i, j)] = 0.0
+                self.h[(i, j)] = float("inf")
                 self.PARENT[(i, j)] = None
 
+        self.h[self.s_goal] = 0.0
+
     def run(self, s_start, s_end):
         self.insert(s_end, 0)
         while True:
@@ -61,28 +67,31 @@ class Dstar:
             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)
+            s = self.s_start
+            self.visited = []
+            while s != self.s_goal:
+                if self.is_collision(s, self.PARENT[s]):
+                    self.modify(s)
+                    continue
+                s = self.PARENT[s]
+            self.path = self.extract_path(self.s_start, self.s_goal)
+            self.plot_visited(self.visited)
             self.plot_path(self.path)
+            self.count += 1
             self.fig.canvas.draw_idle()
 
     def extract_path(self, s_start, s_end):
-        path = []
+        path = [s_start]
         s = s_start
         while True:
             s = self.PARENT[s]
+            path.append(s)
             if s == s_end:
                 return path
-            path.append(s)
 
     def process_state(self):
         s = self.min_state()
+        self.visited.append(s)
         if s is None:
             return -1
         k_old = self.get_k_min()
@@ -156,6 +165,7 @@ class Dstar:
 
     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:
@@ -163,16 +173,55 @@ class Dstar:
 
         return nei_list
 
-    def cost(self, s_start, s_end):
-        if s_start in self.obs or s_end in self.obs:
+    def cost(self, s_start, s_goal):
+        """
+        Calculate cost for this motion
+        :param s_start: starting node
+        :param s_goal: end node
+        :return:  cost for this motion
+        :note: cost function could be more complicate!
+        """
+
+        if self.is_collision(s_start, s_goal):
             return float("inf")
-        return 1
 
-    @staticmethod
-    def plot_path(path):
+        return math.hypot(s_goal[0] - s_start[0], s_goal[1] - s_start[1])
+
+    def is_collision(self, s_start, s_end):
+        if s_start in self.obs or s_end in self.obs:
+            return True
+
+        if s_start[0] != s_end[0] and s_start[1] != s_end[1]:
+            if s_end[0] - s_start[0] == s_start[1] - s_end[1]:
+                s1 = (min(s_start[0], s_end[0]), min(s_start[1], s_end[1]))
+                s2 = (max(s_start[0], s_end[0]), max(s_start[1], s_end[1]))
+            else:
+                s1 = (min(s_start[0], s_end[0]), max(s_start[1], s_end[1]))
+                s2 = (max(s_start[0], s_end[0]), min(s_start[1], s_end[1]))
+
+            if s1 in self.obs or s2 in self.obs:
+                return True
+
+        return False
+
+    def plot_path(self, path):
         px = [x[0] for x in path]
         py = [x[1] for x in path]
-        plt.plot(px, py, marker='o')
+        plt.plot(px, py, linewidth=2)
+        plt.plot(self.s_start[0], self.s_start[1], "bs")
+        plt.plot(self.s_goal[0], self.s_goal[1], "gs")
+
+    def plot_visited(self, visited):
+        color = ['gainsboro', 'lightgray', 'silver', 'darkgray',
+                 'bisque', 'navajowhite', 'moccasin', 'wheat',
+                 'powderblue', 'skyblue', 'lightskyblue', 'cornflowerblue']
+
+        if self.count >= len(color) - 1:
+            self.count = 0
+
+        for x in visited:
+            if x not in self.obs:
+                plt.plot(x[0], x[1], marker='s', color=color[self.count])
 
 
 def main():

Search-based Planning/Search_2D/LPAstar.py

@@ -28,8 +28,7 @@ class LpaStar:
         self.x = self.Env.x_range
         self.y = self.Env.y_range
 
-        self.U = {}
-        self.g, self.rhs = {}, {}
+        self.g, self.rhs, self.U = {}, {}, {}
 
         for i in range(self.Env.x_range):
             for j in range(self.Env.y_range):
@@ -38,13 +37,15 @@ class LpaStar:
 
         self.rhs[self.s_start] = 0
         self.U[self.s_start] = self.CalculateKey(self.s_start)
+        self.visited = []
+        self.count = 0
 
         self.fig = plt.figure()
 
     def run(self):
         self.Plot.plot_grid("Lifelong Planning A*")
 
-        self.ComputePath()
+        self.ComputeShortestPath()
         self.plot_path(self.extract_path())
         self.fig.canvas.mpl_connect('button_press_event', self.on_press)
 
@@ -57,6 +58,8 @@ class LpaStar:
         else:
             x, y = int(x), int(y)
             print("Change position: x =", x, ",", "y =", y)
+            self.visited = []
+            self.count += 1
             if (x, y) not in self.obs:
                 self.obs.add((x, y))
                 plt.plot(x, y, 'sk')
@@ -68,13 +71,15 @@ class LpaStar:
             for s_n in self.get_neighbor((x, y)):
                 self.UpdateVertex(s_n)
 
-            self.ComputePath()
+            self.ComputeShortestPath()
+            self.plot_visited(self.visited)
             self.plot_path(self.extract_path())
             self.fig.canvas.draw_idle()
 
-    def ComputePath(self):
+    def ComputeShortestPath(self):
         while True:
             s, v = self.TopKey()
+            self.visited.append(s)
             if v >= self.CalculateKey(self.s_goal) and \
                     self.rhs[self.s_goal] == self.g[self.s_goal]:
                 break
@@ -141,19 +146,36 @@ class LpaStar:
         else:
             return math.hypot(goal[0] - s[0], goal[1] - s[1])
 
-    def cost(self, s_start, s_end):
+    def cost(self, s_start, s_goal):
         """
-        calculate edge cost: (s_start, s_end)
-        :param s_start: start node
-        :param s_end: end node
-        :return: cost
+        Calculate cost for this motion
+        :param s_start: starting node
+        :param s_goal: end node
+        :return:  cost for this motion
+        :note: cost function could be more complicate!
         """
 
-        # if one of the vertex in obstacles: return infinity.
-        if s_start in self.obs or s_end in self.obs:
+        if self.is_collision(s_start, s_goal):
             return float("inf")
 
-        return 1
+        return math.hypot(s_goal[0] - s_start[0], s_goal[1] - s_start[1])
+
+    def is_collision(self, s_start, s_end):
+        if s_start in self.obs or s_end in self.obs:
+            return True
+
+        if s_start[0] != s_end[0] and s_start[1] != s_end[1]:
+            if s_end[0] - s_start[0] == s_start[1] - s_end[1]:
+                s1 = (min(s_start[0], s_end[0]), min(s_start[1], s_end[1]))
+                s2 = (max(s_start[0], s_end[0]), max(s_start[1], s_end[1]))
+            else:
+                s1 = (min(s_start[0], s_end[0]), max(s_start[1], s_end[1]))
+                s2 = (max(s_start[0], s_end[0]), min(s_start[1], s_end[1]))
+
+            if s1 in self.obs or s2 in self.obs:
+                return True
+
+        return False
 
     def extract_path(self):
         """
@@ -161,32 +183,46 @@ class LpaStar:
         :return: The planning path
         """
 
-        path = []
+        path = [self.s_goal]
         s = self.s_goal
 
         for k in range(100):
             g_list = {}
             for x in self.get_neighbor(s):
-                g_list[x] = self.g[x]
+                if not self.is_collision(s, x):
+                    g_list[x] = self.g[x]
             s = min(g_list, key=g_list.get)
+            path.append(s)
             if s == self.s_start:
                 break
-            path.append(s)
 
         return list(reversed(path))
 
-    @staticmethod
-    def plot_path(path):
+    def plot_path(self, path):
         px = [x[0] for x in path]
         py = [x[1] for x in path]
-        plt.plot(px, py, marker='o')
+        plt.plot(px, py, linewidth=2)
+        plt.plot(self.s_start[0], self.s_start[1], "bs")
+        plt.plot(self.s_goal[0], self.s_goal[1], "gs")
+
+    def plot_visited(self, visited):
+        color = ['gainsboro', 'lightgray', 'silver', 'darkgray',
+                 'bisque', 'navajowhite', 'moccasin', 'wheat',
+                 'powderblue', 'skyblue', 'lightskyblue', 'cornflowerblue']
+
+        if self.count >= len(color) - 1:
+            self.count = 0
+
+        for x in visited:
+            if x not in self.obs:
+                plt.plot(x[0], x[1], marker='s', color=color[self.count])
 
 
 def main():
     x_start = (5, 5)
     x_goal = (45, 25)
 
-    lpastar = LpaStar(x_start, x_goal, "manhattan")
+    lpastar = LpaStar(x_start, x_goal, "Euclidean")
     lpastar.run()
 
 

Search-based Planning/Search_2D/LRTAstar.py

@@ -132,12 +132,12 @@ class LrtAstarN:
         :return: neighbors
         """
 
-        s_list = set()
+        s_list = []
 
         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:
-                s_list.add(s_next)
+                s_list.append(s_next)
 
         return s_list
 
@@ -175,16 +175,36 @@ class LrtAstarN:
         else:
             return math.hypot(goal[0] - s[0], goal[1] - s[1])
 
-    def cost(self, s_start, s_end):
+    def cost(self, s_start, s_goal):
         """
         Calculate cost for this motion
         :param s_start: starting node
-        :param s_end: end node
+        :param s_goal: end node
         :return:  cost for this motion
         :note: cost function could be more complicate!
         """
 
-        return 1
+        if self.is_collision(s_start, s_goal):
+            return float("inf")
+
+        return math.hypot(s_goal[0] - s_start[0], s_goal[1] - s_start[1])
+
+    def is_collision(self, s_start, s_end):
+        if s_start in self.obs or s_end in self.obs:
+            return True
+
+        if s_start[0] != s_end[0] and s_start[1] != s_end[1]:
+            if s_end[0] - s_start[0] == s_start[1] - s_end[1]:
+                s1 = (min(s_start[0], s_end[0]), min(s_start[1], s_end[1]))
+                s2 = (max(s_start[0], s_end[0]), max(s_start[1], s_end[1]))
+            else:
+                s1 = (min(s_start[0], s_end[0]), max(s_start[1], s_end[1]))
+                s2 = (max(s_start[0], s_end[0]), min(s_start[1], s_end[1]))
+
+            if s1 in self.obs or s2 in self.obs:
+                return True
+
+        return False
 
 
 def main():

Search-based Planning/Search_2D/RTAAstar.py

@@ -184,23 +184,43 @@ class RtaAstar:
         else:
             return math.hypot(goal[0] - s[0], goal[1] - s[1])
 
-    def cost(self, s_start, s_end):
+    def cost(self, s_start, s_goal):
         """
         Calculate cost for this motion
         :param s_start: starting node
-        :param s_end: end node
+        :param s_goal: end node
         :return:  cost for this motion
         :note: cost function could be more complicate!
         """
 
-        return 1
+        if self.is_collision(s_start, s_goal):
+            return float("inf")
+
+        return math.hypot(s_goal[0] - s_start[0], s_goal[1] - s_start[1])
+
+    def is_collision(self, s_start, s_end):
+        if s_start in self.obs or s_end in self.obs:
+            return True
+
+        if s_start[0] != s_end[0] and s_start[1] != s_end[1]:
+            if s_end[0] - s_start[0] == s_start[1] - s_end[1]:
+                s1 = (min(s_start[0], s_end[0]), min(s_start[1], s_end[1]))
+                s2 = (max(s_start[0], s_end[0]), max(s_start[1], s_end[1]))
+            else:
+                s1 = (min(s_start[0], s_end[0]), max(s_start[1], s_end[1]))
+                s2 = (max(s_start[0], s_end[0]), min(s_start[1], s_end[1]))
+
+            if s1 in self.obs or s2 in self.obs:
+                return True
+
+        return False
 
 
 def main():
     s_start = (10, 5)
     s_goal = (45, 25)
 
-    rtaa = RtaAstar(s_start, s_goal, 220, "euclidean")
+    rtaa = RtaAstar(s_start, s_goal, 240, "euclidean")
     plot = plotting.Plotting(s_start, s_goal)
 
     rtaa.searching()

Search-based Planning/Search_2D/astar.py

@@ -113,15 +113,44 @@ class Astar:
         :return: neighbors
         """
 
-        s_list = set()
+        s_list = []
 
         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:
-                s_list.add(s_next)
+            s_list.append(tuple([s[i] + u[i] for i in range(2)]))
 
         return s_list
 
+    def cost(self, s_start, s_goal):
+        """
+        Calculate cost for this motion
+        :param s_start: starting node
+        :param s_goal: end node
+        :return:  cost for this motion
+        :note: cost function could be more complicate!
+        """
+
+        if self.is_collision(s_start, s_goal):
+            return float("inf")
+
+        return math.hypot(s_goal[0] - s_start[0], s_goal[1] - s_start[1])
+
+    def is_collision(self, s_start, s_end):
+        if s_start in self.obs or s_end in self.obs:
+            return True
+
+        if s_start[0] != s_end[0] and s_start[1] != s_end[1]:
+            if s_end[0] - s_start[0] == s_start[1] - s_end[1]:
+                s1 = (min(s_start[0], s_end[0]), min(s_start[1], s_end[1]))
+                s2 = (max(s_start[0], s_end[0]), max(s_start[1], s_end[1]))
+            else:
+                s1 = (min(s_start[0], s_end[0]), max(s_start[1], s_end[1]))
+                s2 = (max(s_start[0], s_end[0]), min(s_start[1], s_end[1]))
+
+            if s1 in self.obs or s2 in self.obs:
+                return True
+
+        return False
+
     def fvalue(self, x):
         """
         f = g + h. (g: cost to come, h: heuristic function)
@@ -164,18 +193,6 @@ class Astar:
         else:
             return math.hypot(goal[0] - s[0], goal[1] - s[1])
 
-    @staticmethod
-    def cost(s_start, s_goal):
-        """
-        Calculate cost for this motion
-        :param s_start: starting node
-        :param s_goal: end node
-        :return:  cost for this motion
-        :note: cost function could be more complicate!
-        """
-
-        return 1
-
 
 def main():
     s_start = (5, 5)

Search-based Planning/Search_2D/bfs.py

@@ -56,15 +56,32 @@ class BFS:
         :return: neighbors
         """
 
-        s_list = set()
+        s_list = []
 
         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:
-                s_list.add(s_next)
+            if not self.is_collision(s, s_next):
+                s_list.append(s_next)
 
         return s_list
 
+    def is_collision(self, s_start, s_end):
+        if s_start in self.obs or s_end in self.obs:
+            return True
+
+        if s_start[0] != s_end[0] and s_start[1] != s_end[1]:
+            if s_end[0] - s_start[0] == s_start[1] - s_end[1]:
+                s1 = (min(s_start[0], s_end[0]), min(s_start[1], s_end[1]))
+                s2 = (max(s_start[0], s_end[0]), max(s_start[1], s_end[1]))
+            else:
+                s1 = (min(s_start[0], s_end[0]), max(s_start[1], s_end[1]))
+                s2 = (max(s_start[0], s_end[0]), min(s_start[1], s_end[1]))
+
+            if s1 in self.obs or s2 in self.obs:
+                return True
+
+        return False
+
     def extract_path(self):
         """
         Extract the path based on the PARENT set.

Search-based Planning/Search_2D/bidirectional_a_star.py

@@ -140,8 +140,7 @@ class BidirectionalAstar:
         else:
             return math.hypot(goal[0] - s[0], goal[1] - s[1])
 
-    @staticmethod
-    def cost(s_start, s_goal):
+    def cost(self, s_start, s_goal):
         """
         Calculate cost for this motion
         :param s_start: starting node
@@ -150,7 +149,27 @@ class BidirectionalAstar:
         :note: cost function could be more complicate!
         """
 
-        return 1
+        if self.is_collision(s_start, s_goal):
+            return float("inf")
+
+        return math.hypot(s_goal[0] - s_start[0], s_goal[1] - s_start[1])
+
+    def is_collision(self, s_start, s_end):
+        if s_start in self.obs or s_end in self.obs:
+            return True
+
+        if s_start[0] != s_end[0] and s_start[1] != s_end[1]:
+            if s_end[0] - s_start[0] == s_start[1] - s_end[1]:
+                s1 = (min(s_start[0], s_end[0]), min(s_start[1], s_end[1]))
+                s2 = (max(s_start[0], s_end[0]), max(s_start[1], s_end[1]))
+            else:
+                s1 = (min(s_start[0], s_end[0]), max(s_start[1], s_end[1]))
+                s2 = (max(s_start[0], s_end[0]), min(s_start[1], s_end[1]))
+
+            if s1 in self.obs or s2 in self.obs:
+                return True
+
+        return False
 
 
 def main():

Search-based Planning/Search_2D/dfs.py

@@ -60,11 +60,28 @@ class DFS:
 
         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:
+            if not self.is_collision(s, s_next):
                 s_list.append(s_next)
 
         return s_list
 
+    def is_collision(self, s_start, s_end):
+        if s_start in self.obs or s_end in self.obs:
+            return True
+
+        if s_start[0] != s_end[0] and s_start[1] != s_end[1]:
+            if s_end[0] - s_start[0] == s_start[1] - s_end[1]:
+                s1 = (min(s_start[0], s_end[0]), min(s_start[1], s_end[1]))
+                s2 = (max(s_start[0], s_end[0]), max(s_start[1], s_end[1]))
+            else:
+                s1 = (min(s_start[0], s_end[0]), max(s_start[1], s_end[1]))
+                s2 = (max(s_start[0], s_end[0]), min(s_start[1], s_end[1]))
+
+            if s1 in self.obs or s2 in self.obs:
+                return True
+
+        return False
+
     def extract_path(self):
         """
         Extract the path based on the relationship of nodes.

Search-based Planning/Search_2D/dijkstra.py

@@ -5,6 +5,7 @@ Dijkstra 2D
 
 import os
 import sys
+import math
 
 sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
                 "/../../Search-based Planning/")
@@ -36,12 +37,12 @@ class Dijkstra:
         :return: path, order of visited nodes in the planning
         """
 
-        while self.OPEN:
+        while not self.OPEN.empty():
             s = self.OPEN.get()
+            self.CLOSED.append(s)
 
-            if s == self.s_goal:                                    # stop condition
+            if s == self.s_goal:
                 break
-            self.CLOSED.append(s)
 
             for s_n in self.get_neighbor(s):
                 new_cost = self.g[s] + self.cost(s, s_n)
@@ -61,12 +62,10 @@ class Dijkstra:
         :return: neighbors
         """
 
-        s_list = set()
+        s_list = []
 
         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:
-                s_list.add(s_next)
+            s_list.append(tuple([s[i] + u[i] for i in range(2)]))
 
         return s_list
 
@@ -88,8 +87,7 @@ class Dijkstra:
 
         return list(path)
 
-    @staticmethod
-    def cost(s_start, s_goal):
+    def cost(self, s_start, s_goal):
         """
         Calculate cost for this motion
         :param s_start: starting node
@@ -98,7 +96,27 @@ class Dijkstra:
         :note: cost function could be more complicate!
         """
 
-        return 1
+        if self.is_collision(s_start, s_goal):
+            return float("inf")
+
+        return math.hypot(s_goal[0] - s_start[0], s_goal[1] - s_start[1])
+
+    def is_collision(self, s_start, s_end):
+        if s_start in self.obs or s_end in self.obs:
+            return True
+
+        if s_start[0] != s_end[0] and s_start[1] != s_end[1]:
+            if s_end[0] - s_start[0] == s_start[1] - s_end[1]:
+                s1 = (min(s_start[0], s_end[0]), min(s_start[1], s_end[1]))
+                s2 = (max(s_start[0], s_end[0]), max(s_start[1], s_end[1]))
+            else:
+                s1 = (min(s_start[0], s_end[0]), max(s_start[1], s_end[1]))
+                s2 = (max(s_start[0], s_end[0]), min(s_start[1], s_end[1]))
+
+            if s1 in self.obs or s2 in self.obs:
+                return True
+
+        return False
 
 
 def main():

Search-based Planning/Search_2D/env.py

@@ -8,7 +8,8 @@ class Env:
     def __init__(self):
         self.x_range = 51  # size of background
         self.y_range = 31
-        self.motions = [(1, 0), (-1, 0), (0, 1), (0, -1)]
+        self.motions = [(-1, 0), (-1, 1), (0, 1), (1, 1),
+                        (1, 0), (1, -1), (0, -1), (-1, -1)]
         self.obs = self.obs_map()
 
     def obs_map(self):

Search-based Planning/Search_2D/plotting.py

@@ -79,35 +79,34 @@ class Plotting:
 
         for x in visited:
             count += 1
-            plt.plot(x[0], x[1], linewidth='3', color=cl, marker='o')
+            plt.plot(x[0], x[1], color=cl, marker='o')
             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
+                length = 20
             elif count < len(visited) * 2 / 3:
-                length = 25
-            else:
                 length = 30
+            else:
+                length = 40
+            #
+            # length = 15
 
             if count % length == 0:
                 plt.pause(0.001)
         plt.pause(0.01)
 
     def plot_path(self, path, cl='r', flag=False):
-        if self.xI in path:
-            path.remove(self.xI)
-
-        if self.xG in path:
-            path.remove(self.xG)
-
         path_x = [path[i][0] for i in range(len(path))]
         path_y = [path[i][1] for i in range(len(path))]
 
         if not flag:
-            plt.plot(path_x, path_y, linewidth='3', color='r', marker='o')
+            plt.plot(path_x, path_y, linewidth='3', color='r')
         else:
-            plt.plot(path_x, path_y, linewidth='3', color=cl, marker='o')
+            plt.plot(path_x, path_y, linewidth='3', color=cl)
+
+        plt.plot(self.xI[0], self.xI[1], "bs")
+        plt.plot(self.xG[0], self.xG[1], "gs")
 
         plt.pause(0.01)