text: #!/usr/bin/env python
  
  ###############################################################################
  # Copyright 2018 The Apollo Authors. All Rights Reserved.
  #
  # Licensed under the Apache License, Version 2.0 (the "License");
  # you may not use this file except in compliance with the License.
  # You may obtain a copy of the License at
  #
  # http://www.apache.org/licenses/LICENSE-2.0
  #
  # Unless required by applicable law or agreed to in writing, software
  # distributed under the License is distributed on an "AS IS" BASIS,
  # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  # See the License for the specific language governing permissions and
  # limitations under the License.
  ###############################################################################
  
  from distance_approach_python_interface import *
  import matplotlib.pyplot as plt
  import matplotlib.patches as patches
  from matplotlib import animation
  import numpy as np
  import time
  import math
  
  def SmoothTrajectory(visualize_flag):
      # initialze object
      OpenSpacePlanner = DistancePlanner()
  
      # parameter(except max, min and car size is defined in proto)
      num_output_buffer = 10000
      sx = -8
      sy = 1.5
      sphi = 0.5
  
      scenario = "backward"
      # scenario = "parallel"
  
      if scenario == "backward":
          # obstacles for distance approach(vertices coords in clock wise order)
          ROI_distance_approach_parking_boundary = (
              c_double * 20)(*[-13.6407054776,
                              0.0140634663703,
                              0.0,
                              0.0,
                              0.0515703622475,
                              -5.15258191624,
                              0.0515703622475,
                              -5.15258191624,
                              2.8237895441,
                              -5.15306980547,
                              2.8237895441,
                              -5.15306980547,
                              2.7184833539,
                              -0.0398078878812,
                              16.3592013995,
                              -0.011889513383,
                              16.3591910364,
                              5.60414234644,
                              -13.6406951857,
                              5.61797800844,
                              ])
          OpenSpacePlanner.AddObstacle(
              ROI_distance_approach_parking_boundary)
          ex = 1.359
          ey = -3.86443643718
          ephi = 1.581
          XYbounds = [-13.6406951857, 16.3591910364, -5.15258191624, 5.61797800844]
  
  
      x = (c_double * num_output_buffer)()
      y = (c_double * num_output_buffer)()
      phi = (c_double * num_output_buffer)()
      v = (c_double * num_output_buffer)()
      a = (c_double * num_output_buffer)()
      steer = (c_double * num_output_buffer)()
      opt_x = (c_double * num_output_buffer)()
      opt_y = (c_double * num_output_buffer)()
      opt_phi = (c_double * num_output_buffer)()
      opt_v = (c_double * num_output_buffer)()
      opt_a = (c_double * num_output_buffer)()
      opt_steer = (c_double * num_output_buffer)()
      opt_time = (c_double * num_output_buffer)()
      opt_dual_l = (c_double * num_output_buffer)()
      opt_dual_n = (c_double * num_output_buffer)()
      size = (c_ushort * 1)()
      XYbounds_ctype = (c_double * 4)(*XYbounds)
  
      success = True
      start = time.time()
      print("planning start")
      if not OpenSpacePlanner.DistancePlan(sx, sy, sphi, ex, ey, ephi, XYbounds_ctype):
          print("planning fail")
          success = False
      #   exit()
      planning_time = time.time() - start
      print("planning time is " + str(planning_time))
  
      x_out = []
      y_out = []
      phi_out = []
      v_out = []
      a_out = []
      steer_out = []
      opt_x_out = []
      opt_y_out = []
      opt_phi_out = []
      opt_v_out = []
      opt_a_out = []
      opt_steer_out = []
      opt_time_out = []
      opt_dual_l_out = []
      opt_dual_n_out = []
  
      if visualize_flag and success:
          # load result
          OpenSpacePlanner.DistanceGetResult(x, y, phi, v, a, steer, opt_x,
                                          opt_y, opt_phi, opt_v, opt_a, opt_steer, opt_time,
                                          opt_dual_l, opt_dual_n, size)
          for i in range(0, size[0]):
              x_out.append(float(x[i]))
              y_out.append(float(y[i]))
              phi_out.append(float(phi[i]))
              v_out.append(float(v[i]))
              a_out.append(float(a[i]))
              steer_out.append(float(steer[i]))
              opt_x_out.append(float(opt_x[i]))
              opt_y_out.append(float(opt_y[i]))
              opt_phi_out.append(float(opt_phi[i]))
              opt_v_out.append(float(opt_v[i]))
              opt_a_out.append(float(opt_a[i]))
              opt_steer_out.append(float(opt_steer[i]))
              opt_time_out.append(float(opt_time[i]))
  
          for i in range(0, size[0] * 6):
              opt_dual_l_out.append(float(opt_dual_l[i]))
          for i in range(0, size[0] * 16):
              opt_dual_n_out.append(float(opt_dual_n[i]))
          # trajectories plot
          fig1 = plt.figure(1)
          ax = fig1.add_subplot(111)
          for i in range(0, size[0]):
              # warm start
              downx = 1.055 * math.cos(phi_out[i] - math.pi / 2)
              downy = 1.055 * math.sin(phi_out[i] - math.pi / 2)
              leftx = 1.043 * math.cos(phi_out[i] - math.pi)
              lefty = 1.043 * math.sin(phi_out[i] - math.pi)
              x_shift_leftbottom = x_out[i] + downx + leftx
              y_shift_leftbottom = y_out[i] + downy + lefty
              warm_start_car = patches.Rectangle((x_shift_leftbottom, y_shift_leftbottom), 3.89 + 1.043, 1.055*2,
                                              angle=phi_out[i] * 180 / math.pi, linewidth=1, edgecolor='r', facecolor='none')
              warm_start_arrow = patches.Arrow(
                  x_out[i], y_out[i], 0.25*math.cos(phi_out[i]), 0.25*math.sin(phi_out[i]), 0.2, edgecolor='r',)
              # ax.add_patch(warm_start_car)
              ax.add_patch(warm_start_arrow)
              # distance approach
              downx = 1.055 * math.cos(opt_phi_out[i] - math.pi / 2)
              downy = 1.055 * math.sin(opt_phi_out[i] - math.pi / 2)
              leftx = 1.043 * math.cos(opt_phi_out[i] - math.pi)
              lefty = 1.043 * math.sin(opt_phi_out[i] - math.pi)
              x_shift_leftbottom = opt_x_out[i] + downx + leftx
              y_shift_leftbottom = opt_y_out[i] + downy + lefty
              smoothing_car = patches.Rectangle((x_shift_leftbottom, y_shift_leftbottom), 3.89 + 1.043, 1.055*2,
                                              angle=opt_phi_out[i] * 180 / math.pi, linewidth=1, edgecolor='y', facecolor='none')
              smoothing_arrow = patches.Arrow(
                  opt_x_out[i], opt_y_out[i], 0.25*math.cos(opt_phi_out[i]), 0.25*math.sin(opt_phi_out[i]), 0.2, edgecolor='y',)
              ax.add_patch(smoothing_car)
              ax.add_patch(smoothing_arrow)
  
          ax.plot(sx, sy, "s")
          ax.plot(ex, ey, "s")
          if scenario == "backward":
              left_boundary_x = [-13.6407054776, 0.0, 0.0515703622475]
              left_boundary_y = [0.0140634663703, 0.0, -5.15258191624]
              down_boundary_x = [0.0515703622475, 2.8237895441]
              down_boundary_y = [-5.15258191624, -5.15306980547]
              right_boundary_x = [2.8237895441, 2.7184833539, 16.3592013995]
              right_boundary_y = [-5.15306980547, -0.0398078878812, -0.011889513383]
              up_boundary_x = [16.3591910364, -13.6406951857]
              up_boundary_y = [5.60414234644, 5.61797800844]
              ax.plot(left_boundary_x, left_boundary_y, "k")
              ax.plot(down_boundary_x, down_boundary_y, "k")
              ax.plot(right_boundary_x, right_boundary_y, "k")
              ax.plot(up_boundary_x, up_boundary_y, "k")
  
          plt.axis('equal')
  
          # input plot
          fig2 = plt.figure(2)
          v_graph = fig2.add_subplot(411)
          v_graph.title.set_text('v')
          v_graph.plot(np.linspace(0, size[0], size[0]), v_out)
          v_graph.plot(np.linspace(0, size[0], size[0]), opt_v_out)
          a_graph = fig2.add_subplot(412)
          a_graph.title.set_text('a')
          a_graph.plot(np.linspace(0, size[0], size[0]), a_out)
          a_graph.plot(np.linspace(0, size[0], size[0]), opt_a_out)
          steer_graph = fig2.add_subplot(413)
          steer_graph.title.set_text('steering')
          steer_graph.plot(np.linspace(0, size[0], size[0]), steer_out)
          steer_graph.plot(np.linspace(0, size[0], size[0]), opt_steer_out)
          steer_graph = fig2.add_subplot(414)
          steer_graph.title.set_text('t')
          steer_graph.plot(np.linspace(0, size[0], size[0]), opt_time_out)
          # dual variables
          fig3 = plt.figure(3)
          dual_l_graph = fig3.add_subplot(211)
          dual_l_graph.title.set_text('dual_l')
          dual_l_graph.plot(np.linspace(0, size[0] * 6, size[0] * 6), opt_dual_l_out)
          dual_n_graph = fig3.add_subplot(212)
          dual_n_graph.title.set_text('dual_n')
          dual_n_graph.plot(np.linspace(0, size[0] * 16, size[0] * 16), opt_dual_n_out)
          plt.show()
  
      if not visualize_flag :
          if success :
              # load result
              OpenSpacePlanner.DistanceGetResult(x, y, phi, v, a, steer, opt_x,
                                              opt_y, opt_phi, opt_v, opt_a, opt_steer, opt_time,
                                              opt_dual_l, opt_dual_n, size)
              for i in range(0, size[0]):
                  x_out.append(float(x[i]))
                  y_out.append(float(y[i]))
                  phi_out.append(float(phi[i]))
                  v_out.append(float(v[i]))
                  a_out.append(float(a[i]))
                  steer_out.append(float(steer[i]))
                  opt_x_out.append(float(opt_x[i]))
                  opt_y_out.append(float(opt_y[i]))
                  opt_phi_out.append(float(opt_phi[i]))
                  opt_v_out.append(float(opt_v[i]))
                  opt_a_out.append(float(opt_a[i]))
                  opt_steer_out.append(float(opt_steer[i]))
                  opt_time_out.append(float(opt_time[i]))
          return success, opt_x_out, opt_y_out, opt_phi_out, opt_v_out, opt_a_out, opt_steer_out, opt_time_out, planning_time
  
  if __name__ == '__main__':
      visualize_flag = True
      SmoothTrajectory(visualize_flag)
  