text: """
  
  Frenet optimal trajectory generator
  
  author: Atsushi Sakai (@Atsushi_twi)
  
  Ref:
  
  - [Optimal Trajectory Generation for Dynamic Street Scenarios in a Frenet Frame]
  (https://www.researchgate.net/profile/Moritz_Werling/publication/224156269_Optimal_Trajectory_Generation_for_Dynamic_Street_Scenarios_in_a_Frenet_Frame/links/54f749df0cf210398e9277af.pdf)
  
  - [Optimal trajectory generation for dynamic street scenarios in a Frenet Frame]
  (https://www.youtube.com/watch?v=Cj6tAQe7UCY)
  
  """
  
  import numpy as np
  import matplotlib.pyplot as plt
  import copy
  import math
  import sys
  import os
  
  sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
                  "/../QuinticPolynomialsPlanner/")
  sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
                  "/../CubicSpline/")
  
  try:
      from quintic_polynomials_planner import QuinticPolynomial
      import cubic_spline_planner
  except ImportError:
      raise
  
  SIM_LOOP = 500
  
  # Parameter
  MAX_SPEED = 50.0 / 3.6  # maximum speed [m/s]
  MAX_ACCEL = 2.0  # maximum acceleration [m/ss]
  MAX_CURVATURE = 1.0  # maximum curvature [1/m]
  MAX_ROAD_WIDTH = 7.0  # maximum road width [m]
  D_ROAD_W = 1.0  # road width sampling length [m]
  DT = 0.2  # time tick [s]
  MAX_T = 5.0  # max prediction time [m]
  MIN_T = 4.0  # min prediction time [m]
  TARGET_SPEED = 30.0 / 3.6  # target speed [m/s]
  D_T_S = 5.0 / 3.6  # target speed sampling length [m/s]
  N_S_SAMPLE = 1  # sampling number of target speed
  ROBOT_RADIUS = 2.0  # robot radius [m]
  
  # cost weights
  K_J = 0.1
  K_T = 0.1
  K_D = 1.0
  K_LAT = 1.0
  K_LON = 1.0
  
  show_animation = True
  
  
  class QuarticPolynomial:
  
      def __init__(self, xs, vxs, axs, vxe, axe, time):
          # calc coefficient of quartic polynomial
  
          self.a0 = xs
          self.a1 = vxs
          self.a2 = axs / 2.0
  
          A = np.array([[3 * time ** 2, 4 * time ** 3],
                        [6 * time, 12 * time ** 2]])
          b = np.array([vxe - self.a1 - 2 * self.a2 * time,
                        axe - 2 * self.a2])
          x = np.linalg.solve(A, b)
  
          self.a3 = x[0]
          self.a4 = x[1]
  
      def calc_point(self, t):
          xt = self.a0 + self.a1 * t + self.a2 * t ** 2 + \
               self.a3 * t ** 3 + self.a4 * t ** 4
  
          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
  
          return xt
  
      def calc_second_derivative(self, t):
          xt = 2 * self.a2 + 6 * self.a3 * t + 12 * self.a4 * t ** 2
  
          return xt
  
      def calc_third_derivative(self, t):
          xt = 6 * self.a3 + 24 * self.a4 * t
  
          return xt
  
  
  class FrenetPath:
  
      def __init__(self):
          self.t = []
          # Frenet 坐标系下的相关参数
          self.d = []  # lat 方向的位移
          self.d_d = []
          self.d_dd = []
          self.d_ddd = []
          self.s = []
          self.s_d = []
          self.s_dd = []
          self.s_ddd = []
  
          self.cost_lateral = 0.0  # lat 方向的 cost
          self.cost_cruise = 0.0   # long 方向的 cost
          self.cost_total = 0.0
          
          # 笛卡尔坐标系下的相关参数
          self.x = []  # 笛卡尔坐标系下的 x
          self.y = []  # 笛卡尔坐标系下的 y
          self.yaw = []  # frenet_path 每个点在笛卡尔坐标系下的 yaw 值。
          self.ds = []  # frenet_path 每两个点之间的距离
          self.c = []  # 笛卡尔坐标系下，每个路径点的 curvature
  
  
  def calc_frenet_paths(curr_speed, curr_lat_pos, curr_lat_speed, curr_lat_acc, start_s):
      frenet_paths_list = []
  
      # generate path to each offset goal
      # Lateral motion planning
      for curr_d_sampled in np.arange(-MAX_ROAD_WIDTH, MAX_ROAD_WIDTH, D_ROAD_W):  # 横向位移采样，从 -7 到 7 m，每隔 1 m 进行一次横向采样。
          # MAX_ROAD_WIDTH = 7, D_ROAD_W = 1; 
          for curr_time_sampled in np.arange(MIN_T, MAX_T, DT):  # 横向时间采样；MIN_T = 4 s, MAX_T = 5 s, DT = 0.2 s
              frenet_path = FrenetPath()
  
              # 计算出关于目标状态 curr_d_sampled, curr_time_sampled 的横向五次多项式。
              lateral_quintic_polynomial = QuinticPolynomial(curr_lat_pos, curr_lat_speed, curr_lat_acc, curr_d_sampled, 0.0, 0.0, curr_time_sampled)
  
              # 使用生成的 横向五次多项式 计算 frenet_path 的各项参数（这里的横向轨迹是关于时间 t 的五次多项式，只适用于高速工况）
              frenet_path.t = [t for t in np.arange(0.0, curr_time_sampled, DT)]
              frenet_path.d = [lateral_quintic_polynomial.calc_point(t) for t in frenet_path.t]
              frenet_path.d_d = [lateral_quintic_polynomial.calc_first_derivative(t) for t in frenet_path.t]
              frenet_path.d_dd = [lateral_quintic_polynomial.calc_second_derivative(t) for t in frenet_path.t]
              frenet_path.d_ddd = [lateral_quintic_polynomial.calc_third_derivative(t) for t in frenet_path.t]
  
              # Longitudinal motion planning (Velocity keeping) 纵向规划，这里是 巡航 情况，使用 四次多项式
              for curr_vel_sampled in np.arange(TARGET_SPEED - D_T_S * N_S_SAMPLE,
                                  TARGET_SPEED + D_T_S * N_S_SAMPLE, D_T_S):
                  total_frenet_path = copy.deepcopy(frenet_path)
                  longitudinal_quartic_polynomial = QuarticPolynomial(start_s, curr_speed, 0.0, curr_vel_sampled, 0.0, curr_time_sampled)  # 起始和末状态的加速度都为 0.0。
  
                  total_frenet_path.s = [longitudinal_quartic_polynomial.calc_point(t) for t in frenet_path.t]
                  total_frenet_path.s_d = [longitudinal_quartic_polynomial.calc_first_derivative(t) for t in frenet_path.t]
                  total_frenet_path.s_dd = [longitudinal_quartic_polynomial.calc_second_derivative(t) for t in frenet_path.t]
                  total_frenet_path.s_ddd = [longitudinal_quartic_polynomial.calc_third_derivative(t) for t in frenet_path.t]
  
                  Jp = sum(np.power(total_frenet_path.d_ddd, 2))  # square of jerk in lateral direction
                  Js = sum(np.power(total_frenet_path.s_ddd, 2))  # square of jerk in longitudinal direction
  
                  # square of diff from target speed
                  ds = (TARGET_SPEED - total_frenet_path.s_d[-1]) ** 2
  
                  # 计算该条轨迹的 Cost，用于之后最优轨迹的选择
                  # 横向损失函数：需要 Jerk 尽可能小；达到末状态的时间尽可能短；末状态的横向偏移尽可能小
                  total_frenet_path.cost_lateral = K_J * Jp + K_T * curr_time_sampled + K_D * total_frenet_path.d[-1] ** 2
                  # 纵向损失函数：需要 Jerk 尽可能小；达到末状态的时间尽可能短；末状态的横向偏移尽可能小
                  total_frenet_path.cost_cruise = K_J * Js + K_T * curr_time_sampled + K_D * ds
                  # 总的损失函数
                  total_frenet_path.cost_total = K_LAT * total_frenet_path.cost_lateral + K_LON * total_frenet_path.cost_cruise
  
                  frenet_paths_list.append(total_frenet_path)
  
      return frenet_paths_list
  
  
  def calc_global_paths(frenet_paths_list, cubic_spline2d):  # 计算 frenet_path 在 笛卡尔 Cartesian 坐标系 下的座标点
  
      for frenet_path in frenet_paths_list:
          # calc global positions
          for i in range(len(frenet_path.s)):
              ix, iy = cubic_spline2d.calc_position(frenet_path.s[i])  # ix, iy 是 reference line 上的点在 笛卡尔坐标系下的值。
              # 这个点是 frenet_path 上 s 方向位移为 s[i] 的点在 reference line 上的映射点。
              # 通过得到 ix，iy，在加上 已知 d[i]，可以计算得到 frenet_path 上的点在笛卡尔坐标系下的 x，y 值。
              if ix is None:
                  break
              i_yaw = cubic_spline2d.calc_yaw(frenet_path.s[i])  # i_yaw 是 reference_line 上映射点处的 yaw 值。
              di = frenet_path.d[i]
              cartesian_x = ix + di * math.cos(i_yaw + math.pi / 2.0)
              cartesian_y = iy + di * math.sin(i_yaw + math.pi / 2.0)
              frenet_path.x.append(cartesian_x)
              frenet_path.y.append(cartesian_y)
  
          # calc yaw and ds
          for i in range(len(frenet_path.x) - 1):
              dx = frenet_path.x[i + 1] - frenet_path.x[i]
              dy = frenet_path.y[i + 1] - frenet_path.y[i]
              frenet_path.yaw.append(math.atan2(dy, dx))  # 计算 frenet_path 的第 i 个点在 笛卡尔 坐标系下的 yaw 值
              frenet_path.ds.append(math.hypot(dx, dy))  # 计算 frenet_path 上当前点和下一个点之间的 距离。
  
          frenet_path.yaw.append(frenet_path.yaw[-1]) # 这里把倒数第二个点的 yaw 值也赋给了最后一个点
          frenet_path.ds.append(frenet_path.ds[-1])
  
          # calc curvature
          for i in range(len(frenet_path.yaw) - 1):  # 计算第 i 个点的 curvature：（下一个点的 yaw - 当前点的 yaw） / 两个点之间的距离
              frenet_path.c.append((frenet_path.yaw[i + 1] - frenet_path.yaw[i]) / frenet_path.ds[i])  
  
      return frenet_paths_list
  
  
  def check_collision(fp, ob):
      for i in range(len(ob[:, 0])):
          d = [((ix - ob[i, 0]) ** 2 + (iy - ob[i, 1]) ** 2)
               for (ix, iy) in zip(fp.x, fp.y)]
  
          collision = any([di <= ROBOT_RADIUS ** 2 for di in d])
  
          if collision:
              return False
  
      return True
  
  
  def check_paths(frenet_path_list, obstacle_array):
      ok_ind = []
      for i, _ in enumerate(frenet_path_list):
          if any([v > MAX_SPEED for v in frenet_path_list[i].s_d]):  # Max speed check
              continue
          elif any([abs(a) > MAX_ACCEL for a in
                    frenet_path_list[i].s_dd]):  # Max accel check
              continue
          elif any([abs(c) > MAX_CURVATURE for c in
                    frenet_path_list[i].c]):  # Max curvature check
              continue
          elif not check_collision(frenet_path_list[i], obstacle_array):
              continue
  
          ok_ind.append(i)
  
      return [frenet_path_list[i] for i in ok_ind]
  
  
  def frenet_optimal_planning(cubic_spline2d, start_s, curr_speed, curr_lat_position, curr_lat_speed, curr_lat_acc, obstacle_array):
      # 计算全部采样的 frenet_path，存入 frenet_paths_list
      frenet_paths_list = calc_frenet_paths(curr_speed, curr_lat_position, curr_lat_speed, curr_lat_acc, start_s)
      # 将 frenet_paths_list 中的轨迹转换成 世界坐标系下的 轨迹。
      frenet_paths_list = calc_global_paths(frenet_paths_list, cubic_spline2d)
      frenet_paths_list = check_paths(frenet_paths_list, obstacle_array)
  
      # find minimum cost path
      min_cost = float("inf")
      best_path = None
      for frenet_path in frenet_paths_list:
          if min_cost >= frenet_path.cost_total:
              min_cost = frenet_path.cost_total
              best_path = frenet_path  # 类型：FrenetPath()
  
      return best_path, frenet_paths_list  
  
  
  def generate_target_course(waypoints_x_list, waypoints_y_list):
      cubic_spline2d = cubic_spline_planner.Spline2D(waypoints_x_list, waypoints_y_list)
      s_reference_line_list = np.arange(0, cubic_spline2d.s_distance_list[-1], 0.1)  # 以 0.1 作为采样间隔
  
      rx, ry, ryaw, rk = [], [], [], []
      for s_reference_line_point in s_reference_line_list:
          ix, iy = cubic_spline2d.calc_position(s_reference_line_point)
          rx.append(ix)
          ry.append(iy)
          ryaw.append(cubic_spline2d.calc_yaw(s_reference_line_point))
          rk.append(cubic_spline2d.calc_curvature(s_reference_line_point))
  
      return rx, ry, ryaw, rk, cubic_spline2d
  
  
  def main():
      print(__file__ + " start!!")
  
      # way points
      waypoints_x_list = [0.0, 10.0, 20.5, 35.0, 70.5]
      waypoints_y_list = [0.0, -6.0, 5.0, 6.5, 0.0]
      # obstacle lists
      obstacle_array = np.array([[20.0, 10.0],
                     [30.0, 6.0],
                     [30.0, 8.0],
                     [35.0, 8.0],
                     [50.0, 3.0]
                     ])
  
      target_x_list, target_y_list, target_yaw_list, target_curvature_list, cubic_spline2d = generate_target_course(waypoints_x_list, waypoints_y_list)  
      # 生成参考轨迹，包括：
      #   每个轨迹点的 x，y 坐标信息
      #   在路径点处的航向角 yaw
      #   在路径点处的轨迹曲率 tc - curvature
      #   csp - 实例化的 cubic spline 对象（csp = cubic_spline_planner.Spline2D(x, y)）
  
      # initial state
      curr_speed = 10.0 / 3.6  # current speed [m/s]
      curr_lat_position = 2.0  # current lateral position [m]
      curr_lat_speed = 0.0  # current lateral speed [m/s]
      curr_lat_acc = 0.0  # current lateral acceleration [m/s]
      start_s = 0.0  # current vehicle position projected on reference line 
      # 这里关于 Frenet 坐标系下 s 的设计思路：
      #   根据路径点使用 cubic spline 生成平滑的参考线，同时已经计算了参考线上全部路径点的 s 值，起始点为 0。
      #   然后规划的起始点的 s 值 就是这些 计算好的路径点 的 s 值。在 Apollo 中规划起始点的 s 值实际上为 映射点在 reference line 上的 s 值。
      #   换句话说，这里的 cubic spline 就相当于 Apollo 中的 reference line。
  
      # 这里还假设了车辆初始速度方向与 参考线的 s 方向一致，所以不需要对 初始速度进行分解。
  
      area = 20.0  # animation area length [m]
  
      frenet_path_list = []
      for i in range(SIM_LOOP):
          # 进行规划，返回最优轨迹；
          # path 的数据类型：FrenetPath()
          best_path, frenet_path_list = frenet_optimal_planning(
              cubic_spline2d, start_s, curr_speed, curr_lat_position, curr_lat_speed, curr_lat_acc, obstacle_array)
  
          # 更新车辆当前位姿信息
          start_s = best_path.s[1]  # 这里体现了 Apollo 中轨迹拼接的思想
          curr_lat_position = best_path.d[1]
          curr_lat_speed = best_path.d_d[1]
          curr_lat_acc = best_path.d_dd[1]
          curr_speed = best_path.s_d[1]
  
          if np.hypot(best_path.x[1] - target_x_list[-1], best_path.y[1] - target_y_list[-1]) <= 1.0:
              print("Goal")
              break
  
          if show_animation:  # pragma: no cover
              plt.cla()
              # for stopping simulation with the esc key.
              plt.gcf().canvas.mpl_connect(
                  'key_release_event',
                  lambda event: [exit(0) if event.key == 'escape' else None])
              plt.plot(waypoints_x_list, waypoints_y_list, "xr")
              plt.plot(target_x_list, target_y_list)
              plt.plot(obstacle_array[:, 0], obstacle_array[:, 1], "xk")
              plt.plot(best_path.x[1:], best_path.y[1:], "-or")
              plt.plot(best_path.x[1], best_path.y[1], "vc")
              for path in frenet_path_list:
                  plt.plot(path.x, path.y, "--")
  
              # plt.xlim(path.x[1] - area, path.x[1] + area)
              # plt.ylim(path.y[1] - area, path.y[1] + area)
              plt.xlim((-10, 80))
              plt.ylim((-10, 20))
              plt.title("v[km/h]:" + str(curr_speed * 3.6)[0:4])
              plt.grid(True)
              plt.pause(0.0001)
  
      print("Finish")
      if show_animation:  # pragma: no cover
          plt.grid(True)
          plt.pause(0.0001)
          plt.show()
  
  
  if __name__ == '__main__':
      main()
  