Apollo Spark Project Study Notes——Apollo Open Space Planning Algorithm Principle and Practice

foreword

The course link of Apollo Spark Project is as follows
Spark Project 2.0 Basic Course: https://apollo.baidu.com/community/online-course/2
Spark Project 2.0 Special Course: https://apollo.baidu.com/community/online-course/ 12

1. General introduction of open space planning algorithm

    The configuration of the open space algorithm is mainly in valet_parking_config.pb.txt, which is divided into four parts: OPEN_SPACE_ROI_DECIDER, OPEN_SPACE_TRAJECTORY_PROVIDER, OPEN_SPACE_TRAJECTORY_PARTITION, OPEN_SPACE_FALLBACK_DECIDER.

// /home/yuan/apollo-edu/modules/planning/conf/scenario/valet_parking_config.pb.txt
stage_config: {
    
    
  stage_type: VALET_PARKING_PARKING
  enabled: true
  task_type: OPEN_SPACE_ROI_DECIDER
  task_type: OPEN_SPACE_TRAJECTORY_PROVIDER
  task_type: OPEN_SPACE_TRAJECTORY_PARTITION
  task_type: OPEN_SPACE_FALLBACK_DECIDER

1.1 Task: OPEN_SPACE_ROI_DECIDER

According to the road boundary, the parking space boundary generates a drivable area
ps: if there are obstacles in the drivable area, the boundary of the obstacle needs to be taken into consideration.

1.2 Task: OPEN_SPACE_TRAJECTORY_PROVIDER

Plan rough collision-free trajectories

    Planning a collision-free trajectory is mainly divided into the following two steps: first, use the hybrid Astar algorithm and RS curve to plan a collision-free feasible trajectory, but this trajectory has the following problems: the trajectory curve may change abruptly, which does not satisfy the vehicle Kinematic requirements. Therefore, further smoothing is required, which uses the IAPS/OBCA algorithm to smooth the trajectory to generate a collision-free trajectory that meets the vehicle dynamics constraints.

1.3 Task: OPEN_SPACE_TRAJECTORY_PARTITION

The task of smoothing
    the trajectory is to divide the trajectory of the vehicle according to whether it is a forward trajectory or a reverse trajectory, and then decide which trajectory to send and whether to switch the trajectory according to the position of the vehicle.

1.4 Task: OPEN_SPACE_FALLBACK_DECIDER

    Check whether the planned trajectory collides with obstacles. If a collision occurs, it will enter the FALLBACK state, plan a parking trajectory, and then re-plan the path.

2. Path planning algorithm based on hybrid A*

    The Astar algorithm is widely used in the field of robot path planning, so I won’t go into detail here. Readers who want to know, please refer to this article——Automatic Driving Path Planning
——A* (Astar) Algorithm

2.1 Brief idea of ​​hybrid A*

    Next, let’s talk about the problems of the Astar algorithm.
    It can be seen that the path generated by the Astar algorithm is not smooth and does not satisfy the kinematic constraints of the vehicle.     According to the dynamic model of the vehicle, the heading of the actual vehicle is consistent with the velocity direction of the center of the rear axle of the vehicle. Therefore, we can equivalently regard the motion path of the vehicle as a section of arcs with different curvature radii at the center point of the rear axle, as shown in the figure below.     The turning radius of the rear axle of the vehicle can be calculated from the turning angle of the front wheels of the vehicle. $$\tan \left(d\right)=\frac{L}{R}$$    We can sample the front wheel angles. Different front wheel angles correspond to different turning radii. In each sampling period, we drive a fixed length of arc, so that we can get the smooth path shown in the figure below.     As a result, Astar's original grid map is not applicable to the current trajectory, so it is necessary to add $The\; variable\; \theta$ heading angle describes the trajectory of the vehicle, that is, the original two-dimensional node$(x,y)$ becomes a three-dimensional node$(x,y,\theta )$ .     Below is the format of a 3D node in Apollo.

    Among them x_grid, y_grid, phi_gridare respectively $(x,y,\theta )$ , ie$(x,y,\theta )$ divided by the length in the direction of accuracy. Determine whether the node has been traversed by whether the strings composed of the three indexes are the same.     Because it is difficult to reach the end point simply by searching, we need to analyze and expand the path nodes.

2.2 RS curve

1990 Optimal paths for a car that goes both forwards and backwards. J. A. Reeds, L. A. Shepp. Pacific J. Math. 145(2): 367-393 (1990).
    In 1990, the ReedShepp curve was proposed by JA Reeds, LA Shepp: Given a starting point and an end point, it can be connected by an arc and a straight line with a fixed radius. There are 48 combinations of arcs and straight lines. They proved that any combination of the shortest path is among the 48 combinations. They also gave the analytical solutions of 48 combinations and their solution methods.
Among them, $C$ stands for arc,$S$ stands for straight line, | stands for direction transformation

    However, this method cannot judge obstacles, so an obstacle detection step is required to filter out the path that collides with obstacles.

2.3 Solving process of Hybrid A* algorithm in Apollo

    The following is the solution process of the Hybrid A* algorithm in Apollo.
code segment

// /home/yuan/apollo-edu/modules/planning/open_space/coarse_trajectory_generator/hybrid_a_star.cc
grid_a_star_heuristic_generator_->GenerateDpMap(ex, ey, XYbounds_,
                                                  obstacles_linesegments_vec_);
  ADEBUG << "map time " << Clock::NowInSeconds() - map_time;
  // load open set, pq
  open_set_.emplace(start_node_->GetIndex(), start_node_);
  open_pq_.emplace(start_node_->GetIndex(), start_node_->GetCost());
  // Hybrid A* begins
  size_t explored_node_num = 0;
  double astar_start_time = Clock::NowInSeconds();
  double heuristic_time = 0.0;
  double rs_time = 0.0;
  while (!open_pq_.empty()) {
    
    
    // take out the lowest cost neighboring node
    const std::string current_id = open_pq_.top().first;
    open_pq_.pop();
    std::shared_ptr<Node3d> current_node = open_set_[current_id];
    // check if an analystic curve could be connected from current
    // configuration to the end configuration without collision. if so, search
    // ends.
    const double rs_start_time = Clock::NowInSeconds();
    if (AnalyticExpansion(current_node)) {
    
    
      break;
    }
    const double rs_end_time = Clock::NowInSeconds();
    rs_time += rs_end_time - rs_start_time;
    close_set_.emplace(current_node->GetIndex(), current_node);
    for (size_t i = 0; i < next_node_num_; ++i) {
    
    
      std::shared_ptr<Node3d> next_node = Next_node_generator(current_node, i);
      // boundary check failure handle
      if (next_node == nullptr) {
    
    
        continue;
      }
      // check if the node is already in the close set
      if (close_set_.find(next_node->GetIndex()) != close_set_.end()) {
    
    
        continue;
      }
      // collision check
      if (!ValidityCheck(next_node)) {
    
    
        continue;
      }
      if (open_set_.find(next_node->GetIndex()) == open_set_.end()) {
    
    
        explored_node_num++;
        const double start_time = Clock::NowInSeconds();
        CalculateNodeCost(current_node, next_node);
        const double end_time = Clock::NowInSeconds();
        heuristic_time += end_time - start_time;
        open_set_.emplace(next_node->GetIndex(), next_node);
        open_pq_.emplace(next_node->GetIndex(), next_node->GetCost());
      }
    }
  }

The first step is to solve the cost value of each two-dimensional node based on the dynamic programming algorithm, and use it as a heuristic function of the hybrid Astar GenerateDpMap.

grid_a_star_heuristic_generator_->GenerateDpMap(ex, ey, XYbounds_,
                                                  obstacles_linesegments_vec_);

Then define two collections, open collection, open_set_closed collection close_set_and a priority queue open_pq_. Save the open collection, and add the starting point to the open collection.

// load open set, pq
  open_set_.emplace(start_node_->GetIndex(), start_node_);
  open_pq_.emplace(start_node_->GetIndex(), start_node_->GetCost());

  close_set_.emplace(current_node->GetIndex(), current_node);

Then define a whlie loop, and pop up the node with the smallest cost value in each loop.

  while (!open_pq_.empty()) {
    
    
    // take out the lowest cost neighboring node
    const std::string current_id = open_pq_.top().first;
    open_pq_.pop();
    std::shared_ptr<Node3d> current_node = open_set_[current_id];

In the loop, whether there is a collision-free RS curve from the state of the current node to the state of the target node (judged by a AnalyticExpansionfunction), if an analytical solution is found, exit the loop directly

 if (AnalyticExpansion(current_node)) {
    
    
      break;
    }

If not, add the node to the close set

 close_set_.emplace(current_node->GetIndex(), current_node);

And expand ( Next_node_generator) the node to visit its adjacent nodes. And perform collision detection on the extended nodes.

for (size_t i = 0; i < next_node_num_; ++i) {
    
    
      std::shared_ptr<Node3d> next_node = Next_node_generator(current_node, i);
      // boundary check failure handle
      if (next_node == nullptr) {
    
    
        continue;
      }
      // check if the node is already in the close set
      if (close_set_.find(next_node->GetIndex()) != close_set_.end()) {
    
    
        continue;
      }
      // collision check
      if (!ValidityCheck(next_node)) {
    
    
        continue;
      }

If the node has not been traversed and has not collided with obstacles, add it to the open set and calculate the cost value of the node ( CalculateNodeCost)

      if (open_set_.find(next_node->GetIndex()) == open_set_.end()) {
    
    
        explored_node_num++;
        const double start_time = Clock::NowInSeconds();
        CalculateNodeCost(current_node, next_node);
        const double end_time = Clock::NowInSeconds();
        heuristic_time += end_time - start_time;
        open_set_.emplace(next_node->GetIndex(), next_node);
        open_pq_.emplace(next_node->GetIndex(), next_node->GetCost());
      }

Hybrid Astar + ReedShepp curve planning effect

It can be seen that some trajectories are not smooth, so the smoothing algorithm needs to be further smoothed.

3. Trajectory planning algorithm based on OBCA

3.1 Characteristics of OBCA algorithm

OBCA Algorithm
• The OBCA algorithm is based on Model Predictive Control (MPC) to establish a model and use an optimization algorithm to solve it. It
can add obstacle constraints
. It can realize horizontal and vertical joint planning
. It can generate trajectories that meet vehicle kinematics constraints
. References
1.TDR- OBCA: A Reliable Planner for Autonomous Driving in Free-SpaceEnvironment https://arxiv.org/abs/2009.11345 (Apollo improvement)
2. Optimization-Based Collision Avoidance https://arxiv.org/abs/1711.03449 (OBCA original paper)

3.2 Model Predictive Control MPC

    Model predictive control will discretize the future time domain into multiple breaks through a sampling time. Given the control model and reference value curve, calculate the input sequence that makes the predicted output closest to the reference output, and apply the first component of the input to system.

    Because the model predictive control problem is still to be transformed into an optimization problem in the end, it is necessary to design the objective function and constraint function; at the same time, MPC is also a prediction problem, so a prediction model needs to be established.

3.3 Model predictive control state equation

    Next, let's look at how the MPC model predictive control state equation is established. First define the state variables of the own vehicle, including the kk of the own vehicleCoordinates of time$k$$(x,y)$ , velocity$v$ , heading angle$\varphi$。$$x(k)=\left(\begin{array}{c}{x}_x(k)\\ x_y\left(k\right)\\ x_v\left(k\right)\\ x_{}(k)\end{array}\right)$$     and enter $u_k$,$u_k$Including the front wheel angle of the main vehicle $\delta \left(k\right)$ and acceleration$a(k)]$。$$u(k)=\left[d\right(k),a(k){]}^{The\; T}$$     motion control model is the two-degree-of-freedom motion model of the vehicle, and the following formula is$k+1$ time andkkthThe state correspondence at time$k$ :     In the control algorithm, what we need is$The\; first\; component\; of\; u$ ; in the trajectory algorithm, we need all states as the final output trajectory.

3.4 Using hyperplane to construct obstacle constraints

    The OBCA algorithm mainly considers the constraints of obstacles, and uses hyperplanes to construct the constraints of obstacles.
    After construction, it is represented by the following vector constraints:

• 当$x_1<x_2$时,$$\begin{array}{c}{A}_m=\left(\begin{array}{c}-a\\ 1\end{array}\right)\\ b_m=b\\ X=\left(\begin{array}{c}x\\ y\end{array}\right)\\ A_{m}X\le b_m\end{array}$$
• 当$x_1>x_2$时$$\begin{array}{c}{A}_m=\left(\begin{array}{c}a\\ -1\end{array}\right)\\ b_m=-b\\ X=\left(\begin{array}{c}x\\ y\end{array}\right)\\ A_{m}X\le b_m\end{array}$$

    By solving for $A$ and$The\; coordinates\; of\; B$ , that is, A m A_mcan be obtained$A_m$and $b_m$.For

    obstacles, which can be described by four sides in space, obstacle constraints can be expressed as:     where $$A^m=\{\begin{array}{c}{A}_{ab}\\ A_{bc}\\ A_{dc}\\ A_d\end{array},b^m=\{\begin{array}{c}{b}_{ab}\\ b_{bc}\\ b_{dc}\\ b_d\end{array}$$
    The representation of the self-vehicle is also similar, and it can also be constructed with a hyperplane. ego vehicle occupies space $B$ can be expressed as: B : = { e : G e ≤ g } \mathbb{B}: = \{ e:Ge \le g\}B:={ e:Ge≤g}    式中：$$G=\left[\begin{array}{c}1\\ 0\\ -1\\ 0\end{array}\begin{array}{c}0\\ 1\\ 0\\ -1\end{array}\right],g=\left[\begin{array}{c}\frac{1}{2}\\ \frac{}{2}\\ \frac{l}{2}\\ \frac{}{2}\end{array}\right]$$    car at $k$ time stateE \mathbb{E}The space occupied by$E\; can\; be\; obtained\; by\; rotation\; and\; translation:$

    $R\left(x\right(k))$ is the rotation matrix,$t\left(x\right(k))$ is the translation matrix.
    To meet the requirement that there is no collision between the occupied space of the main vehicle and the obstacle space, the intersection of the occupied space of the obstacle and the occupied space of the main vehicle is empty: at the same time, it is necessary to define the    minimum distance between the main vehicle and the obstacle ($dist$ is the function with the smallest norm when the host vehicle vector translates in any direction and coincides with the obstacle. Simply put, it is the smallest distance between the two sets): if you want to ensure that the distance from the obstacle > d_min,
    then :    $l_m,m_m$is the Lagrangian multiplier.

    Next, let's see how these formulas are obtained. First of all, the original problem is based on $The\; distance\; of\; the\; position\; of\; E$ from the obstacle.
    $e$ is the space occupied by the ego vehicleE ( x ) \mathbb{E}(x)Any point in$E$$($$x$$)$$o$ occupies space${\mathbb{O}}_m$Any point in , respectively satisfy the following constraints:
    the space occupied by the ego vehicle E ( x ) \mathbb{E}(x)Any point in$E$$($$x$$)$$o$ occupies space$O_m$The smallest Euclidean distance ( $and$ ,oooThe norm formed by$o$$d_{min}$, then there are:     here will $e$ becomes$R(x(k))e^{\prime }+t(x(k))$，$e^{\prime }$ is a point in the space occupied by the origin, and$the\; position\; of\; point\; e$ .
    Since the norm contains two optimization variables, it is not easy to calculate. So introduce a new optimization variable$w$和约束。$$w=R(x(k))e^{\prime }+t(x(k))-oSo$$     the problem is transformed into solving whenwwWhen the binorm of$w$$e^{\text{'}}$和$The\; value\; of\; o$ . However, in convex optimization problems, such conditional extremum problems are not well studied. Therefore, it is necessary to transform the conditional extreme value problem into an unconditional extreme value problem.
    We call the conditional extremum problem the original problem, expressed as follows:
    its Lagrangian function is:    through the Lagrangian function, the problem with constraints can be transformed into a dual problem without constraints.
    Next, put$L(x,l,\upsilon )$ definitionλ$l,\upsilon$ is regarded as a constant, and the solution is in$Taking\; the\; minimum\; value\; of\; the\; Lagrangian\; function\; in\; the\; domain\; of\; x$ , the dual function of the Lagrangian is obtained (the Lagrangian function is atxxThe infimum in the domain of$x$ ):     the dual problem of the original problem is the maximum value of the dual function of the Lagrangian function:    we generally define the original problem as$p^{\ast }$ , the dual problem is defined as$d^{\ast }$。
$$\begin{array}{l}\underset{x}{\min }{f}_0(x)=p^{\ast }\\ \underset{l,u}{\max }g(\lambda ,u)=d^{\ast }\end{array}$$

    For convex optimization problems, $$p^{\ast }\ge d^{\ast }$$

    Back to the original question:

    Introducing the Lagrangian variables $\mu ,\lambda ,z$ , then its Lagrangian dual function is

---

3.5 MPC constrained design

• Keep a certain distance between the planned trajectory and obstacles
  
• The starting point and end point of the planned trajectory satisfy the given state
  
• Iteration of states satisfies the kinematic constraints:
  
• The state quantity satisfies the programming limit ( $x$ ,$y$ ,$The\; value\; of\; v$ is limited, and the heading angle can be any value)
  
• The input volume meets the vehicle limit (maximum acceleration and yaw rate)
  

3.6 MPC objective function design

    The objective function of MPC is to predict each state x ( k ) x(k) in the time domainSummation of the loss function of$x$$($$k$$)$ :     The loss function has the following requirements:

• Track reference path changes: $x(k)\to x_{ref}$
• Acceleration as small as possible : $u(k)\downarrow$
• The first input component is as close as possible to the current state input : $u(1)\to \tilde{u}$
• The rate of change of the input quantity is as small as possible : $\dot{u}(k)\downarrow$

    The following formula is $costt$ cost function, each$cost$ are represented by two norms.

3.7 OBCA algorithm (DISTANCE_APPROACH_IPOPT_FIXED_TS)

    In this way, the programming problem of OBCA can be transformed into a nonlinear problem and solved by IPOPT.     The dual variables also need to be initialized to convert the maximum value problem into a minimum value problem, which is conducive to solving.
    In addition to OBCA (DISTANCE_APPROACH_IPOPT_FIXED_TS), Apollo has some derivative algorithms.

3.8 OBCA derivative algorithm

3.8.1 Variable sampling time ( DISTANCE_APPROACH_IPOPT )

    The original sampling time is $t_S$, no matter how optimized it is, it will reach the end at the last moment. Then add a sampling time coefficient $t\left(k\right)$ , the sampling time becomes$t_s\ast t\left(k\right)$ , thus shortening the sampling time.

3.8.2 End point relaxation ( DISTANCE_APPROACH_IPOPT_RELAX_END )

4. Path planning algorithm based on DL-IAPS

    Although the OBCA algorithm solves the constraint of no collision in open spaces and obstacles through the strong duality of convex optimization, the robustness and efficiency of the algorithm are low. Therefore, Apollo designed a horizontal and vertical decoupling algorithm.

Open Space Trajectory Planning Algorithm with Horizontal and Vertical Decoupling

Zhou J, He R, Wang Y, et al. Dl-iaps and pjso: A path/speed decoupled trajectory optimization and its application in autonomous driving[J]. arXiv preprint arXiv:2009.11135, 2020.

    The DL-IAPS algorithm is a segmented path planning algorithm. The OBCA algorithm is smoothed based on the complete path, while the DL-IAPS algorithm is based on the mixture $A^{\ast }$ For the generated path, judge whether the trajectory is moving forward or backward, and perform segmental smoothing, while ensuring that the smoothed path will not collide with obstacles and satisfy the maximum curvature constraint.
    The speed planning algorithm sampled by this algorithm is the PJSO algorithm, which is similar to the speed planning algorithm based on quadratic programming ( Apollo Spark Project Study Notes—Principles and Practices of Apollo Speed ​​Planning Algorithm ). The position, velocity and acceleration are sampled on the path planned by DL-IAPS, and are solved by quadratic programming.

4.1 DL-IAPS （Dual Loop Iterative Anchoring Path Smoothing）

Algorithm Pseudocode

    It mainly includes two layers of loops. The outer loop is a loop for processing and obstacle collision constraints, and the inner loop is a loop for path smoothing. For path planning problems in open spaces, the difficulty lies in the solution of obstacle constraints.

    DL-IAPS is similar to the smoothing algorithm of the reference line ( Apollo Spark Project Study Notes - Analysis and Implementation of the Reference Line Smoothing Algorithm (U-curve scene simulation debugging as an example) ) , but after each cycle, the results and All obstacles are checked for collision. If a collision occurs, the boundingbox rectangle is reduced and smoothed again, and so on until the constraint is satisfied.
    The reference line generally comes from the lane line of the map, so its curvature is not large. At the same time, because the number of points smoothed by the reference line is relatively large, the requirement for real-time performance is relatively high. The reference line smoothing algorithm adopts a quadratic Smoothing algorithm for planning.
    But for path planning in open space, the obtained reference path comes from the search results of hybrid Astar, and the curvature of the reference path itself is relatively large, so the curvature constraint needs to be taken into consideration.
    Apollo uses a SQP sequence quadratic programming algorithm to solve nonlinear constraint problems.
    First, linearize the constraint function and expand it to Taylor:

    Retaining the primary term, we get the following formula

    Obviously, here we need to know the constraint function in $X_0$The value of $F(X_0)$ and derivative value$F^{\prime }(X_0)$ . In sequential quadratic programming, the last smoothing result can be used as the reference point this time, and a credible interval constraint (the following formula) is added to the optimization point to avoid too far constraints between the points of the two plannings.

    Simplify the above problem into a quadratic programming problem. Its objective function contains two terms,the first term is the cost of adjacent smoothness, andthe second term is the slack variable of the curvature constraint.

    The figure below shows the overall architecture of Open Space Planner. Firstly, the drivable area is determined by Open Space Decider, and then a rough trajectory is searched by the hybrid Astar algorithm. Then use the DL-IAPS algorithm to smooth the curvature of the obtained rough trajectory, and obtain a path that satisfies the curvature constraint and has no collision. Then, speed planning is performed on the smoothed path, and finally a trajectory is generated.

    Comparison between algorithms. The improved algorithm has the advantage of increasing efficiency.

5. Open space planning algorithm practice

Cloud Experiment Address - Open Space Planning of Apollo Planning

Start DreamView first

bash scripts/bootstrap.sh

Mode selection Mkz Standard Debug, map selection ApolloVirutal Map, open Sim_Controlmode, open PNCMonitor, wait for the Mkz car model and map to appear in the middle area of ​​the screen, which means successfully entering the simulation mode.

Click Module Controller on the left Tab column to start the Planning, Prediction, Routing, and TaskManager modules. If you need to record data, open the Recorder module.

5.1 Practice of Autonomous Parking Scenarios

After the module is started, find the parking space in the middle of the map, select a point on the road in front of the parking space and one of the parking spaces as the end points, and click Send Request to issue a routing request. In the pnc monitor on the right, you can see the set drivable area boundary, the warm start curve is the planned rough trajectory (mixed Astar), and the smooth is the smoothed curve.

Autonomous parking goes through the following three stages.

Trajectory of the OBCA algorithm

Adjust the weight of the path planning_config.pb.txtfollowed distance approachby the algorithm , , , restart the planning algorithm, and observe the difference in the planning trajectory.warm startweight_xweight yweight_phi

        distance_approach_config {
    
    
          weight_steer: 0.3
          weight_a: 1.1
          weight_steer_rate: 3.0
          weight_a_rate: 2.5
          weight_x: 0.0//改为2.0
          weight_y: 0.0//改为2.0
          weight_phi: 0.0//改为2.0
          weight_v: 0.0

It can be seen that the trajectory is closer to the trajectory planned by the hybrid Astar.

planning.confSet use_iterative_anchoring_smootherit to true, enable_parallel_trajectory_smoothingset it to true, use the DL-IAPS algorithm to smooth, and observe the difference in the planned trajectory

--enable_smoother_failsafe
--enable_parallel_trajectory_smoothing=true
--nouse_s_curve_speed_smooth
--use_iterative_anchoring_smoother=true

The DL-IAPS algorithm directly smoothes the first segment into a straight line.

5.2 Pull over scene practice:

Change planning.conffrom enable_scenario_pull_overfalse to true to open the side parking scene

Select San Mateo for the map, reopen the Routing, planning, prediction, and taskmanager modules, and select the side parking scene in scenario_set for simulation. Because

there are obstacles around, the planned trajectory is different.

5.3 python script experiment

Open the notebook and enter the following command.

%matplotlib notebook
run modules/tools/open_space_visualization/distance_approach_visualizer.py

Vertical parking operation



script code

#!/usr/bin/env python3

###############################################################################
# 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.
###############################################################################

import math
import time
import sys
from matplotlib import animation
import matplotlib.patches as patches
import matplotlib.pyplot as plt
import numpy as np

from distance_approach_python_interface import *


result_file = "/tmp/open_space_osqp_ipopt.csv"


# def SmoothTrajectory(visualize_flag):
def SmoothTrajectory(visualize_flag, sx, sy, sphi):
    # initialze object
    OpenSpacePlanner = DistancePlanner()

    # parameter(except max, min and car size is defined in proto)
    num_output_buffer = 10000
    #sphi = 0.0
# 更改scenario，进行不同场景下的泊车
    scenario = "backward"
    #scenario = "parallel"
    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]
    if scenario == "parallel":
        left_boundary_x = [-13.64, 0.0, 0.0]
        left_boundary_y = [0.0, 0.0, -2.82]
        down_boundary_x = [0.0, 9.15]
        down_boundary_y = [-2.82, -2.82]
        right_boundary_x = [9.15, 9.15, 16.35]
        right_boundary_y = [-2.82, 0.0, 0.0]
        up_boundary_x = [16.35, -13.64]
        up_boundary_y = [5.60, 5.60]
    bound_x = left_boundary_x + down_boundary_x + right_boundary_x + up_boundary_x
    bound_y = left_boundary_y + down_boundary_y + right_boundary_y + up_boundary_y
    bound_vec = []
    for i in range(0, len(bound_x)):
        bound_vec.append(bound_x[i])
        bound_vec.append(bound_y[i])
    if scenario == "backward":
        # obstacles for distance approach(vertices coords in clock wise order)
        ROI_distance_approach_parking_boundary = (
            c_double * 20)(*bound_vec)
        OpenSpacePlanner.AddObstacle(
            ROI_distance_approach_parking_boundary)
        # parking lot position
        ex = 1.359
        ey = -3.86443643718
        ephi = 1.581
        XYbounds = [min(bound_x), max(bound_x), min(bound_y), max(bound_y)]

    if scenario == "parallel":
        # obstacles for distance approach(vertices coords in clock wise order)
        ROI_distance_approach_parking_boundary = (
            c_double * 20)(*bound_vec)
        OpenSpacePlanner.AddObstacle(
            ROI_distance_approach_parking_boundary)
        # parking lot position
        ex = 3.29
        ey = -1.359
        ephi = 0
        XYbounds = [min(bound_x), max(bound_x), min(bound_y), max(bound_y)]

    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)
    hybrid_time = (c_double * 1)(0.0)
    dual_time = (c_double * 1)(0.0)
    ipopt_time = (c_double * 1)(0.0)

    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:
        # 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,
                                           hybrid_time, dual_time, ipopt_time)
        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, figsize = [9,6])
        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')
            #ax.add_patch(warm_start_car)
            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_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.plot(x_out, y_out, "r")
            ax.plot(opt_x_out, opt_y_out, "y")
            ax.add_patch(smoothing_car)
            # ax.add_patch(smoothing_arrow)

        ax.plot(sx, sy, "s")
        ax.plot(ex, ey, "s")
        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, figsize = [9,6])
        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('phi')
        steer_graph.plot(np.linspace(0, size[0], size[0]), opt_phi_out)
        # dual variables
        fig3 = plt.figure(3, figsize = [9,6])
        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()
        return True

    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,
                                               hybrid_time, dual_time, ipopt_time)
            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]))
            # check end_pose distacne
            end_pose_dist = math.sqrt((opt_x_out[-1] - ex)**2 + (opt_y_out[-1] - ey)**2)
            end_pose_heading = abs(opt_phi_out[-1] - ephi)
            reach_end_pose = (end_pose_dist <= 0.1 and end_pose_heading <= 0.17)
        else:
            end_pose_dist = 100.0
            end_pose_heading = 100.0
            reach_end_pose = 0
        return [success, end_pose_dist, end_pose_heading, reach_end_pose, opt_x_out, opt_y_out, opt_phi_out, opt_v_out, opt_a_out, opt_steer_out, opt_time_out,
                hybrid_time, dual_time, ipopt_time, planning_time]
    return False


if __name__ == '__main__':
    visualize_flag = True
    sx = 0.0
    sy = 3
    sphi = 0.0
    SmoothTrajectory(visualize_flag, sx, sy, sphi)
    sys.exit()
    visualize_flag = False
    planning_time_stats = []
    hybrid_time_stats = []
    dual_time_stats = []
    ipopt_time_stats = []
    end_pose_dist_stats = []
    end_pose_heading_stats = []

    test_count = 0
    success_count = 0
    for sx in np.arange(-10, 10, 1.0):
        for sy in np.arange(2, 4, 0.5):
            print("sx is " + str(sx) + " and sy is " + str(sy))
            test_count += 1
            result = SmoothTrajectory(visualize_flag, sx, sy, sphi)
            # if result[0] and result[3]:  # success cases only
            if result[0]:
                success_count += 1
                planning_time_stats.append(result[-1])
                ipopt_time_stats.append(result[-2][0])
                dual_time_stats.append(result[-3][0])
                hybrid_time_stats.append(result[-4][0])
                end_pose_dist_stats.append(result[1])
                end_pose_heading_stats.append(result[2])

    print("success rate is " + str(float(success_count) / float(test_count)))
    print("min is " + str(min(planning_time_stats)))
    print("max is " + str(max(planning_time_stats)))
    print("average is " + str(sum(planning_time_stats) / len(planning_time_stats)))
    print("max end_pose_dist difference is: " + str(max(end_pose_dist_stats)))
    print("min end_pose_dist difference is: " + str(min(end_pose_dist_stats)))
    print("average end_pose_dist difference is: " +
          str(sum(end_pose_dist_stats) / len(end_pose_dist_stats)))
    print("max end_pose_heading difference is: " + str(max(end_pose_heading_stats)))
    print("min end_pose_heading difference is: " + str(min(end_pose_heading_stats)))
    print("average end_pose_heading difference is: " +
          str(sum(end_pose_heading_stats) / len(end_pose_heading_stats)))

    module_timing = np.asarray([hybrid_time_stats, dual_time_stats, ipopt_time_stats])
    np.savetxt(result_file, module_timing, delimiter=",")

    print("average hybrid time(s): %4.4f, with max: %4.4f, min: %4.4f" % (
        sum(hybrid_time_stats) / len(hybrid_time_stats) / 1000.0, max(hybrid_time_stats) / 1000.0,
        min(hybrid_time_stats) / 1000.0))
    print("average dual time(s): %4.4f, with max: %4.4f, min: %4.4f" % (
        sum(dual_time_stats) / len(dual_time_stats) / 1000.0, max(dual_time_stats) / 1000.0,
        min(dual_time_stats) / 1000.0))
    print("average ipopt time(s): %4.4f, with max: %4.4f, min: %4.4f" % (
        sum(ipopt_time_stats) / len(ipopt_time_stats) / 1000.0, max(ipopt_time_stats) / 1000.0,
        min(ipopt_time_stats) / 1000.0))

Parallel parking run (failed, there are still some problems with the script)