【Apollo學(xué)習(xí)筆記】——規(guī)劃模塊TASK之PATH_ASSESSMENT_DECIDER

這篇具有很好參考價值的文章主要介紹了【Apollo學(xué)習(xí)筆記】——規(guī)劃模塊TASK之PATH_ASSESSMENT_DECIDER。希望對大家有所幫助。如果存在錯誤或未考慮完全的地方，請大家不吝賜教，您也可以點擊"舉報違法"按鈕提交疑問。

TASK系列解析文章

1.【Apollo學(xué)習(xí)筆記】——規(guī)劃模塊TASK之LANE_CHANGE_DECIDER
2.【Apollo學(xué)習(xí)筆記】——規(guī)劃模塊TASK之PATH_REUSE_DECIDER
3.【Apollo學(xué)習(xí)筆記】——規(guī)劃模塊TASK之PATH_BORROW_DECIDER
4.【Apollo學(xué)習(xí)筆記】——規(guī)劃模塊TASK之PATH_BOUNDS_DECIDER
5.【Apollo學(xué)習(xí)筆記】——規(guī)劃模塊TASK之PIECEWISE_JERK_PATH_OPTIMIZER
6.【Apollo學(xué)習(xí)筆記】——規(guī)劃模塊TASK之PATH_ASSESSMENT_DECIDER
7.【Apollo學(xué)習(xí)筆記】——規(guī)劃模塊TASK之PATH_DECIDER
8.【Apollo學(xué)習(xí)筆記】——規(guī)劃模塊TASK之RULE_BASED_STOP_DECIDER
9.【Apollo學(xué)習(xí)筆記】——規(guī)劃模塊TASK之SPEED_BOUNDS_PRIORI_DECIDER&&SPEED_BOUNDS_FINAL_DECIDER
10.【Apollo學(xué)習(xí)筆記】——規(guī)劃模塊TASK之SPEED_HEURISTIC_OPTIMIZER
11.【Apollo學(xué)習(xí)筆記】——規(guī)劃模塊TASK之SPEED_DECIDER
12.【Apollo學(xué)習(xí)筆記】——規(guī)劃模塊TASK之PIECEWISE_JERK_SPEED_OPTIMIZER
13.【Apollo學(xué)習(xí)筆記】——規(guī)劃模塊TASK之PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZER(一)
14.【Apollo學(xué)習(xí)筆記】——規(guī)劃模塊TASK之PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZER(二)

前言

在Apollo星火計劃學(xué)習(xí)筆記——Apollo路徑規(guī)劃算法原理與實踐與【Apollo學(xué)習(xí)筆記】——Planning模塊講到……Stage::Process的PlanOnReferenceLine函數(shù)會依次調(diào)用task_list中的TASK，本文將會繼續(xù)以LaneFollow為例依次介紹其中的TASK部分究竟做了哪些工作。由于個人能力所限，文章可能有紕漏的地方，還請批評斧正。

在modules/planning/conf/scenario/lane_follow_config.pb.txt配置文件中，我們可以看到LaneFollow所需要執(zhí)行的所有task。

stage_config: {
  stage_type: LANE_FOLLOW_DEFAULT_STAGE
  enabled: true
  task_type: LANE_CHANGE_DECIDER
  task_type: PATH_REUSE_DECIDER
  task_type: PATH_LANE_BORROW_DECIDER
  task_type: PATH_BOUNDS_DECIDER
  task_type: PIECEWISE_JERK_PATH_OPTIMIZER
  task_type: PATH_ASSESSMENT_DECIDER
  task_type: PATH_DECIDER
  task_type: RULE_BASED_STOP_DECIDER
  task_type: SPEED_BOUNDS_PRIORI_DECIDER
  task_type: SPEED_HEURISTIC_OPTIMIZER
  task_type: SPEED_DECIDER
  task_type: SPEED_BOUNDS_FINAL_DECIDER
  task_type: PIECEWISE_JERK_SPEED_OPTIMIZER
  # task_type: PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZER
  task_type: RSS_DECIDER

本文將繼續(xù)介紹LaneFollow的第6個TASK——PATH_ASSESSMENT_DECIDER

PATH_ASSESSMENT_DECIDER功能簡介

路徑評價，選出最優(yōu)路徑

【Apollo學(xué)習(xí)筆記】——規(guī)劃模塊TASK之PATH_ASSESSMENT_DECIDER,Apollo,決策規(guī)劃,算法,自動駕駛,planning,apollo,人工智能
由PATH_BOUNDS_DECIDER可以知道會產(chǎn)生以下幾種類型的路徑邊界：

fallback
fallback+lanechange
fallback+pullover
fallback+regular

依據(jù)不同的邊界會產(chǎn)生不同的路徑，接著便需要篩選出一條最優(yōu)的路徑。依據(jù)以下規(guī)則，進(jìn)行評價：

路徑是否和障礙物碰撞
路徑長度
路徑是否會停在對向車道
路徑離自車遠(yuǎn)近
哪個路徑更早回自車道
…

PATH_ASSESSMENT_DECIDER相關(guān)信息

輸入：Status PathAssessmentDecider::Process(Frame* const frame, ReferenceLineInfo* const reference_line_info)
輸入Frame，reference_line_info。
輸出：路徑排序之后，選擇第一個路徑。結(jié)果保存在reference_line_info中

PATH_ASSESSMENT_DECIDER總體流程

【Apollo學(xué)習(xí)筆記】——規(guī)劃模塊TASK之PATH_ASSESSMENT_DECIDER,Apollo,決策規(guī)劃,算法,自動駕駛,planning,apollo,人工智能

【Apollo學(xué)習(xí)筆記】——規(guī)劃模塊TASK之PATH_ASSESSMENT_DECIDER,Apollo,決策規(guī)劃,算法,自動駕駛,planning,apollo,人工智能
首先來看看PathAssessmentDecider::Process流程部分：

Process部分主要完成路徑重復(fù)使用判斷、去除無效路徑、分析路徑并加入重要信息提供給速度決策部分、排序選擇最優(yōu)的路徑以及最后的更新必要的信息。

1. 去除無效路徑

  // 1. Remove invalid path.
  // 1. 刪掉無效路徑.
  std::vector<PathData> valid_path_data;
  for (const auto& curr_path_data : candidate_path_data) {
    // RecordDebugInfo(curr_path_data, curr_path_data.path_label(),
    //                 reference_line_info);
    if (curr_path_data.path_label().find("fallback") != std::string::npos) {
      // fallback的無效路徑是偏離參考線以及道路的路徑
      if (IsValidFallbackPath(*reference_line_info, curr_path_data)) {
        valid_path_data.push_back(curr_path_data);
      }
    } else {
      // regular的無效路徑是偏離參考線、道路，碰撞，停在相鄰的逆向車道的路徑。
      if (IsValidRegularPath(*reference_line_info, curr_path_data)) {
        valid_path_data.push_back(curr_path_data);
      }
    }
  }
  const auto& end_time1 = std::chrono::system_clock::now();
  std::chrono::duration<double> diff = end_time1 - end_time0;
  ADEBUG << "Time for path validity checking: " << diff.count() * 1000
         << " msec.";

其中fallback的無效路徑是偏離參考線以及道路的路徑。regular的無效路徑是偏離參考線、道路，碰撞，停在相鄰的逆向車道的路徑。

2. 分析并加入重要信息給speed決策

  // 2. Analyze and add important info for speed decider to use
  // 2. 分析并加入重要信息給speed決策
  size_t cnt = 0;
  const Obstacle* blocking_obstacle_on_selflane = nullptr;
  for (size_t i = 0; i != valid_path_data.size(); ++i) {
    auto& curr_path_data = valid_path_data[i];
    if (curr_path_data.path_label().find("fallback") != std::string::npos) {
      // remove empty path_data.
      if (!curr_path_data.Empty()) {
        if (cnt != i) {
          valid_path_data[cnt] = curr_path_data;
        }
        ++cnt;
      }
      continue;
    }
    // 添加相關(guān)信息
    SetPathInfo(*reference_line_info, &curr_path_data);
    // Trim all the lane-borrowing paths so that it ends with an in-lane
    // position.
    // 修剪所有路徑（只要不是pull-over），使其能夠以in-lane結(jié)尾
    if (curr_path_data.path_label().find("pullover") == std::string::npos) {
      TrimTailingOutLanePoints(&curr_path_data);
    }

    // find blocking_obstacle_on_selflane, to be used for lane selection later
    // 找到self_lane上的阻塞障礙物, 為下一步選擇車道做準(zhǔn)備
    if (curr_path_data.path_label().find("self") != std::string::npos) {
      const auto blocking_obstacle_id = curr_path_data.blocking_obstacle_id();
      blocking_obstacle_on_selflane =
          reference_line_info->path_decision()->Find(blocking_obstacle_id);
    }

    // remove empty path_data.
    if (!curr_path_data.Empty()) {
      if (cnt != i) {
        valid_path_data[cnt] = curr_path_data;
      }
      ++cnt;
    }

    // RecordDebugInfo(curr_path_data, curr_path_data.path_label(),
    //                 reference_line_info);
    ADEBUG << "For " << curr_path_data.path_label() << ", "
           << "path length = " << curr_path_data.frenet_frame_path().size();
  }
  valid_path_data.resize(cnt);
  // If there is no valid path_data, exit.
  // 如果沒有有效路徑，退出
  if (valid_path_data.empty()) {
    const std::string msg = "Neither regular nor fallback path is valid.";
    AERROR << msg;
    return Status(ErrorCode::PLANNING_ERROR, msg);
  }
  ADEBUG << "There are " << valid_path_data.size() << " valid path data.";
  const auto& end_time2 = std::chrono::system_clock::now();
  diff = end_time2 - end_time1;
  ADEBUG << "Time for path info labeling: " << diff.count() * 1000 << " msec.";

SetPathInfo

void PathAssessmentDecider::SetPathInfo(
    const ReferenceLineInfo& reference_line_info, PathData* const path_data) {
  // Go through every path_point, and label its:
  //  - in-lane/out-of-lane info (side-pass or lane-change)
  //  - distance to the closest obstacle.
  std::vector<PathPointDecision> path_decision;

  // 0. Initialize the path info.
  InitPathPointDecision(*path_data, &path_decision);

  // 1. Label caution types, differently for side-pass or lane-change.
  if (reference_line_info.IsChangeLanePath()) {
    // If lane-change, then label the lane-changing part to
    // be out-on-forward lane.
    SetPathPointType(reference_line_info, *path_data, true, &path_decision);
  } else {
    // Otherwise, only do the label for borrow-lane generated paths.
    // 僅僅對借道進(jìn)行標(biāo)記
    if (path_data->path_label().find("fallback") == std::string::npos &&
        path_data->path_label().find("self") == std::string::npos) {
      SetPathPointType(reference_line_info, *path_data, false, &path_decision);
    }
  }

  // SetObstacleDistance(reference_line_info, *path_data, &path_decision);
  path_data->SetPathPointDecisionGuide(std::move(path_decision));
}

這一部分中函數(shù)SetPathInfo完成以下功能：初始化path info；根據(jù)是lane-change還是side-pass，設(shè)置路徑點的類型；添加相關(guān)決策引導(dǎo)信息等信息。

SetPathPointType

在設(shè)置路徑點的類型時涉及到SetPathPointType這一個函數(shù)。
流程如下圖所示：

【Apollo學(xué)習(xí)筆記】——規(guī)劃模塊TASK之PATH_ASSESSMENT_DECIDER,Apollo,決策規(guī)劃,算法,自動駕駛,planning,apollo,人工智能

void PathAssessmentDecider::SetPathPointType(
    const ReferenceLineInfo& reference_line_info, const PathData& path_data,
    const bool is_lane_change_path,
    std::vector<PathPointDecision>* const path_point_decision) {
  // Sanity checks.
  CHECK_NOTNULL(path_point_decision);

  // Go through every path_point, and add in-lane/out-of-lane info.
  const auto& discrete_path = path_data.discretized_path();
  const auto& vehicle_config =
      common::VehicleConfigHelper::Instance()->GetConfig();
  const double ego_length = vehicle_config.vehicle_param().length();
  const double ego_width = vehicle_config.vehicle_param().width();
  const double ego_back_to_center =
      vehicle_config.vehicle_param().back_edge_to_center();
  // 車輛幾何中心點與車輛后軸的偏移距離
  const double ego_center_shift_distance =
      ego_length / 2.0 - ego_back_to_center;

  bool is_prev_point_out_lane = false;
  for (size_t i = 0; i < discrete_path.size(); ++i) {
    // 以車輛后軸中心獲取boundingbox
    const auto& rear_center_path_point = discrete_path[i];
    const double ego_theta = rear_center_path_point.theta();
    Box2d ego_box({rear_center_path_point.x(), rear_center_path_point.y()},
                  ego_theta, ego_length, ego_width);
    Vec2d shift_vec{ego_center_shift_distance * std::cos(ego_theta),
                    ego_center_shift_distance * std::sin(ego_theta)};
    // 將boundingbox從車輛后軸中心變換到幾何中心(apollo在這里采用的是AABB的boundingbox，其中有些細(xì)節(jié)等之后再細(xì)看)
    ego_box.Shift(shift_vec);
    // 得到SL坐標(biāo)系下的boundary
    SLBoundary ego_sl_boundary;
    if (!reference_line_info.reference_line().GetSLBoundary(ego_box,
                                                            &ego_sl_boundary)) {
      ADEBUG << "Unable to get SL-boundary of ego-vehicle.";
      continue;
    }
    double lane_left_width = 0.0;
    double lane_right_width = 0.0;
    double middle_s =
        (ego_sl_boundary.start_s() + ego_sl_boundary.end_s()) / 2.0;
    if (reference_line_info.reference_line().GetLaneWidth(
            middle_s, &lane_left_width, &lane_right_width)) {
      // Rough sl boundary estimate using single point lane width
      double back_to_inlane_extra_buffer = 0.2;
      double in_and_out_lane_hysteresis_buffer =
          is_prev_point_out_lane ? back_to_inlane_extra_buffer : 0.0;

      // Check for lane-change and lane-borrow differently:
      if (is_lane_change_path) {
        // For lane-change path, only transitioning part is labeled as
        // out-of-lane.
        if (ego_sl_boundary.start_l() > lane_left_width ||
            ego_sl_boundary.end_l() < -lane_right_width) {
          // This means that ADC hasn't started lane-change yet.
          // 再次重申，變道時是以要變道的目標(biāo)車道作為參考線
          std::get<1>((*path_point_decision)[i]) =
              PathData::PathPointType::IN_LANE;
        } else if (ego_sl_boundary.start_l() >
                       -lane_right_width + back_to_inlane_extra_buffer &&
                   ego_sl_boundary.end_l() <
                       lane_left_width - back_to_inlane_extra_buffer) {
          // This means that ADC has safely completed lane-change with margin.
          std::get<1>((*path_point_decision)[i]) =
              PathData::PathPointType::IN_LANE;
        } else {
          // ADC is right across two lanes.
          std::get<1>((*path_point_decision)[i]) =
              PathData::PathPointType::OUT_ON_FORWARD_LANE;
        }
      } else {
        // For lane-borrow path, as long as ADC is not on the lane of
        // reference-line, it is out on other lanes. It might even be
        // on reverse lane!
        if (ego_sl_boundary.end_l() >
                lane_left_width + in_and_out_lane_hysteresis_buffer ||
            ego_sl_boundary.start_l() <
                -lane_right_width - in_and_out_lane_hysteresis_buffer) {
          if (path_data.path_label().find("reverse") != std::string::npos) {
            std::get<1>((*path_point_decision)[i]) =
                PathData::PathPointType::OUT_ON_REVERSE_LANE;
          } else if (path_data.path_label().find("forward") !=
                     std::string::npos) {
            std::get<1>((*path_point_decision)[i]) =
                PathData::PathPointType::OUT_ON_FORWARD_LANE;
          } else {
            std::get<1>((*path_point_decision)[i]) =
                PathData::PathPointType::UNKNOWN;
          }
          if (!is_prev_point_out_lane) {
            if (ego_sl_boundary.end_l() >
                    lane_left_width + back_to_inlane_extra_buffer ||
                ego_sl_boundary.start_l() <
                    -lane_right_width - back_to_inlane_extra_buffer) {
              is_prev_point_out_lane = true;
            }
          }
        } else {
          // The path point is within the reference_line's lane.
          std::get<1>((*path_point_decision)[i]) =
              PathData::PathPointType::IN_LANE;
          if (is_prev_point_out_lane) {
            is_prev_point_out_lane = false;
          }
        }
      }
    } else {
      AERROR << "reference line not ready when setting path point guide";
      return;
    }
  }
}

PS：關(guān)于ego_box.Shift(shift_vec);這一步是如何實現(xiàn)的，可以關(guān)注這篇博客：Apollo EM中path_assesment_task相關(guān)細(xì)節(jié)的討論

3. 排序選擇最優(yōu)的路徑

  ... ...
  // 3. Pick the optimal path.
  // 3. 選擇最優(yōu)路徑，兩兩比較路徑。排序是根據(jù) ComparePathData 函數(shù)的返回值進(jìn)行的。
  std::sort(valid_path_data.begin(), valid_path_data.end(),
            std::bind(ComparePathData, std::placeholders::_1,
                      std::placeholders::_2, blocking_obstacle_on_selflane));

  ADEBUG << "Using '" << valid_path_data.front().path_label()
         << "' path out of " << valid_path_data.size() << " path(s)";
  if (valid_path_data.front().path_label().find("fallback") !=
      std::string::npos) {
    FLAGS_static_obstacle_nudge_l_buffer = 0.8;
  }
  *(reference_line_info->mutable_path_data()) = valid_path_data.front();
  reference_line_info->SetBlockingObstacle(
      valid_path_data.front().blocking_obstacle_id());
  const auto& end_time3 = std::chrono::system_clock::now();
  diff = end_time3 - end_time2;
  ADEBUG << "Time for optimal path selection: " << diff.count() * 1000
         << " msec.";
  ... ...

主要排序規(guī)則在ComparePathData函數(shù)中。

bool ComparePathData(const PathData& lhs, const PathData& rhs,
                     const Obstacle* blocking_obstacle) {
  ADEBUG << "Comparing " << lhs.path_label() << " and " << rhs.path_label();
  // Empty path_data is never the larger one.
  // 空的路徑永遠(yuǎn)排在后面
  if (lhs.Empty()) {
    ADEBUG << "LHS is empty.";
    return false;
  }
  if (rhs.Empty()) {
    ADEBUG << "RHS is empty.";
    return true;
  }
  // Regular path goes before fallback path.regular > fallback
  // 如果lhs是regular路徑而rhs是fallback路徑，那么lhs會被認(rèn)為更好，返回true。
  bool lhs_is_regular = lhs.path_label().find("regular") != std::string::npos;
  bool rhs_is_regular = rhs.path_label().find("regular") != std::string::npos;
  if (lhs_is_regular != rhs_is_regular) {
    return lhs_is_regular;
  }
  // Select longer path.
  // If roughly same length, then select self-lane path.
  bool lhs_on_selflane = lhs.path_label().find("self") != std::string::npos;
  bool rhs_on_selflane = rhs.path_label().find("self") != std::string::npos;
  static constexpr double kSelfPathLengthComparisonTolerance = 15.0;
  static constexpr double kNeighborPathLengthComparisonTolerance = 25.0;
  double lhs_path_length = lhs.frenet_frame_path().back().s();
  double rhs_path_length = rhs.frenet_frame_path().back().s();
  // 至少其中有一條是self_lane
  if (lhs_on_selflane || rhs_on_selflane) {
    // 如果兩條路徑的長度相差超過了kSelfPathLengthComparisonTolerance（在這里是15.0），那么較長的路徑將被認(rèn)為更好。
    if (std::fabs(lhs_path_length - rhs_path_length) >
        kSelfPathLengthComparisonTolerance) {
      return lhs_path_length > rhs_path_length;
    } else {
      // 如果兩條路徑的長度相差在這個容差范圍內(nèi)，并且其中一條路徑在"self"車道上，那么"self"車道上的路徑將被認(rèn)為更好。
      return lhs_on_selflane;
    }
  } else {
    // 沒有一條是self_lane
    if (std::fabs(lhs_path_length - rhs_path_length) >
        kNeighborPathLengthComparisonTolerance) {
      return lhs_path_length > rhs_path_length;
    }
  }
  // If roughly same length, and must borrow neighbor lane,
  // then prefer to borrow forward lane rather than reverse lane.
  int lhs_on_reverse =
      ContainsOutOnReverseLane(lhs.path_point_decision_guide());
  int rhs_on_reverse =
      ContainsOutOnReverseLane(rhs.path_point_decision_guide());
  // TODO(jiacheng): make this a flag.
  // 如果需要借用逆向車道的次數(shù)差超過了6次，那么次數(shù)較少的路徑將被認(rèn)為更好（相當(dāng)于選擇逆向距離短的）。
  if (std::abs(lhs_on_reverse - rhs_on_reverse) > 6) {
    return lhs_on_reverse < rhs_on_reverse;
  }
  // For two lane-borrow directions, based on ADC's position,
  // select the more convenient one.
  if ((lhs.path_label().find("left") != std::string::npos &&
       rhs.path_label().find("right") != std::string::npos) ||
      (lhs.path_label().find("right") != std::string::npos &&
       rhs.path_label().find("left") != std::string::npos)) {
    if (blocking_obstacle) {
      // select left/right path based on blocking_obstacle's position
      // 有障礙物，選擇合適的方向，左或右借道
      const double obstacle_l =
          (blocking_obstacle->PerceptionSLBoundary().start_l() +
           blocking_obstacle->PerceptionSLBoundary().end_l()) /
          2;
      ADEBUG << "obstacle[" << blocking_obstacle->Id() << "] l[" << obstacle_l
             << "]";
      // 如果阻擋障礙物的橫向位置大于0（在障礙物的右側(cè)），那么含有"right"的路徑將被認(rèn)為更好；否則，含有"left"的路徑將被認(rèn)為更好。
      return (obstacle_l > 0.0
                  ? (lhs.path_label().find("right") != std::string::npos)
                  : (lhs.path_label().find("left") != std::string::npos));
    } else {
      // select left/right path based on ADC's position
      // 無障礙物，根據(jù)adc的位置選擇借道方向
      double adc_l = lhs.frenet_frame_path().front().l();
      if (adc_l < -1.0) {
        return lhs.path_label().find("right") != std::string::npos;
      } else if (adc_l > 1.0) {
        return lhs.path_label().find("left") != std::string::npos;
      }
    }
  }
  // If same length, both neighbor lane are forward,
  // then select the one that returns to in-lane earlier.
  // 路徑長度相同，相鄰車道都是前向的，選擇較早返回自車道的路徑
  static constexpr double kBackToSelfLaneComparisonTolerance = 20.0;
  int lhs_back_idx = GetBackToInLaneIndex(lhs.path_point_decision_guide());
  int rhs_back_idx = GetBackToInLaneIndex(rhs.path_point_decision_guide());
  double lhs_back_s = lhs.frenet_frame_path()[lhs_back_idx].s();
  double rhs_back_s = rhs.frenet_frame_path()[rhs_back_idx].s();
  if (std::fabs(lhs_back_s - rhs_back_s) > kBackToSelfLaneComparisonTolerance) {
    return lhs_back_idx < rhs_back_idx;
  }
  // If same length, both forward, back to inlane at same time,
  // select the left one to side-pass.
  // 如果路徑長度相同，前向借道，返回自車道時間相同，選擇從左側(cè)借道的路徑
  bool lhs_on_leftlane = lhs.path_label().find("left") != std::string::npos;
  bool rhs_on_leftlane = rhs.path_label().find("left") != std::string::npos;
  if (lhs_on_leftlane != rhs_on_leftlane) {
    ADEBUG << "Select " << (lhs_on_leftlane ? "left" : "right") << " lane over "
           << (!lhs_on_leftlane ? "left" : "right") << " lane.";
    return lhs_on_leftlane;
  }
  // Otherwise, they are the same path, lhs is not < rhs.
  // 最后如果兩條路徑相同，則 lhs is not < rhl
  return false;
}

路徑排序規(guī)則如下：（道路評估的優(yōu)劣通過排序獲得）

1.空的路徑永遠(yuǎn)排在后面
2.regular > fallback
3.如果self-lane有一個存在，選擇那個。如果都存在，選擇較長的.如果長度接近，選擇self-lane如果self-lane都不存在，選擇較長的路徑
4.如果路徑長度接近，且都要借道:

(1) 都要借逆向車道，選擇距離短的
(2) 針對具有兩個借道方向的情況:
- 有障礙物，選擇合適的方向，左或右借道
- 無障礙物，根據(jù)adc的位置選擇借道方向
(3) 路徑長度相同，相鄰車道都是前向的，選擇較早返回自車道的路徑
(4) 如果路徑長度相同，前向借道，返回自車道時間相同，選擇從左側(cè)借道的路徑

5.最后如果兩條路徑相同，則 lhs is not < rhl

排序之后：選擇最優(yōu)路徑，即第一個路徑

4. 更新必要的信息

  // 4. Update necessary info for lane-borrow decider's future uses.
  // Update front static obstacle's info.
  auto* mutable_path_decider_status = injector_->planning_context()
                                          ->mutable_planning_status()
                                          ->mutable_path_decider();
  if (reference_line_info->GetBlockingObstacle() != nullptr) {
    int front_static_obstacle_cycle_counter =
        mutable_path_decider_status->front_static_obstacle_cycle_counter();
    mutable_path_decider_status->set_front_static_obstacle_cycle_counter(
        std::max(front_static_obstacle_cycle_counter, 0));
    mutable_path_decider_status->set_front_static_obstacle_cycle_counter(
        std::min(front_static_obstacle_cycle_counter + 1, 10));
    mutable_path_decider_status->set_front_static_obstacle_id(
        reference_line_info->GetBlockingObstacle()->Id());
  } else {
    int front_static_obstacle_cycle_counter =
        mutable_path_decider_status->front_static_obstacle_cycle_counter();
    mutable_path_decider_status->set_front_static_obstacle_cycle_counter(
        std::min(front_static_obstacle_cycle_counter, 0));
    mutable_path_decider_status->set_front_static_obstacle_cycle_counter(
        std::max(front_static_obstacle_cycle_counter - 1, -10));
  }

  // Update self-lane usage info.
  if (reference_line_info->path_data().path_label().find("self") !=
      std::string::npos) {
    // && std::get<1>(reference_line_info->path_data()
    //                 .path_point_decision_guide()
    //                 .front()) == PathData::PathPointType::IN_LANE)
    int able_to_use_self_lane_counter =
        mutable_path_decider_status->able_to_use_self_lane_counter();

    if (able_to_use_self_lane_counter < 0) {
      able_to_use_self_lane_counter = 0;
    }
    mutable_path_decider_status->set_able_to_use_self_lane_counter(
        std::min(able_to_use_self_lane_counter + 1, 10));
  } else {
    mutable_path_decider_status->set_able_to_use_self_lane_counter(0);
  }

  // Update side-pass direction.
  if (mutable_path_decider_status->is_in_path_lane_borrow_scenario()) {
    bool left_borrow = false;
    bool right_borrow = false;
    const auto& path_decider_status =
        injector_->planning_context()->planning_status().path_decider();
    for (const auto& lane_borrow_direction :
         path_decider_status.decided_side_pass_direction()) {
      if (lane_borrow_direction == PathDeciderStatus::LEFT_BORROW &&
          reference_line_info->path_data().path_label().find("left") !=
              std::string::npos) {
        left_borrow = true;
      }
      if (lane_borrow_direction == PathDeciderStatus::RIGHT_BORROW &&
          reference_line_info->path_data().path_label().find("right") !=
              std::string::npos) {
        right_borrow = true;
      }
    }

    mutable_path_decider_status->clear_decided_side_pass_direction();
    if (right_borrow) {
      mutable_path_decider_status->add_decided_side_pass_direction(
          PathDeciderStatus::RIGHT_BORROW);
    }
    if (left_borrow) {
      mutable_path_decider_status->add_decided_side_pass_direction(
          PathDeciderStatus::LEFT_BORROW);
    }
  }
  const auto& end_time4 = std::chrono::system_clock::now();
  diff = end_time4 - end_time3;
  ADEBUG << "Time for FSM state updating: " << diff.count() * 1000 << " msec.";

  // Plot the path in simulator for debug purpose.
  RecordDebugInfo(reference_line_info->path_data(), "Planning PathData",
                  reference_line_info);
  return Status::OK();

更新必要信息：

1.更新adc前方靜態(tài)障礙物的信息
2.更新自車道使用信息
3.更新旁車道的方向根據(jù)：PathDeciderStatus是RIGHT_BORROW或LEFT_BORROW判斷是從左側(cè)借道，還是從右側(cè)借道文章來源地址http://www.zghlxwxcb.cn/news/detail-679223.html

到了這里，關(guān)于【Apollo學(xué)習(xí)筆記】——規(guī)劃模塊TASK之PATH_ASSESSMENT_DECIDER的文章就介紹完了。如果您還想了解更多內(nèi)容，請在右上角搜索TOY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關(guān)文章，希望大家以后多多支持TOY模板網(wǎng)！