From c3621e74b59c8126a946824dd1351993c2228c0a Mon Sep 17 00:00:00 2001
From: zhm-real <zhou.hm0420@gmail.com>
Date: Sat, 27 Jun 2020 23:23:59 -0700
Subject: [PATCH] add LTRA*-1 step

---
 Search-based Planning/.idea/workspace.xml     |  19 +--
 .../Search_2D/{ara_star.py => ARA_star.py}    |   0
 .../Search_2D/{ida_star.py => IDA_star.py}    |   0
 Search-based Planning/Search_2D/LRTA_star.py  | 117 ++++++++++++++++++
 4 files changed, 129 insertions(+), 7 deletions(-)
 rename Search-based Planning/Search_2D/{ara_star.py => ARA_star.py} (100%)
 rename Search-based Planning/Search_2D/{ida_star.py => IDA_star.py} (100%)
 create mode 100644 Search-based Planning/Search_2D/LRTA_star.py

diff --git a/Search-based Planning/.idea/workspace.xml b/Search-based Planning/.idea/workspace.xml
index 0cf8218..f4555aa 100644
--- a/Search-based Planning/.idea/workspace.xml	
+++ b/Search-based Planning/.idea/workspace.xml	
@@ -19,7 +19,12 @@
     <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 afterPath="$PROJECT_DIR$/Search_2D/LRTA_star.py" 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/ara_star.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/ARA_star.py" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/Search_2D/ida_star.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/IDA_star.py" afterDir="false" />
+    </list>
     <option name="EXCLUDED_CONVERTED_TO_IGNORED" value="true" />
     <option name="SHOW_DIALOG" value="false" />
     <option name="HIGHLIGHT_CONFLICTS" value="true" />
@@ -64,8 +69,8 @@
       </list>
     </option>
   </component>
-  <component name="RunManager" selected="Python.bidirectionalAstar3D">
-    <configuration name="a_star" type="PythonConfigurationType" factoryName="Python" temporary="true" nameIsGenerated="true">
+  <component name="RunManager" selected="Python.LRTA_star">
+    <configuration name="LRTA_star" type="PythonConfigurationType" factoryName="Python" temporary="true">
       <module name="Search-based Planning" />
       <option name="INTERPRETER_OPTIONS" value="" />
       <option name="PARENT_ENVS" value="true" />
@@ -77,7 +82,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/a_star.py" />
+      <option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_2D/LRTA_star.py" />
       <option name="PARAMETERS" value="" />
       <option name="SHOW_COMMAND_LINE" value="false" />
       <option name="EMULATE_TERMINAL" value="false" />
@@ -193,19 +198,19 @@
     </configuration>
     <list>
       <item itemvalue="Python.dijkstra" />
-      <item itemvalue="Python.a_star" />
       <item itemvalue="Python.bidirectional_a_star" />
       <item itemvalue="Python.bfs" />
       <item itemvalue="Python.dfs" />
       <item itemvalue="Python.bidirectionalAstar3D" />
+      <item itemvalue="Python.LRTA_star" />
     </list>
     <recent_temporary>
       <list>
+        <item itemvalue="Python.LRTA_star" />
+        <item itemvalue="Python.bidirectional_a_star" />
         <item itemvalue="Python.bidirectionalAstar3D" />
         <item itemvalue="Python.bfs" />
         <item itemvalue="Python.dfs" />
-        <item itemvalue="Python.bidirectional_a_star" />
-        <item itemvalue="Python.a_star" />
       </list>
     </recent_temporary>
   </component>
diff --git a/Search-based Planning/Search_2D/ara_star.py b/Search-based Planning/Search_2D/ARA_star.py
similarity index 100%
rename from Search-based Planning/Search_2D/ara_star.py
rename to Search-based Planning/Search_2D/ARA_star.py
diff --git a/Search-based Planning/Search_2D/ida_star.py b/Search-based Planning/Search_2D/IDA_star.py
similarity index 100%
rename from Search-based Planning/Search_2D/ida_star.py
rename to Search-based Planning/Search_2D/IDA_star.py
diff --git a/Search-based Planning/Search_2D/LRTA_star.py b/Search-based Planning/Search_2D/LRTA_star.py
new file mode 100644
index 0000000..7d3ba64
--- /dev/null
+++ b/Search-based Planning/Search_2D/LRTA_star.py	
@@ -0,0 +1,117 @@
+"""
+LRTA_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 queue
+from Search_2D import plotting
+from Search_2D import env
+
+
+class LrtAstar:
+    def __init__(self, x_start, x_goal, heuristic_type):
+        self.xI, self.xG = x_start, x_goal
+        self.heuristic_type = heuristic_type
+
+        self.Env = env.Env()  # class Env
+
+        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")}
+        self.OPEN = queue.QueuePrior()  # priority queue / OPEN
+        self.OPEN.put(self.xI, self.h(self.xI))
+        self.CLOSED = set()
+        self.Parent = {self.xI: self.xI}
+
+    def searching(self):
+        h = {self.xI: self.h(self.xI)}
+        s = self.xI
+        parent = {self.xI: self.xI}
+        visited = []
+        count = 0
+        while s != self.xG:
+            count += 1
+            print(count)
+            visited.append(s)
+            h_list = {}
+            for u in self.u_set:
+                s_next = tuple([s[i] + u[i] for i in range(len(s))])
+                if s_next not in self.obs:
+                    if s_next not in h:
+                        h[s_next] = self.h(s_next)
+                    h_list[s_next] = self.get_cost(s, s_next) + h[s_next]
+            h_new = min(h_list.values())
+            if h_new > h[s]:
+                h[s] = h_new
+            s_child = min(h_list, key=h_list.get)
+            parent[s_child] = s
+            s = s_child
+        # path_get = self.extract_path(parent)
+        return [], visited
+
+    def extract_path(self, parent):
+        path = [self.xG]
+        s = self.xG
+
+        while True:
+            s = parent[s]
+            path.append(s)
+
+            if s == self.xI:
+                break
+
+        return path
+
+    def h(self, s):
+        heuristic_type = self.heuristic_type
+        goal = self.xG
+
+        if heuristic_type == "manhattan":
+            return abs(goal[0] - s[0]) + abs(goal[1] - s[1])
+        elif heuristic_type == "euclidean":
+            return ((goal[0] - s[0]) ** 2 + (goal[1] - s[1]) ** 2) ** (1 / 2)
+        else:
+            print("Please choose right heuristic type!")
+
+    @staticmethod
+    def get_cost(x, u):
+        """
+        Calculate cost for this motion
+
+        :param x: current node
+        :param u: input
+        :return:  cost for this motion
+        :note: cost function could be more complicate!
+        """
+
+        return 1
+
+
+def main():
+    x_start = (10, 5)  # Starting node
+    x_goal = (45, 25)  # Goal node
+
+    lrtastar = LrtAstar(x_start, x_goal, "manhattan")
+    plot = plotting.Plotting(x_start, x_goal)  # class Plotting
+
+    path, visited = lrtastar.searching()
+    pathx = [x[0] for x in path]
+    pathy = [x[1] for x in path]
+    vx = [x[0] for x in visited]
+    vy = [x[1] for x in visited]
+    plot.plot_grid("test")
+    plt.plot(pathx, pathy, 'r')
+    plt.plot(vx, vy, 'gray')
+    plt.show()
+
+
+if __name__ == '__main__':
+    main()