From 089cd15f8a7e0a227ca484364a78bd26031106e9 Mon Sep 17 00:00:00 2001
From: Atsushi Sakai <asakai.amsl+github@gmail.com>
Date: Fri, 4 May 2018 09:09:46 +0900
Subject: [PATCH] LQR local planner initial commit

---
 PathPlanning/LQRPlanner/LQRplanner.py | 39 +++++++++++++++++++++++++++
 1 file changed, 39 insertions(+)
 create mode 100644 PathPlanning/LQRPlanner/LQRplanner.py

diff --git a/PathPlanning/LQRPlanner/LQRplanner.py b/PathPlanning/LQRPlanner/LQRplanner.py
new file mode 100644
index 00000000..bca4f291
--- /dev/null
+++ b/PathPlanning/LQRPlanner/LQRplanner.py
@@ -0,0 +1,39 @@
+"""
+
+LQR local path planning module
+
+author: Atsushi Sakai (@Atsushi_twi)
+
+"""
+
+import matplotlib.pyplot as plt
+
+show_animation = True
+
+
+def LQRplanning(sx, sy, gx, gy):
+
+    rx, ry = [], []
+
+    return rx, ry
+
+
+def main():
+    print(__file__ + " start!!")
+
+    sx = 0.0
+    sy = 0.0
+    gx = 10.0
+    gy = 5.0
+
+    rx, ry = LQRplanning(sx, sy, gx, gy)
+
+    plt.plot(sx, sy, "xb")
+    plt.plot(gx, gy, "xb")
+    plt.plot(rx, ry)
+    plt.axis("equal")
+    plt.show()
+
+
+if __name__ == '__main__':
+    main()