keep DRY

PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py

@@ -17,6 +17,17 @@ import matplotlib.pyplot as plt
 import copy
 import math
 import cubic_spline_planner
+import sys
+import os
+
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
+                "/../QuinticPolynomialsPlanner/")
+
+try:
+    from quintic_polynomials_planner import QuinticPolynomial
+except ImportError:
+    raise
+
 
 SIM_LOOP = 500
 
@@ -44,50 +55,6 @@ KLON = 1.0
 show_animation = True
 
 
-class quintic_polynomial:
-
-    def __init__(self, xs, vxs, axs, xe, vxe, axe, T):
-
-        # calc coefficient of quintic polynomial
-        self.a0 = xs
-        self.a1 = vxs
-        self.a2 = axs / 2.0
-
-        A = np.array([[T**3, T**4, T**5],
-                      [3 * T ** 2, 4 * T ** 3, 5 * T ** 4],
-                      [6 * T, 12 * T ** 2, 20 * T ** 3]])
-        b = np.array([xe - self.a0 - self.a1 * T - self.a2 * T**2,
-                      vxe - self.a1 - 2 * self.a2 * T,
-                      axe - 2 * self.a2])
-        x = np.linalg.solve(A, b)
-
-        self.a3 = x[0]
-        self.a4 = x[1]
-        self.a5 = x[2]
-
-    def calc_point(self, t):
-        xt = self.a0 + self.a1 * t + self.a2 * t**2 + \
-            self.a3 * t**3 + self.a4 * t**4 + self.a5 * t**5
-
-        return xt
-
-    def calc_first_derivative(self, t):
-        xt = self.a1 + 2 * self.a2 * t + \
-            3 * self.a3 * t**2 + 4 * self.a4 * t**3 + 5 * self.a5 * t**4
-
-        return xt
-
-    def calc_second_derivative(self, t):
-        xt = 2 * self.a2 + 6 * self.a3 * t + 12 * self.a4 * t**2 + 20 * self.a5 * t**3
-
-        return xt
-
-    def calc_third_derivative(self, t):
-        xt = 6 * self.a3 + 24 * self.a4 * t + 60 * self.a5 * t**2
-
-        return xt
-
-
 class quartic_polynomial:
 
     def __init__(self, xs, vxs, axs, vxe, axe, T):
@@ -164,7 +131,8 @@ def calc_frenet_paths(c_speed, c_d, c_d_d, c_d_dd, s0):
         for Ti in np.arange(MINT, MAXT, DT):
             fp = Frenet_path()
 
-            lat_qp = quintic_polynomial(c_d, c_d_d, c_d_dd, di, 0.0, 0.0, Ti)
+            # lat_qp = quintic_polynomial(c_d, c_d_d, c_d_dd, di, 0.0, 0.0, Ti)
+            lat_qp = QuinticPolynomial(c_d, c_d_d, c_d_dd, di, 0.0, 0.0, Ti)
 
             fp.t = [t for t in np.arange(0.0, Ti, DT)]
             fp.d = [lat_qp.calc_point(t) for t in fp.t]