planning: add open space algorithm documents

planning: add open space algorithm documents

planning: update the documents

planning: update the documents

docs/technical_documents/hybrid_a_star_en.md

+# GENERATE COARSE TRAJECTORY IN THE OPEN SPACE
+
+# Introduction
+The goal of htbrid_a_star is to generate the coarse trajectory in the open space. Hybrid_a_star contains node3d， grid_search， reeds_shepp_path and hybrid_a_star. hybrid_a_star is the most important component generating the coarse trajectory and call the grid_search and reeds_shepp_path.
+
+# Where is the code
+Please refer to [hybrid a star.cc](https://github.com/ApolloAuto/apollo/tree/master/modules/planning/open_space/coarse_trajectory_generator/hybrid_a_star.cc)
+
+# Code Reading
+1. Input: current point(planned start point), goal point(planned end point), ROI_xy_boundary(the maximum and minimum boundary value of x and y), obstacles vertices vector(the corner position information). The function is follow:
+   ```  cpp
+        bool HybridAStar::Plan(double sx, double sy, double sphi, 
+                              double ex, double ey, double ephi,
+                              const std::vector<double>& XYbounds,
+                              const std::vector<std::vector<common::math::Vec2d>>& obstacles_vertices_vec,HybridAStartResult* result)
+   ```
+The input HybridAStar::Plan() is called by the open_space_trajectory_provider.cc, please refer to [open_space_trajectory_provider.cc](https://github.com/ApolloAuto/apollo/blob/master/modules/planning/tasks/optimizers/open_space_trajectory_generation/open_space_trajectory_provider.cc)
+
+2. Construct obstacles_linesegment_vector. The main method is to form a line segment from a single obstacle point in order; then, each obstacle line segment is stored in obstacles_linesegment_vector that will be used to generate the DP map.
+    ``` cpp
+        std::vector<std::vector<common::math::LineSegment2d>>
+        obstacles_linesegments_vec;
+        for (const auto& obstacle_vertices : obstacles_vertices_vec) {
+            size_t vertices_num = obstacle_vertices.size();
+            std::vector<common::math::LineSegment2d> obstacle_linesegments;
+            for (size_t i = 0; i < vertices_num - 1; ++i) {
+            common::math::LineSegment2d line_segment = common::math::LineSegment2d(
+                obstacle_vertices[i], obstacle_vertices[i + 1]);
+            obstacle_linesegments.emplace_back(line_segment);
+            }
+            obstacles_linesegments_vec.emplace_back(obstacle_linesegments);
+        }
+        obstacles_linesegments_vec_ = std::move(obstacles_linesegments_vec);
+    ```
+3. Construct the planned point same as Node3d, please refer to [node3d.h](https://github.com/ApolloAuto/apollo/blob/master/modules/planning/open_space/coarse_trajectory_generator/node3d.h). The planned starting point and the ending point are constructed in the form of Node3d that will be save to open set and will be checked by the ValidityCheck() function.
+    ``` cpp
+        start_node_.reset(
+        new Node3d({sx}, {sy}, {sphi}, XYbounds_, planner_open_space_config_));
+        end_node_.reset(
+        new Node3d({ex}, {ey}, {ephi}, XYbounds_, planner_open_space_config_));
+    ```
+
+4. Enter the main while loop to get a set of nodes.
+   1. Exit trajectory generation when open_pq_ is empty. The open_pq_ is a std::priority_queue type that the first element represents the order of nodes in the open set, the second element represents the cost of nodes which storage in descending order. 
+   2. Use AnalyticExpansion() function to determine whether there is a collision free trajectory that based on the reeds_shepp_path from current point to target end point. if it exists, exit the while loop search.
+      ``` cpp
+        if (AnalyticExpansion(current_node)) {
+            break;
+        }
+      ```
+   3. Store the current point in the close set and Expand the next node according to the bicycle kinematics model. The number of nodes is a parameter. Generate the next node by Next_node_generator() function and use ValidityCheck() function to detect this node.
+
+5. Generate the coarse trajectory by nodes. The GetResult() function is used to generate the coarse trajectory.
+   ``` cpp
+   bool HybridAStar::GetResult(HybridAStartResult* result)
+   ```
+6. Output: The optput is partial trajectory information, which include x,y,phi,v,a,steer,s.
+
+# Algorithm Detail
+   ``` cpp
+   bool HybridAStar::ValidityCheck(std::shared_ptr<Node3d> node)
+   ```
+   The detection method is based on boundary range judgment and boundary overlap judgment.
+1. Parameter: the input parameter is node which same as node3d. 
+2. Introduction: the function is used to check for collisions. 
+3. Process detail: 
+   1. Boundary range judgment. If the x and y of node exceed the range of the corresponding x and y of boundary, then return false,         reprents invalid. 
+   2. Boundary overlap judgment. If the bounding box of vehicle overlaps any line segment, then return false. Judge the overlap by          whether the line and box intersect.
+
+    ``` cpp
+    bool GridSearch::GenerateDpMap(const double ex, const double ey, 
+                                   const std::vector<double>& XYbounds,
+                                   const std::vector<std::vector<common::math::LineSegment2d>> &obstacles_linesegments_vec) 
+    ```
+    the function is used to generate dp map by dynamic programming, please refer (https://github.com/ApolloAuto/apollo/blob/master/modules/planning/open_space/coarse_trajectory_generator/grid_search.cc)
+1. Parameter: ex and ey are the postion of goal point, XYbounds_ is the boundary of x and y, obstacles_linesegments_ is the line            segments which is composed of boundary point.
+2. Introduction: the function is used to generate the dp map
+3. Process detail: 
+   1. Grid the XYbounds_ according to grid resolution, then get the max grid.
+   2. Dp map store the cost of node.
+
+    ``` cpp
+    bool HybridAStar::AnalyticExpansion(std::shared_ptr<Node3d> current_node)
+    ```
+    The function is used to check if an analystic curve could be connected from current configuration to the end configuration without collision. if so, search ends.
+1. Parameter: current node is start point of planning.
+2. introduction: the function based on the reeds shepp method which is a geometric algorithm composed of arc and line. Reeds shepp is       used for search acceleration. 
+3. Process detail:
+   1. Generate the reeds shepps path by the ShortestRSP() function. The length is optimal and shortest.
+   2. Check the path is collision free by the RSPCheck() function which call the ValidityCheck() function.
+   3. Load the whole reeds shepp path as nodes and add nodes to the close set.
+
+    ``` cpp
+    bool HybridAStar::AnalyticExpansion(std::shared_ptr<Node3d> current_node)
+    ```
+    The funtion is used to generate next node based on the current node.
+1. Parameter: the current node of the search and the next node serial number 
+2. Introduction: the next node is calculated based on steering wheel uniform sampling and vehicle kinematics.
+3. Process detail: 
+   1. The steering angle is calculated according to the number and order of sampling points.
+   2. Generate the next node according to the kinematic model.
+   3. Check if the next node runs outside of XY boundary.
+
+    ``` cpp
+    void HybridAStar::CalculateNodeCost(std::shared_ptr<Node3d> current_node,
+                                        std::shared_ptr<Node3d> next_node)
+    ```
+   The function is used to calculate the cost of node.
+1. Parameter: current node(vehicle position) and next node.
+2. Introduction: the calculated cost include trajectory cost and heuristic cost considering obstacles based on holonomic.
+3. Process detail: 
+   1. the trajectory cost include the current node's trajectory cost and the trajectory cost from current node to next node.
+   2. trajectory cost is determined by the sampling distance, the gear change between them and the steering change rate. 
+   3. heuristic cost is get from the dp map.
\ No newline at end of file

docs/technical_documents/open_space_trajectory_optimizer_en.md

+# OPTIMIZE COARSE TRAJECTORY
+
+# Introduction
+The goal of this part is to optimizes the initial trajectory in the open space. Open_space_trajectory_optimizer is able to call a variety of different optimization algorithms.
+
+# Where is the code
+Please refer [open_space_trajectory_optimizer.cc](https://github.com/ApolloAuto/apollo/tree/master/modules/planning/tasks/optimizers/open_space_trajectory_generation/open_space_trajectory_optimizer.cc)
+
+# Code Reading
+1. Input: stitching trajectory is provided by the open_space_trajectory_provider, planned target point, boundary of x and y, rotation angle relative to the corner of parking space, the reference origin point, line segment of boundary.
+   ``` cpp
+   Status OpenSpaceTrajectoryOptimizer::Plan(const std::vector<common::TrajectoryPoint>& stitching_trajectory,
+                                             const std::vector<double>& end_pose, 
+                                             const std::vector<double>& XYbounds,
+                                             double rotate_angle, 
+                                             const Vec2d& translate_origin,
+                                             const Eigen::MatrixXi& obstacles_edges_num,
+                                             const Eigen::MatrixXd& obstacles_A, 
+                                             const Eigen::MatrixXd& obstacles_b,
+                                             const std::vector<std::vector<Vec2d>>& obstacles_vertices_vec,
+                                             double* time_latency)
+   ```
+
+2. Before optimization, some unreasonable cases are exited from the optimization process and implement some preprocessing. 
+   1. The unreasonable case is below: 
+      1. Input data is empty.
+      ``` cpp
+      if (XYbounds.empty() || end_pose.empty() || obstacles_edges_num.cols() == 0 ||
+          obstacles_A.cols() == 0 || obstacles_b.cols() == 0) {
+          ADEBUG << "OpenSpaceTrajectoryOptimizer input data not ready";
+          return Status(ErrorCode::PLANNING_ERROR, "OpenSpaceTrajectoryOptimizer input data not ready");
+      }
+      ```
+      2. Starting point of planning is near the end point.
+      ``` cpp
+        if (IsInitPointNearDestination(stitching_trajectory.back(), end_pose,
+                                        rotate_angle, translate_origin)) {
+            ADEBUG << "Planning init point is close to destination, skip new "
+                    "trajectory generation";
+            return Status(ErrorCode::OK,
+                        "Planning init point is close to destination, skip new "
+                        "trajectory generation");
+        }
+      ```
+      3. End of the stitching trajectory is rotated and translated, and the trajectory information is converted according to the corner     of the parking space.
+      ``` cpp
+      PathPointNormalizing(rotate_angle, translate_origin, &init_x, &init_y,
+                            &init_phi)
+      ```
+3. Generate the coarse trajectory based on the warm start technology which is Hybrid A* algorithm.
+   ``` cpp
+   if (warm_start_->Plan(init_x, init_y, init_phi, end_pose[0], end_pose[1],
+                        end_pose[2], XYbounds, obstacles_vertices_vec,
+                        &result)) {
+    ADEBUG << "State warm start problem solved successfully!";
+    } else {
+        ADEBUG << "State warm start problem failed to solve";
+        return Status(ErrorCode::PLANNING_ERROR,
+                    "State warm start problem failed to solve");
+    }
+   ```
+4. According to FLAGS_enable_parallel_trajectory_smoothing to achieve different optimization process. When                                  FLAGS_enable_parallel_trajectory_smoothing is false, the optimization process is as follows: 
+   1. (x, y, phi, V) and (ster, a) of initial trajectory points in hybrid_a_star are stored into xws and UWS respectively through           LoadHybridAstarResultInEigen() function, and xws and UWS are used to generate the subsequent smooth trajectory.
+   2. Generate the smooth trajectory by the GenerateDistanceApproachTraj() function.
+    ``` cpp
+        LoadHybridAstarResultInEigen(&result, &xWS, &uWS);
+
+        const double init_steer = trajectory_stitching_point.steer();
+        const double init_a = trajectory_stitching_point.a();
+        Eigen::MatrixXd last_time_u(2, 1);
+        last_time_u << init_steer, init_a;
+
+        const double init_v = trajectory_stitching_point.v();
+
+        if (!GenerateDistanceApproachTraj(
+                xWS, uWS, XYbounds, obstacles_edges_num, obstacles_A, obstacles_b,
+                obstacles_vertices_vec, last_time_u, init_v, &state_result_ds,
+                &control_result_ds, &time_result_ds, &l_warm_up, &n_warm_up,
+                &dual_l_result_ds, &dual_n_result_ds)) {
+        return Status(ErrorCode::PLANNING_ERROR,
+                        "distance approach smoothing problem failed to solve");
+        }
+    ```
+
+5. When FLAGS_enable_parallel_trajectory_smoothing is true, the optimization process is as follows: 
+   1. Trajectorypartition() function is used to segment the initial trajectory.
+   2. Use loadhybridastarresultineigen() function to store the partitioned trajetory into xws and UWS respectively.
+   3. Set the initial information(a,V) of each trajectory.
+   4. the initial information of the first trajectory is the end point of the stitching trajectory.
+   5. In the next trajectory, the initial information is set to zero. At the start of the trajectory, the vehicle is stationary. 
+    ``` cpp
+    if (!warm_start_->TrajectoryPartition(result, &partition_trajectories)) {
+        return Status(ErrorCode::PLANNING_ERROR, "Hybrid Astar partition failed");
+    }
+    ```
+6. Use combinetrajectories() function to integrate the parameter information after segmented optimization.
+   ``` cpp
+   CombineTrajectories(xWS_vec, uWS_vec, state_result_ds_vec,
+                       control_result_ds_vec, time_result_ds_vec,
+                       l_warm_up_vec, n_warm_up_vec, dual_l_result_ds_vec,
+                       dual_n_result_ds_vec, &xWS, &uWS, &state_result_ds,
+                       &control_result_ds, &time_result_ds, &l_warm_up,
+                       &n_warm_up, &dual_l_result_ds, &dual_n_result_ds)
+   ```
+7. Converting trajectory information to world coordinate system.
+   ``` cpp
+   for (size_t i = 0; i < state_size; ++i) {
+        PathPointDeNormalizing(rotate_angle, translate_origin,
+                               &(state_result_ds(0, i)), 
+                               &(state_result_ds(1, i)),
+                               &(state_result_ds(2, i)));
+  }
+   ```
+8. The trajectory information is loaded by loadtrajectory() function. Because the current optimization does not consider the end point control state, the end-point control state of the trajectory is processed (Steer = 0, a = 0).
+    ``` cpp
+    LoadTrajectory(state_result_ds, control_result_ds, time_result_ds)
+    ```
+
+9. Output: Optput is optimized trajectory information.
+
+
+# Algorithm Detail
+    ```cpp
+    LoadHybridAstarResultInEigen(&partition_trajectories[i], &xWS_vec[i],&uWS_vec[i])
+    ```
+    The function is to transform the initial trajectory information into the form needed for optimization.
+1. Parameter: the initial trajectory and parameter matrix.
+2. Introduction: the trajectory information is transformed into matrix form.
+3. Process detail: 
+   1. Transform the x,y,phi,v,steer to the matrix combined with horizon.
+   2. Store the transformed information to the matrix.
+

docs/technical_documents/open_space_trajectory_provider_en.md

+# GENERATE FINAL TRAJECTORY
+
+# Introduction
+The goal of this part is to generate the final trajectory in the open space. Open_space_trajectory_provider is very important to control the flow and call the hybrid a star and trajectory smoothing algorithm. 
+
+# Where is the code
+Please refer to [open_space_trajectory_provider.cc](https://github.com/ApolloAuto/apollo/tree/master/modules/planning/tasks/optimizers/open_space_trajectory_generation/open_space_trajectory_provider.cc)
+
+# Code Reading
+1. Input: open_space_trajectory_provider::Process() is called by the OPEN_SPACE_TRAJECTORY_PROVIDER task of VALET_PARKING_PARKING stage, please refer to [valet_parking_config.pb.txt](https://github.com/ApolloAuto/apollo/blob/master/modules/planning/conf/scenario/valet_parking_config.pb.txt).
+
+2. There is a stop trajectory which generated in the park and go check stage. In order to ensure safety, it is necessary in this case. 
+    ``` cpp
+    if (injector_->planning_context()
+          ->mutable_planning_status()
+          ->mutable_park_and_go()
+          ->in_check_stage()) {
+        ADEBUG << "ParkAndGo Stage Check.";
+        GenerateStopTrajectory(trajectory_data);
+        return Status::OK();
+    }
+    ```
+3. Start thread when getting in Process() for the first time. This will call the GenerateTrajectoryThread() function to plan the first trajectory and will update three kinds of trajectory state: trajectory_updated_, trajectory_skipped_, trajectory_error_.
+    ``` cpp
+    if (FLAGS_enable_open_space_planner_thread && !thread_init_flag_) {
+        task_future_ = cyber::Async(&OpenSpaceTrajectoryProvider::GenerateTrajectoryThread, this);
+        thread_init_flag_ = true;
+    }
+    ```
+4. Whether vehicle is stoped due to fallback is determined by the IsVehicleStopDueToFallBack() function. This determines the final trajectory planning.
+5. If vehicle is stopped due to fallback, replan stitching trajectory by ComputeReinitStitchingTrajectory() function. If not, replan stitching trajectory by ComputeStitchingTrajectory(), please refer to [trajectory_stitcher.cc](https://github.com/ApolloAuto/apollo/blob/master/modules/planning/common/trajectory_stitcher.cc).
+6. Generate trajectory depends on the FLAGS_enable_open_space_planner_thread. A stop trajectory is generated in the following cases:
+   1. Planning thread is stopped. 
+   2. The vehicle arrives near the destination. 
+   3. trajectory_error_ is triggered for more than 10 seconds.
+   4. Previous frame planning failed. 
+   5. If the trajectory can be updated normally, the optimized trajectory is output normally. 
+
+7. Output: the optput is final trajectory information.
+
+# Algorithm Detail
+    ``` cpp
+    bool OpenSpaceTrajectoryProvider::IsVehicleStopDueToFallBack(const bool is_on_fallback, 
+                                                                 const common::VehicleState& vehicle_state)
+    ```
+    The function is used to judge whether the vehicle is stopped due to fallback.
+1. Parameter: The input parameter is fallback flag of previous frame and vehicle states.
+2. Introduction: The flag and vehicle state can be used to design the logic. 
+3. Process detail: 
+    1. Fallback flag judgment: if the flag is false, then return false. 
+    2. When the vehicle speed and acceleration are less than the threshold, the result is true, indicating that it is caused by fall        back.
+
+    ``` cpp
+    std::vector<TrajectoryPoint> TrajectoryStitcher::ComputeStitchingTrajectory(const VehicleState& vehicle_state, 
+                                                                                const double current_timestamp,
+                                                                                const double planning_cycle_time, 
+                                                                                const size_t preserved_points_num,
+                                                                                const bool replan_by_offset, 
+                                                                                const PublishableTrajectory* prev_trajectory,
+                                                                                std::string* replan_reason)
+    ```
+    The function is used to stitch trajectory and is used to replan based on some unreasonable case.
+1. Parameter: Vehicle state, current_timestamp, planning cycle time, replan_by_offset, previous trajectory and the reason of replanning.
+2. Introduction: Handle some unreasonable case by replanning, stitch trajectory and post process. 
+3. Process detail: 
+   1. It will re-plan the trajectory in following cases:
+      1. Stitching trajectory is disabled by gflag.
+      2. There is no previous trajectory.
+      3. Not in autopilot mode. 
+      4. The number of points in the previous frame is zero. 
+      5. The current time is less than the trajectory start time of the previous frame.
+      6. The current time is more than the trajetory end time of the previous frame.
+      7. The matching path point is empty. 
+      8. The horizontal and vertical deviation of the projection point is greater than the threshold. 
+    2. Stitch trajectory according to the planning period and the position of the projection point.
+    3. Determine whether each trajectory point of the stitching trajectory is empty, and if it is empty, it will replan.
+
+    ``` cpp
+    std::vector<TrajectoryPoint>TrajectoryStitcher::ComputeReinitStitchingTrajectory(const double planning_cycle_time, 
+                                                                                     const VehicleState& vehicle_state)
+    ```
+    The function is used to initialize stitching trajectory and get the initial point. 
+1. Parameter: The planned cycle time and vehicle state
+2. Introduction: The function can get the diffrent initial point based on the different logic.
+3. Process detail:
+   1. When the vehicle speed and acceleration are less than the threshold, the message of initial point is from vehicle state.           2. When the vehicle speed and acceleration satisfy the threshold, the vehicle state is calculated based on the kinematics model.
+
+