Ego planner code parsing Bspline_ Optimizer section (1)

initControlPoints

From the beginning ( Or start at the current position ) For the former 2/3 The line segment formed by each control point of is represented by step_size Check whether there are obstacles at each point on the line segment for the step .

1、 If There are obstacles at this point , There is no obstacle at the last point

If there are no obstacles at more than two consecutive points or the first segment , be Enter the obstacle sign location true

Record the trajectory of obstacles in the control section ( The first i paragraph ), The corresponding control point in front of the obstacle in_id = i - 1, That is the second i-1 Control points ;

There is... In succession / The barrier free counter is set 0;

The obstacle end sign is set false;

2、 If There are no obstacles at this point , There is an obstacle at the last point

Record the trajectory of the obstacle in the control section ( The first i paragraph ), The corresponding first control point out of the obstacle out_id=i, That is the second i-1 Control points ;

There is... In succession / The barrier free counter is set 0;

Possible obstacle sign location true;

3、 If This point is different from the previous point / The situation without obstacles is the same , Then continuous / The barrier free counter adds 1

If there is an obstacle, the sign position is true;

And there are obstacles at more than two consecutive points before, or the current trajectory segment is the last segment

Then the position of the obstacle sign may appear false, The position of the obstacle sign true.

If you enter and exit obstacles at the same time, the sign position is true, Then the sign position false, Control the obstacles to (in_id,out_id) Push the segment_ids( Obstacle section ).

notes ： Each obstacle segment is a pair of control points , That is, the first control point before entering the obstacle and the first control point after exiting the obstacle ,( The code corresponds to (in_id,out_id)).

 std::vector<std::vector<Eigen::Vector3d>> BsplineOptimizer::initControlPoints(Eigen::MatrixXd &init_points, bool flag_first_init /*= true*/)
  {// Initialize the control point （ The control points , First initialization flag ）
  //MatrixXd Indicates that the element type of any size is double Matrix variable of , Its size can only be known after it is assigned at run time 

    if (flag_first_init)// First initialization 
    {
      cps_.clearance = dist0_;//  A safe distance from 
      cps_.resize(init_points.cols());
      //init_points Initialize the number of columns of the control point matrix cols(), That is, at the beginning of initialization, there are several control points 
      //cps Is the dynamic matrix of control points , Can pass resize() Function to dynamically modify the size of the matrix 
      // Now its size is the number of columns of the initialization control point matrix 
      cps_.points = init_points;// The control points initialized at the first initialization are all control points 
    }


    /*** Segment the initial trajectory according to obstacles ***/
    // Segment the initial trajectory according to the obstacles 
    constexpr int ENOUGH_INTERVAL = 2;// Continue to control points ( Judge whether you are continuously in an obstacle or not ）
    //constexpr Constant expression 
    double step_size = grid_map_->getResolution() / ((init_points.col(0) - init_points.rightCols(1)).norm() / (init_points.cols() - 1)) / 2;
    //col(0) Inside is the coordinates of the point xyz,rightCols(1) Is the last column of the matrix 
    //（ First control point init_points.col(0) Location - The last control point init_points.rightCols(1) Location ） The norm of 
    // That is, find the distance between the first control point and the last control point 
    //(init_points.cols() - 1)) Is the number of control points -1
    //step_size Expected step ： The resolution of the / distance /（ Number of control points -1）/2
    // And the distance /（ Number of control points -1） That is, the distance between the control point and the control point 
    // Purpose ：step_size Check whether there are obstacles at each point on the line segment for the step 

    int in_id, out_id;// Enter the control point of the obstacle  , The control point of the obstacle 
    vector<std::pair<int, int>> segment_ids;// Control section ( The starting control point of this control end in_id And end control points out_id)
    //segment_ids by std::pair<int, int>(in_id, out_id)
    int same_occ_state_times = ENOUGH_INTERVAL + 1;// There is... In succession / Barrier free counter 
    bool occ, last_occ = false;// There are obstacles at this point occ,last_occ Previous point obstacle 
    bool flag_got_start = false, flag_got_end = false, flag_got_end_maybe = false;
    // Enter the obstacle sign location , The position of the obstacle sign , Possible obstacle sign location 
    int i_end = (int)init_points.cols() - order_ - ((int)init_points.cols() - 2 * order_) / 3; 
    //i_end For the purpose of controlling the total number of points 2/3 Again -1 It's about .
    // only check closed 2/3 points. Check only the adjacent 2/3 Control points for .

    for (int i = order_; i <= i_end; ++i)//i For track segment 
    {
      for (double a = 1.0; a >= 0.0; a -= step_size)
      // For the former 2/3 The line segment formed by each control point of is represented by step_size Check whether there are obstacles at each point on the line segment for the step 
      {
        occ = grid_map_->getInflateOccupancy(a * init_points.col(i - 1) + (1 - a) * init_points.col(i));
        // Occupy （ obstacle ）= Get the expansion barrier （a* Initialize the... In the control point i-1 Control points ）+(1 - a) Initialize the... In the control point i Control points 
        

        if (occ && !last_occ)// If there are obstacles at this point , There is no obstacle at the last point 
        {
          if (same_occ_state_times > ENOUGH_INTERVAL || i == order_)
          // There have been more than two consecutive points （ continued ENOUGH_INTERVAL by 2） There are no obstacles or (i == order_) Is the first segment 
          {
            in_id = i - 1;// Record the trajectory of obstacles in the control section , The starting point of the corresponding obstacle segment ( The control points )in_id = i - 1
            flag_got_start = true;// Enter the obstacle sign location true
          }
          same_occ_state_times = 0;// There is... In succession / The barrier free counter is set 0
          flag_got_end_maybe = false; // terminate in advance, No early termination 
          // The obstacle end sign is set false;
        }
        else if (!occ && last_occ)// The point is not in the obstacle , But the last one was an obstacle 
        {
          out_id = i;
          // Record the trajectory of the obstacle in the control section ( The first i paragraph ), The end point of the corresponding obstacle segment out_id=i
          flag_got_end_maybe = true;// Possible obstacle sign location true;
          same_occ_state_times = 0;// There is... In succession / The barrier free counter is set 0
        }
        else// If this point is different from the previous point / The situation without obstacles is the same 
        {
          ++same_occ_state_times;// There is... In succession / The barrier free counter adds 1
        }

        if (flag_got_end_maybe && (same_occ_state_times > ENOUGH_INTERVAL || (i == (int)init_points.cols() - order_)))
        //flag_got_end_maybe, If there is an obstacle, the sign position is true
        // And there have been obstacles at more than two consecutive points before 
        // Or the current track segment is the last segment (i == (int)init_points.cols() - order_)

        {
          flag_got_end_maybe = false;// Then the position of the obstacle sign may appear false
          flag_got_end = true;// The position of the obstacle sign true.
        }

        last_occ = occ;

        if (flag_got_start && flag_got_end)// If you enter and exit obstacles at the same time, the sign position is true
        {
          flag_got_start = false;//
          flag_got_end = false;// At the same time, the sign positions of entering and exiting obstacles are false
          segment_ids.push_back(std::pair<int, int>(in_id, out_id));
          // Control the obstacles to (in_id,out_id) Join in segment_ids.
        }
      }
      // For the former 1/3 Line segment formed by each control point of , With step_size Check whether there are obstacles at each point on the line segment for the step .
      // If there is an obstacle at this point, there is no obstacle at the previous point ,『 If there are no obstacles at more than two consecutive points or the first segment 
      // Then enter the obstacle sign position true, Record the trajectory of obstacles at the control point of the second segment in_id』,
      // There is... In succession / The barrier free counter is set 0, The obstacle end sign is set false;
      // If there is no obstacle at this point, there is an obstacle at the last point , Record the trajectory of the obstacle at the control point of the segment out_id,
      // There is... In succession / The barrier free counter is set 0, Possible obstacle sign location true;
      // If this point is different from the previous point / The situation without obstacles is the same , Then continuous / The barrier free counter adds 1
      // If you enter and exit obstacles at the same time, the sign position is true, Then the sign position false,
      // Control the obstacles to (in_id,out_id) Push the segment_ids.
    }

 

/*** a star search ***/A Star Search

1、 After traversing all segment tracks , If there are no obstacles , Returns empty vector<std::pair<int,int>>.

2、 Otherwise, for each obstacle segment segment_ids, Use from the first control point before entering the obstacle A* The algorithm plans a trajectory to the control point after the first obstacle , Get the path point a_star_pathes.

/*** a star search ***/
    //A Star Search 
    vector<vector<Eigen::Vector3d>> a_star_pathes;//A Star path point 
    // After traversing all segment tracks , If there are no obstacles , Returns empty vector<std::pair<int,int>>.
    // Otherwise, for each obstacle segment segment_ids, Use from the control point before entering the obstacle A* Algorithm 
    // Plan a track to the first obstacle control point , Get the path point a_star_pathes.
    for (size_t i = 0; i < segment_ids.size(); ++i)
    // Number of segments 
    {
      //cout << "in=" << in.transpose() << " out=" << out.transpose() << endl;
      Eigen::Vector3d in(init_points.col(segment_ids[i].first)), out(init_points.col(segment_ids[i].second));
      //init_points.col(segment_ids[i].first) It is the control point for entering the obstacle section 
      //init_points.col(segment_ids[i].second) For the first obstacle control point 
      if (a_star_->AstarSearch(/*(in-out).norm()/10+0.05*/ 0.1, in, out))
      //A Star planning 
      {
        a_star_pathes.push_back(a_star_->getPath());// Path point a_star_pathes
      }
      else
      {
        ROS_ERROR("a star error, force return!");
        return a_star_pathes;
      }
    }

  /*** calculate bounds ***/ Calculate the boundary of obstacle section

For each obstacle segment

1、 If it is the first paragraph Line segment And the number of obstacles is greater than 1, be id_low_bound= First control point ,id_up_bound= The midpoint between the end point of the first obstacle and the start point of the second obstacle ;

If the number of obstacles is 1, be id_up_bound= The last control point .

2、 If the obstacle segment is the last segment Line segment , be id_low_bound= The midpoint between the start point of the last obstacle and the end point of the penultimate obstacle ,id_up_bound= The last control point

3、 If the obstacle section is Middle section ,id_low_bound= The midpoint between the end point of the previous obstacle and the start point of the current obstacle ,id_up_bound= The midpoint between the end point of the current obstacle and the start point of the next obstacle .

Last Control the boundary of each obstacle point pair (id_low_bound,id_up_bound) Deposit in bounds.

/*** calculate bounds ***/
    // Calculate the boundary of each obstacle segment , That is, the upper bound of each obstacle segment and the corresponding control point of the next session 
    int id_low_bound, id_up_bound;//
    vector<std::pair<int, int>> bounds(segment_ids.size());
    for (size_t i = 0; i < segment_ids.size(); i++)
    // Traverse each obstacle segment 
    {

      if (i == 0) // first segment
      // If the obstacle segment is the first segment 
      {
        id_low_bound = order_;//id_low_bound For the first control point 
        if (segment_ids.size() > 1)// Number of obstacles >1
        {
          id_up_bound = (int)(((segment_ids[0].second + segment_ids[1].first) - 1.0f) / 2); // id_up_bound : -1.0f fix()
          //id_up_bound= The midpoint between the end point of the first obstacle and the start point of the second obstacle 
        }
        else// If the number of obstacles is 1
        {
          id_up_bound = init_points.cols() - order_ - 1;
          // be id_up_bound= The last control point 
        }
      }
      else if (i == segment_ids.size() - 1) // last segment, i != 0 here
      // If it is the last obstacle 
      {
        id_low_bound = (int)(((segment_ids[i].first + segment_ids[i - 1].second) + 1.0f) / 2); // id_low_bound : +1.0f ceil()
        // be id_low_bound= The midpoint between the start point of the last obstacle and the end point of the penultimate obstacle 
        id_up_bound = init_points.cols() - order_ - 1;
        //id_up_bound= The last control point 
      }
      else
      // If it is the middle segment 
      {
        id_low_bound = (int)(((segment_ids[i].first + segment_ids[i - 1].second) + 1.0f) / 2); // id_low_bound : +1.0f ceil()
        //id_low_bound= The midpoint between the end point of the previous obstacle and the start point of the current obstacle 
        id_up_bound = (int)(((segment_ids[i].second + segment_ids[i + 1].first) - 1.0f) / 2);  // id_up_bound : -1.0f fix()
        //id_up_bound= The midpoint between the end point of the current obstacle and the start point of the next obstacle .
      }
      bounds[i] = std::pair<int, int>(id_low_bound, id_up_bound);
      // The boundary of each obstacle segment (id_low_bound,id_up_bound) Deposit in bounds
    }

 /*** Adjust segment 0 ***/ Adjust the obstacle section

Adjust the obstacle section

For each obstacle control point pair ( The first control point before entering the obstacle , The first control point after getting out of the obstacle )

reason ： Ensure that each obstacle section has enough points to generate enough thrust ( Push the section away from the obstacle )

If the number of control points between control point pairs is less than the required number ( That is, the minimum number of points =round( Total number of initial control points * Minimum percentage ))

It's in Obstacles in each section Part of the The border Expand inward on both sides , Add the number of points to both sides of the obstacle segment =( Minimum points - Current points ）/2

1. The first control point of the final obstacle section = The first control point of obstacle section - Added points >= The first point of the barrier segment boundary ？ If yes, the first starting control point of the final obstacle segment is the initial control point of the obstacle segment , If less than, it is the first point of the boundary
2. The exit control point of the final obstacle section = The first control point of the obstacle exit section + Added points <= The last point of the barrier section boundary ？ If yes, the last control point returning to the final obstacle section is the control point of the obstacle section , If greater than, it is the last point of the boundary

If there are enough control points, keep the original obstacle control point pair

/*** Adjust segment 0 ***/
    vector<std::pair<int, int>> final_segment_ids(segment_ids.size());
    // The final obstacle segment 
    constexpr double MINIMUM_PERCENT = 0.0; // Minimum percentage 
    // Each segment is guaranteed to have sufficient points to generate sufficient thrust
    // Ensure that each obstacle section has enough points to generate enough thrust ( Push the section away from the obstacle )
    int minimum_points = round(init_points.cols() * MINIMUM_PERCENT), num_points;
    // Define the minimum number of points =round( Total number of initial control points * Minimum percentage ), rounding 
    // The number of points is int type 
    for (size_t i = 0; i < segment_ids.size(); i++)
    // Traverse each obstacle segment 
    {
      /*** Adjust segment length ***/
      // Adjust the length of obstacle section 
      num_points = segment_ids[i].second - segment_ids[i].first + 1;
      // Count the number of points = The control point at the end of the obstacle section - The control point at the beginning of the obstacle section 
      //( Number of points ： As the first 8 Control points - The first 3 One control point is 8-3=5 Control points )
      //cout << "i = " << i << " first = " << segment_ids[i].first << " second = " << segment_ids[i].second << endl;
      if (num_points < minimum_points)
      // If the number of points in the obstacle section is less than the minimum number of points 
      {
        // The boundary of the control point pair of each obstacle segment extends inward on both sides 
        double add_points_each_side = (int)(((minimum_points - num_points) + 1.0f) / 2);
        // Add the number of points to both sides of the obstacle segment =( Minimum points - Current points ）/2

        final_segment_ids[i].first = segment_ids[i].first - add_points_each_side >= bounds[i].first ? segment_ids[i].first - add_points_each_side : bounds[i].first;
        // The first control point of the final obstacle section = The first control point of obstacle section - Added points >= The first point of the barrier segment boundary ？ If yes, the first starting control point of the final obstacle segment is the initial control point of the obstacle segment , If less than, it is the first point of the boundary 
        final_segment_ids[i].second = segment_ids[i].second + add_points_each_side <= bounds[i].second ? segment_ids[i].second + add_points_each_side : bounds[i].second;
        // The exit control point of the final obstacle section = The first control point of the obstacle exit section + Added points <= The last point of the barrier section boundary ？ If yes, the last control point returning to the final obstacle section is the control point of the obstacle section , If greater than, it is the last point of the boundary 
      }
      else// Otherwise, keep the original obstacle control point pair 
      {
        final_segment_ids[i].first = segment_ids[i].first;
        final_segment_ids[i].second = segment_ids[i].second;
      }

      //cout << "final:" << "i = " << i << " first = " << final_segment_ids[i].first << " second = " << final_segment_ids[i].second << endl;
    }