From 84797da6fbc8d5adc1bb7fb6fc2e02c900ae4205 Mon Sep 17 00:00:00 2001
From: AtsushiSakai <asakai.amsl+github@gmail.com>
Date: Wed, 5 Jul 2017 22:48:04 -0700
Subject: [PATCH] add model_trajectory_generator

---
 .../model_trajectory_generator.py             | 42 +++++++++++++++++++
 1 file changed, 42 insertions(+)
 create mode 100644 PathPlanning/LatticePlanner/model_trajectory_generator.py

diff --git a/PathPlanning/LatticePlanner/model_trajectory_generator.py b/PathPlanning/LatticePlanner/model_trajectory_generator.py
new file mode 100644
index 00000000..3f2f87fc
--- /dev/null
+++ b/PathPlanning/LatticePlanner/model_trajectory_generator.py
@@ -0,0 +1,42 @@
+"""
+Model trajectory generator
+
+author: Atsushi Sakai
+"""
+
+import numpy as np
+import scipy.interpolate
+import matplotlib.pyplot as plt
+import unicycle_model
+
+
+def generate_trajectory(v, kp):
+
+    state = State()
+
+
+def main():
+    print(__file__ + " start!!")
+
+    v = 10.0 / 3.6  # [m/s]
+    s = 10.0  # [m]
+    k0 = 0.0
+    km = -1.0
+    kf = 1.0
+    ntime = 20.0
+
+    time = s / v  # [s]
+    xk = np.array([0.0, time / 2.0, time])
+    yk = np.array([k0, km, kf])
+    t = np.append(np.arange(0.0, time, time / ntime), time)
+    kp = scipy.interpolate.spline(xk, yk, t, order=2)
+
+    #  plt.plot(xk, yk, "xr")
+    #  plt.plot(t, kp)
+    #  plt.show()
+
+    x, y = generate_trajectory(v, kp)
+
+
+if __name__ == '__main__':
+    main()