Apollo Planning学习(6)-------PATH_ASSESSMENT_DECIDER

PATH_ASSESSMENT_DECIDER

PathAssessmentDecider会依据设计好的规则筛选处最终的path，并在规划路径上的采样点添加标签
（IN_LANE、OUT_ON_FORWARD_LANE、OUT_ON_REVERSE_LANE等），作为路径筛选的依据，并为速度规划提供限制。

在学习代码之前先了解一下该决策器中常用的一种数据结构

namespace {
// PointDecision contains (s, PathPointType, distance to closest obstacle).
using PathPointDecision = std::tuple<double, PathData::PathPointType, double>;
constexpr double kMinObstacleArea = 1e-4;
} 

enum class PathPointType {
    IN_LANE,
    OUT_ON_FORWARD_LANE,
    OUT_ON_REVERSE_LANE,
    OFF_ROAD,
    UNKNOWN,
  };

和别的decider一样，处理主要逻辑的函数为process，我们从代码入手，进入源代码分析

Status PathAssessmentDecider::Process(
    Frame* const frame, ReferenceLineInfo* const reference_line_info) {
  // Sanity checks.
  CHECK_NOTNULL(frame);
  CHECK_NOTNULL(reference_line_info);
  // skip path_assessment_decider if reused path
  // 如果路径重复使用则跳过
  if (FLAGS_enable_skip_path_tasks && reference_line_info->path_reusable()) {
    return Status::OK();
  }
  // 返回类型为const std::vector<PathData>&
  // pathdata这个类在路径\modules\planning\common\path\path_data.h下
  const auto& candidate_path_data = reference_line_info->GetCandidatePathData();

  if (candidate_path_data.empty()) {
    ADEBUG << "Candidate path data is empty.";
  } else {
    ADEBUG << "There are " << candidate_path_data.size() << " candidate paths";
  }
  const auto& end_time0 = std::chrono::system_clock::now();

  // 1. Remove invalid path.即移除无效路径
  std::vector<PathData> valid_path_data;
  for (const auto& curr_path_data : candidate_path_data) {
  // 这个for循环涉及的两个子函数较好理解，就不做单独分析
    if (curr_path_data.path_label().find("fallback") != std::string::npos) {
        // 这里fallback的无效路径是偏离参考线过多，偏离road过多
      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.";

  // 2. Analyze and add important info for speed decider to use
  // 2. 分析并加入重要信息，提供给速度决策器
  // 这部分没太读懂，放在后面学习速度规划时再回过头分析
  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;
    }
    // 此函数后面简单拿出来分析一下
    // 主要作用就是设置一些相关的路径信息
    SetPathInfo(*reference_line_info, &curr_path_data);
    // Trim all the lane-borrowing paths so that it ends with an in-lane
    // position.
    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
    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.";

  // 3. Pick the optimal path.
  // 排序选出最优的轨迹
  /*
  		这部分排序的优先顺序为：
  		1.空的路径永远排在后面
		2.Regular path优先于fallback path；
		3.如果两条路径至少有一条是self_lane，并且两条路径长度的差大于15m，选择路径长的；
		4.如果两条路径至少有一条是self_lane，并且两条路径长度的差小于5m，是self_lane的；
		5.如果两条路径都不是self_lane，并且两条路径长度的差大于25m，选择路径长的；
		6.选择占据反向车道更少的路径；
	    7.如果有block obstacle，选择占据空间少的方向的路径；
		8.如果没有block obstacle，选择ADC靠近方向的路径，使车辆少打方向盘；
		9.选择返回本车道更早的路径；
		10.在上述情况无法区分的情况下选择左侧的路径；
	具体代码实现看函数bool ComparePathData(const PathData& lhs, const PathData& rhs,
                     const Obstacle* blocking_obstacle)
                     此函数里的if-else条件就是这些优先顺序规则，就不单独拿出来分析了
  */
  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.";

  reference_line_info->SetCandidatePathData(std::move(valid_path_data));

  // 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.根据PathDeciderStatus是left_borrow或right_borrow，即判断是从左侧借道，还是从右侧借道
  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();
}

PathAssessmentDecider::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.
  // PointDecision contains (s, PathPointType, distance to closest obstacle).
  // using PathPointDecision = std::tuple<double, PathData::PathPointType, double>;
  std::vector<PathPointDecision> path_decision;

  // 0. Initialize the path info.
  // 这里就是简单的初始化较好理解，就不单独拿出来分析
  InitPathPointDecision(*path_data, &path_decision);

  // 1. 贴上警示类型标签，不同于侧道或变道。
  if (reference_line_info.IsChangeLanePath()) {
    // 如果变道，则将变道部分标记为在前向车道外。
    // 后面单独分析 SetPathPointType 这个函数
    SetPathPointType(reference_line_info, *path_data, true, &path_decision);
  } else {
    // 否则，仅为借用车道生成的路径添加标签。
    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));
}

PathAssessmentDecider::SetPathPointType简单分析

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) {
  // 检查输入
  CHECK_NOTNULL(path_point_decision);

  // 浏览每个路径点，并添加车道内/车道外信息。
  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) {
    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)};
    ego_box.Shift(shift_vec);
    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:
      // 这里处理逻辑在面对lanechange和laneborrow有着不同的处理逻辑
      if (is_lane_change_path) {
        // 对于车道变更路径，只有过渡部分标记为车道外。
        // 这里注意lanechange时的参考线有可能是目标车道参考线
        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.
          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 {
        // 对于车道借用路径，只要ADC不在参考线的车道上，它就在其他车道上。
        // 它甚至可能在反向车道上！
        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;
    }
  }
}