1、背景描述

地图：sunnyvale_big_loop
在这里插入图片描述

2、调试信息

1、变道前：
ref_info size = 2

best_ref_info: 3143_1_-2
best_ref_info: 96_1_-2
best_ref_info: 42_1_-3
best_ref_info: 2414_1_-3
best_ref_info: 128_1_-2
best_ref_info: 97_1_-2
best_ref_info: 1775_1_-2
target_ref_info: 3143_1_-1
target_ref_info: 1770_1_-1
target_ref_info: 42_1_-1
target_ref_info: 2414_1_-1
target_ref_info: 10761a_1_-1
target_ref_info: 162_1_-3
target_ref_info: 9760_1_-3
target_ref_info: 1771_1_-3

2、变道后：
ref_info size = 1

best_ref_info: 3143_1_-1
best_ref_info: 1770_1_-1
best_ref_info: 42_1_-1
best_ref_info: 2414_1_-1
best_ref_info: 10761a_1_-1
best_ref_info: 162_1_-3
best_ref_info: 9760_1_-3
best_ref_info: 1771_1_-3
target_ref_info: 3143_1_-1
target_ref_info: 1770_1_-1
target_ref_info: 42_1_-1
target_ref_info: 2414_1_-1
target_ref_info: 10761a_1_-1
target_ref_info: 162_1_-3
target_ref_info: 9760_1_-3
target_ref_info: 1771_1_-3

3、代码分析


const ReferenceLineInfo *Frame::FindDriveReferenceLineInfo() {
  double min_cost = std::numeric_limits<double>::infinity();
  drive_reference_line_info_ = nullptr;
  
  for (const auto &reference_line_info : reference_line_info_) {
    if (reference_line_info.IsDrivable() &&
        reference_line_info.Cost() < min_cost) {
      drive_reference_line_info_ = &reference_line_info;
      min_cost = reference_line_info.Cost();
    }
  }
  
  //const auto &reference_line_info=reference_line_info_.back();
  //drive_reference_line_info_=&reference_line_info;
  return drive_reference_line_info_;
}

const ReferenceLineInfo *Frame::FindTargetReferenceLineInfo() {
  //add by sam********************************************************start//
  //AINFO<<reference_line_info_.size();
  //add by sam*********************************************************end//
  const ReferenceLineInfo *target_reference_line_info = nullptr;
  for (const auto &reference_line_info : reference_line_info_) {
    if (reference_line_info.IsChangeLanePath()) {
      return &reference_line_info;
    }
    target_reference_line_info = &reference_line_info;
  }
  
  //const auto &reference_line_info=reference_line_info_.back();
  //target_reference_line_info=&reference_line_info;
  return target_reference_line_info;
}

因为不能跨车道的原因，每次计算出来的reference_line最多只有两条。两个referenceline的切换在下面这段代码中进行体现。

Stage::StageStatus LaneFollowStage::Process(
    const TrajectoryPoint& planning_start_point, Frame* frame) {
  bool has_drivable_reference_line = false;

  ADEBUG << "Number of reference lines:\t"
         << frame->mutable_reference_line_info()->size();

  unsigned int count = 0;

  for (auto& reference_line_info : *frame->mutable_reference_line_info()) {
    // TODO(SHU): need refactor
    if (count++ == frame->mutable_reference_line_info()->size()) {
      break;
    }
    ADEBUG << "No: [" << count << "] Reference Line.";
    ADEBUG << "IsChangeLanePath: " << reference_line_info.IsChangeLanePath();

    if (has_drivable_reference_line) {
      reference_line_info.SetDrivable(false);
      break;
    }

    auto cur_status =
        PlanOnReferenceLine(planning_start_point, frame, &reference_line_info);

    if (cur_status.ok()) {
      if (reference_line_info.IsChangeLanePath()) {
        ADEBUG << "reference line is lane change ref.";
        ADEBUG << "FLAGS_enable_smarter_lane_change: "
               << FLAGS_enable_smarter_lane_change;
        if (reference_line_info.Cost() < kStraightForwardLineCost &&
            (LaneChangeDecider::IsClearToChangeLane(&reference_line_info) ||
             FLAGS_enable_smarter_lane_change)) {
          // If the path and speed optimization succeed on target lane while
          // under smart lane-change or IsClearToChangeLane under older version
          has_drivable_reference_line = true;
          reference_line_info.SetDrivable(true);
          LaneChangeDecider::UpdatePreparationDistance(
              true, frame, &reference_line_info, injector_->planning_context());
          ADEBUG << "\tclear for lane change";
        } else {
          LaneChangeDecider::UpdatePreparationDistance(
              false, frame, &reference_line_info,
              injector_->planning_context());
          reference_line_info.SetDrivable(false);
          ADEBUG << "\tlane change failed";
        }
      } else {
        ADEBUG << "reference line is NOT lane change ref.";
        has_drivable_reference_line = true;
      }
    } else {
      reference_line_info.SetDrivable(false);
    }
  }

  return has_drivable_reference_line ? StageStatus::RUNNING
                                     : StageStatus::ERROR;
}