Ego planner code parsing Bspline_ Optimizer section (2)

/*** Assign data to each segment ***/

Select the base point of the obstacle surface pij

For each adjusted obstacle control point pair

In the corresponding path point set a_star_pathes And the control point i The vector formed is approximately perpendicular to the path point of the vector formed by the front and rear control points as the traction point of the control point intersection_point , And will got_intersection_id Set as the index of the current control point .

Astar_id The number of indexes of is only A Half of the star path point , Main cause ： Calculating all path points requires a lot of calculation

If the traction point is found in the previous step and the distance between the traction point and the control point is greater than 0, Then... Corresponding to this control point cps_.flag_temp Set as true.

In the expansion grid map , In the distance between the traction point and the control point, take the resolution as the step , Find the grid point on the obstacle surface between the control point and the traction point ( That is, the position of the first occupied point ), And press the point into the corresponding cps_.base_point basic point pij in , The unit direction from the control point to the traction point is pressed into the corresponding cps_.direction, namely vij in .

otherwise got_intersection_id Set as -1,cps_.flag_temp Set as false

If there are no other control points between the current obstacle control point pair , It's in a_star_pathes Find the path point corresponding to the vector formed by the control point pair, which is approximately perpendicular to the vector formed by the control point pair, as the traction point of the control point intersection_point,got_intersection_id Set as the index of the control point of the current entry obstacle , If the distance between the towing point and the midpoint is greater than 1cm, This control point corresponds to cps_.flag_temp Set as true, The control point is pressed into the corresponding cps_.base_point, The unit direction vector of the traction point and the midpoint is pressed cps_.direction.

If there is a control point between the obstacle control point pair, there is no traction point and traction direction , Then the last traction point and direction will be the most default point and direction , Press in the corresponding cps_.base_point and cps_.direction.

Finally, press the adjusted obstacle control points into final_segment_ids And back to .

 

/*** Assign data to each segment ***/
    // Assign data to each obstacle segment 
    for (size_t i = 0; i < segment_ids.size(); i++)
    // Traverse each control point pair segment_ids Enter the obstacle section 
    {
      
      // step 1
      for (int j = final_segment_ids[i].first; j <= final_segment_ids[i].second; ++j)
      // For each adjusted obstacle control point pair 
        cps_.flag_temp[j] = false;
      
      // step 2
      int got_intersection_id = -1;
      //got_intersection_id Index of the traction point set as the control point 
      for (int j = segment_ids[i].first + 1; j < segment_ids[i].second; ++j)
      {
        Eigen::Vector3d ctrl_pts_law(cps_.points.col(j + 1) - cps_.points.col(j - 1)), intersection_point;
        //ctrl_pts_law： The first j+1 Subtract the... From the control point j-1 Control points  （ namely j-1 The control point of this position points to j+1 The control point of this position ）
        // Define control points law It is the front and rear control points of the current control point 
        // Traction point of control point intersection_point
        // There is one for each obstacle segment Asatr Algorithm generated path a_star_pathes
        int Astar_id = a_star_pathes[i].size() / 2, last_Astar_id; 
        //Astar_id The number of indexes of is only A Half of the star path point , Main cause ： Calculating all path points requires a lot of calculation 
        // Let "Astar_id = id_of_the_most_far_away_Astar_point" will be better, but it needs more computation
        double val = (a_star_pathes[i][Astar_id] - cps_.points.col(j)).dot(ctrl_pts_law), last_val = val;
        // value =A Star index point -( Control point in obstacle section ( Start from the control point in front of the first obstacle )) And ctrl_pts_law The product of vectors 
        // The vector has ( Size and direction )
        while (Astar_id >= 0 && Astar_id < (int)a_star_pathes[i].size()) 
        {// If Astar Number of indexes >=0 And Astar The number of index points is less than all Astar The value of the path point 
          last_Astar_id = Astar_id;//Astar The index point is the last index 

          if (val >= 0)// How this value >=0, be 
            --Astar_id;// The new traction point is ：Astar Traction point -1
          else// How this value <0, be 
            ++Astar_id;// The new traction point is ：Astar Traction point +1

          val = (a_star_pathes[i][Astar_id] - cps_.points.col(j)).dot(ctrl_pts_law);
          // value =A Star index point -( Control point in obstacle section ( From the control point in front of the first obstacle +1 Start )) And ctrl_pts_law The product of vectors 
          //a.dot(b)  And  np.dot(a,b) The effect is the same , Calculate inner product 


          if (val * last_val <= 0 && (abs(val) > 0 || abs(last_val) > 0)) 
          // If the value * The final value <=0 And val>0 Or finally last_val > 0
          // val = last_val = 0.0 is not allowed
          {
            intersection_point =// Traction point 
                a_star_pathes[i][Astar_id] +
                ((a_star_pathes[i][Astar_id] - a_star_pathes[i][last_Astar_id]) *
                 (ctrl_pts_law.dot(cps_.points.col(j) - a_star_pathes[i][Astar_id]) / ctrl_pts_law.dot(a_star_pathes[i][Astar_id] - a_star_pathes[i][last_Astar_id])) // = t
                );
                //= The first i strip Astar The number of path points in the planned path - the last one A Star path point *（( The control points in the obstacle section are similar to Astar The number of path points in the planned path ) And ctrl_pts_law The product of vectors /( The control points in the obstacle section are similar to Astar The number of path points in the planned path ) And ctrl_pts_law The product of vectors .
                // Find the path point of the vector formed by the control point that is approximately perpendicular to the vector formed by the front and rear control points 
            //cout << "i=" << i << " j=" << j << " Astar_id=" << Astar_id << " last_Astar_id=" << last_Astar_id << " intersection_point = " << intersection_point.transpose() << endl;

            got_intersection_id = j;// Index of towing points 
            break;
          }
        }

        if (got_intersection_id >= 0)// If the traction point is found in the previous step and the distance between the traction point and the control point is greater than 0
        {
          cps_.flag_temp[j] = true;
          // Then... Corresponding to this control point cps_.flag_temp Set as true
          double length = (intersection_point - cps_.points.col(j)).norm();
          if (length > 1e-5)// If the distance between the traction point and the control point is greater than 1cm
          {
            for (double a = length; a >= 0.0; a -= grid_map_->getResolution())
            {
              occ = grid_map_->getInflateOccupancy((a / length) * intersection_point + (1 - a / length) * cps_.points.col(j));
              // In the expansion grid map , Find the grid point on the obstacle surface between the control point and the traction point 
              if (occ || a < grid_map_->getResolution())
              {
                if (occ)
                  a += grid_map_->getResolution();
                
                cps_.base_point[j].push_back((a / length) * intersection_point + (1 - a / length) * cps_.points.col(j));
                // And press the grid points on the surface of the obstacle into the corresponding cps_.base_point In the base point 
                cps_.direction[j].push_back((intersection_point - cps_.points.col(j)).normalized());
                // The unit direction from the control point to the traction point is pressed into the corresponding cps_.direction.
                break;
                // otherwise got_intersection_id Set as -1,cps_.flag_temp Set as false
              }
            }
          }
        }
      }

      /* Corner case: the segment length is too short. Here the control points may outside the A* path, leading to opposite gradient direction. So I have to take special care of it */
      // Tangent line   Corner condition ： The length of the line segment is too short . here , The control point may be located A* Out of the path , Causes the opposite gradient direction .
      if (segment_ids[i].second - segment_ids[i].first == 1)
      //  If there are no other control points between the current obstacle control point pair 
      {
        Eigen::Vector3d ctrl_pts_law(cps_.points.col(segment_ids[i].second) - cps_.points.col(segment_ids[i].first)), intersection_point;
        //ctrl_pts_law It is the first control point after getting out of the obstacle - The first control point before entering the obstacle 
        Eigen::Vector3d middle_point = (cps_.points.col(segment_ids[i].second) + cps_.points.col(segment_ids[i].first)) / 2;
        // Control point to midpoint ,
        int Astar_id = a_star_pathes[i].size() / 2, last_Astar_id; // Let "Astar_id = id_of_the_most_far_away_Astar_point" will be better, but it needs more computation
        //Astar_id Indexes 
        double val = (a_star_pathes[i][Astar_id] - middle_point).dot(ctrl_pts_law), last_val = val;
        // stay a_star_pathes Find the point in which the vector formed by the control point pair is approximately perpendicular to the vector formed by the control point pair 
        while (Astar_id >= 0 && Astar_id < (int)a_star_pathes[i].size())
        {
          last_Astar_id = Astar_id;

          if (val >= 0)
            --Astar_id;
          else
            ++Astar_id;

          val = (a_star_pathes[i][Astar_id] - middle_point).dot(ctrl_pts_law);

          if (val * last_val <= 0 && (abs(val) > 0 || abs(last_val) > 0)) // val = last_val = 0.0 is not allowed
          {
            intersection_point =
                a_star_pathes[i][Astar_id] +
                ((a_star_pathes[i][Astar_id] - a_star_pathes[i][last_Astar_id]) *
                 (ctrl_pts_law.dot(middle_point - a_star_pathes[i][Astar_id]) / ctrl_pts_law.dot(a_star_pathes[i][Astar_id] - a_star_pathes[i][last_Astar_id])) // = t
                );
                // The corresponding path point is used as the traction point of the control point intersection_point

            if ((intersection_point - middle_point).norm() > 0.01) // 1cm.
            // If the distance between the towing point and the midpoint is greater than 1cm
            {
              cps_.flag_temp[segment_ids[i].first] = true;
              // This control point corresponds to cps_.flag_temp Set as true
              cps_.base_point[segment_ids[i].first].push_back(cps_.points.col(segment_ids[i].first));
              // The control point is pressed into the corresponding cps_.base_point
              cps_.direction[segment_ids[i].first].push_back((intersection_point - middle_point).normalized());
              // The unit direction vector of the traction point and the midpoint is pressed cps_.direction

              got_intersection_id = segment_ids[i].first;
              //got_intersection_id Set as the index of the control point of the current entry obstacle 
            }
            break;
          }
        }
      }

      //step 3
      if (got_intersection_id >= 0)
      // If there is a control point between the obstacle control point pair 
      {
        for (int j = got_intersection_id + 1; j <= final_segment_ids[i].second; ++j)
          if (!cps_.flag_temp[j])// There is no traction point and traction direction 
          {
            // Then the last traction point and direction will be the most default point and direction 
            cps_.base_point[j].push_back(cps_.base_point[j - 1].back());
            cps_.direction[j].push_back(cps_.direction[j - 1].back());
          }

        for (int j = got_intersection_id - 1; j >= final_segment_ids[i].first; --j)
          if (!cps_.flag_temp[j])// There is no traction point and traction direction 
          {
            // Then the last traction point and direction will be the most default point and direction 
            cps_.base_point[j].push_back(cps_.base_point[j + 1].back());
            cps_.direction[j].push_back(cps_.direction[j + 1].back());
            // Add the corresponding cps_.base_point and cps_.direction
          }
      }
      else
      {
        // Just ignore, it does not matter ^_^.
        // ROS_ERROR("Failed to generate direction! segment_id=%d", i);
      }
    }

    return a_star_pathes;// Finally, press the adjusted obstacle control points into final_segment_ids And back to 
  }