What is the basic idea of the function: DPRoadGraph::SamplePathWaypoints()

[davidhopper2003]

Hi, Apollo developers. Can you introduce the basic idea of the function: DPRoadGraph::SamplePathWaypoints() in the file: apollo/modules/planning/tasks/dp_poly_path/dp_road_graph.cc? Thanks a lot in advance.

[lianglia-apollo]

We sample path way points along the road. We will connect those points (and the ego car) with smooth lines and pick one from them based on cost calculation.

[davidhopper2003]

@lianglia-apollo Thanks. I can understand the principle. It would be better if you could provide more details.

[davidhopper2003]

@lianglia-apollo Can I use this picture to understand the algorithm?

[davidhopper2003]

@lianglia-apollo I have made the following comments. Please correct me if there are any mistakes.

bool DPRoadGraph::SamplePathWaypoints(
    const common::TrajectoryPoint &init_point,
    std::vector<std::vector<common::SLPoint>> *const points) {
  CHECK_NOTNULL(points);

  // 最小采样距离
  const double kMinSampleDistance = 40.0;
  // 总长度 = min(初始点 + max(初始速度 × 8, 最小采样距离), 参考线长度）
  const double total_length = std::fmin(
      init_sl_point_.s() + std::fmax(init_point.v() * 8.0, kMinSampleDistance),
      reference_line_.Length());

  // 采样前视时长
  constexpr double kSamplePointLookForwardTime = 4.0;
  // 采样步长 = 初始速度 × 采样前视时长，要求：
  // step_length_min(默认值：8) <= 采样步长 <= step_length_max(默认值：15)
  const double level_distance =
      common::math::Clamp(init_point.v() * kSamplePointLookForwardTime,
                          config_.step_length_min(), config_.step_length_max());
  // 累计轨迹弧长
  double accumulated_s = init_sl_point_.s();
  // 上次轨迹弧长
  double prev_s = accumulated_s;
  // 累计轨迹弧长小于总长度时，将累计轨迹弧长每次加上采样步长，进行循环采样
  for (std::size_t i = 0; accumulated_s < total_length; ++i) {
    accumulated_s += level_distance;
    if (accumulated_s + level_distance / 2.0 > total_length) {
      accumulated_s = total_length;
    }
    // 本次轨迹弧长：取累计轨迹弧长与总长度之间的最小值
    const double s = std::fmin(accumulated_s, total_length);
    // 最小允许采样步长
    constexpr double kMinAllowedSampleStep = 1.0;
    // 若本次轨迹弧长与上次轨迹弧长间的差值小于最小允许采样步长，跳过本次采样
    if (std::fabs(s - prev_s) < kMinAllowedSampleStep) {
      continue;
    }
    prev_s = s;

    // 左车道宽
    double left_width = 0.0;
    // 右车道宽
    double right_width = 0.0;
    reference_line_.GetLaneWidth(s, &left_width, &right_width);

    // 边界缓冲
    constexpr double kBoundaryBuff = 0.20;
    const auto &vehicle_config =
        common::VehicleConfigHelper::instance()->GetConfig();
    const double half_adc_width = vehicle_config.vehicle_param().width() / 2.0;
    // 右侧允许宽度 = 右车道宽 - 半车宽 - 边界缓冲
    const double eff_right_width = right_width - half_adc_width - kBoundaryBuff;
    // 左侧允许宽度 = 左车道宽 - 半车宽 - 边界缓冲
    const double eff_left_width = left_width - half_adc_width - kBoundaryBuff;
    // 每步采样点数
    const size_t num_sample_per_level =
        FLAGS_use_navigation_mode ? config_.navigator_sample_num_each_level()
                                  : config_.sample_points_num_each_level();
    // 默认横向采样间隔
    double kDefaultUnitL = 1.2 / (num_sample_per_level - 1);
    if (reference_line_info_.IsChangeLanePath() && !IsSafeForLaneChange()) {
      kDefaultUnitL = 1.0;
    }
    // 横向采样距离
    const double sample_l_range = kDefaultUnitL * (num_sample_per_level - 1);
    // 右采样边界（车辆右侧为负值）
    double sample_right_boundary = -eff_right_width;
    // 左采样边界（车辆左侧为正值）
    double sample_left_boundary = eff_left_width;
    // 参考线正在改变车道时
    if (reference_line_info_.IsChangeLanePath()) {
      // 右采样边界取右采样边界与初始点横向偏移之间的最小值
      sample_right_boundary = std::fmin(-eff_right_width, init_sl_point_.l());
      // 左采样边界取左采样边界与初始点横向偏移之间的最大值
      sample_left_boundary = std::fmax(eff_left_width, init_sl_point_.l());

      // 若初始点横向偏移 > 左侧允许宽度，则将右侧采样边界设置为右侧采样边界与（初始点横向偏移
      // - 横向采样距离）之间的最大值
      if (init_sl_point_.l() > eff_left_width) {
        sample_right_boundary = std::fmax(sample_right_boundary,
                                          init_sl_point_.l() - sample_l_range);
      }
      // 若初始点横向偏移 < 右侧允许宽度，则将左侧采样边界设置为左侧采样边界与（初始点横向偏移
      // + 横向采样距离）之间的最小值
      if (init_sl_point_.l() < eff_right_width) {
        sample_left_boundary = std::fmin(sample_left_boundary,
                                         init_sl_point_.l() + sample_l_range);
      }
    }

    // 横向采样距离数组
    std::vector<double> sample_l;
    // 参考线正在改变车道且改变车道不安全时，将当前参考线到其他参考线的偏移值存储到横向采样距离数组
    if (reference_line_info_.IsChangeLanePath() && !IsSafeForLaneChange()) {
      sample_l.push_back(reference_line_info_.OffsetToOtherReferenceLine());
    } else {
      // 其他情形，从右采样边界到左采样边界，按照每步采样点数进行均匀采样，并将结果存储到横向采样距离数组
      common::util::uniform_slice(sample_right_boundary, sample_left_boundary,
                                  num_sample_per_level - 1, &sample_l);
      if (HasSidepass()) {
        // currently only left nudge is supported. Need road hard boundary for
        // both sides
        sample_l.clear();
        switch (sidepass_.type()) {
          case ObjectSidePass::LEFT: {
            // 左侧绕行：将（左侧允许宽度 + 左侧绕行距离）存储到横向采样距离数组
            sample_l.push_back(eff_left_width + config_.sidepass_distance());
            break;
          }
          case ObjectSidePass::RIGHT: {
            // 右侧绕行：将-（右侧允许宽度 + 右侧绕行距离）存储到横向采样距离数组
            sample_l.push_back(-eff_right_width - config_.sidepass_distance());
            break;
          }
          default:
            break;
        }
      }
    }
    // 本次采样点数组
    std::vector<common::SLPoint> level_points;
    planning_internal::SampleLayerDebug sample_layer_debug;
    for (size_t j = 0; j < sample_l.size(); ++j) {
      // 横向偏移值
      const double l = sample_l[j];
      constexpr double kResonateDistance = 1e-3;
      common::SLPoint sl;
      // 若为奇数采样点或者（总长度 - 累计轨迹弧长）几乎为0即已抵达采样终点，
      // 则直接将当前采样点坐标设置为(s, l)
      if (j % 2 == 0 ||
          total_length - accumulated_s < 2.0 * kResonateDistance) {
        sl = common::util::MakeSLPoint(s, l);
      } else {
        // 其他情形，将当前采样点坐标设置为（min(总长度，s+误差)，l）
        sl = common::util::MakeSLPoint(
            std::fmin(total_length, s + kResonateDistance), l);
      }
      sample_layer_debug.add_sl_point()->CopyFrom(sl);
      // 将当前采样点坐标存储到本次采样点数组
      level_points.push_back(std::move(sl));
    }
    // 若参考线未改变车道且不绕行，则将横向偏移值为0的采样点（即沿参考线方向的采样点）也加入本次采样点数组
    if (!reference_line_info_.IsChangeLanePath() && !HasSidepass()) {
      auto sl_zero = common::util::MakeSLPoint(s, 0.0);
      sample_layer_debug.add_sl_point()->CopyFrom(sl_zero);
      level_points.push_back(sl_zero);
    }

    if (!level_points.empty()) {
      planning_debug_->mutable_planning_data()
          ->mutable_dp_poly_graph()
          ->add_sample_layer()
          ->CopyFrom(sample_layer_debug);
      // 将本次的所有采样点存储到总采样点数组
      points->emplace_back(level_points);
    }
  }
  return true;
}

[davidhopper2003]

@lianglia-apollo I have three questions according to the following code snippet. First, what is the meaning of the variable of "kResonateDistance"? Second, Why the "s" value is different at the same sampling level? I think all "s" values should be same but "l" values different. Third, from what I understand, "s + kResonateDistance" is almost the same as "s". What is the necessity of this line: sl = common::util::MakeSLPoint(std::fmin(total_length, s + kResonateDistance), l)? What I mean is that we can use sl = common::util::MakeSLPoint(s, l); to replace if (j % 2 == 0 || total_length - accumulated_s < 2.0 * kResonateDistance) { sl = common::util::MakeSLPoint(s, l); } else { sl = common::util::MakeSLPoint(std::fmin(total_length, s + kResonateDistance), l);} .

for (size_t j = 0; j < sample_l.size(); ++j) {
      // 横向偏移值
      const double l = sample_l[j];
      constexpr double kResonateDistance = 1e-3;
      common::SLPoint sl;
      // 若为奇数采样点或者（总长度 - 累计轨迹弧长）几乎为0即已抵达采样终点，
      // 则直接将当前采样点坐标设置为(s, l)
      if (j % 2 == 0 ||
          total_length - accumulated_s < 2.0 * kResonateDistance) {
        sl = common::util::MakeSLPoint(s, l);
      } else {
        // 其他情形，将当前采样点坐标设置为（min(总长度，s+误差)，l）
        sl = common::util::MakeSLPoint(
            std::fmin(total_length, s + kResonateDistance), l);
      }
      sample_layer_debug.add_sl_point()->CopyFrom(sl);
      // 将当前采样点坐标存储到本次采样点数组
      level_points.push_back(std::monove(sl));
}

[gengqx]

for the second question：there is a for loop

  for (std::size_t i = 0; accumulated_s < total_length; ++i) {}

So,it first slice s,then slice l.
for the third question.the type of the s and l is double,MakeSLPoint could generate a proto :SLPoint
it could be found in modules/common/utils/utils.cc

[davidhopper2003]

@gengqx What I mean is that all "s" values at the same sampling level should be equal. And the "l" values are different.

[lianglia-apollo]

@davidhopper2003 We made s slightly different to avoid discontinuity issues at lane boundaries.

[davidhopper2003]

@lianglia-apollo Got it. Thank you very much.