planning相关算法函数

modules/planning/planning_base/common/ego_info.cc

1.CalculateFrontObstacleClearDistance

void EgoInfo::CalculateFrontObstacleClearDistance(
    const std::vector<const Obstacle*>& obstacles) {

  //获取自车(后轴中心)在笛卡尔坐标系下的坐标
  Vec2d position(vehicle_state_.x(), vehicle_state_.y());

  const auto& param = ego_vehicle_config_.vehicle_param();


  //计算自车中心点在自车坐标系(后轴中心为原点)下坐标
  Vec2d vec_to_center(
      (param.front_edge_to_center() - param.back_edge_to_center()) / 2.0,
      (param.left_edge_to_center() - param.right_edge_to_center()) / 2.0);

  //先将自车中心点自车坐标系下的坐标转换为笛卡尔坐标系上的坐标，然后在加上自车(后轴中心)笛卡尔坐标系下的坐标，得到自车中心点在笛卡尔坐标系下的坐标
  Vec2d center(position + vec_to_center.rotate(vehicle_state_.heading()));

  Vec2d unit_vec_heading = Vec2d::CreateUnitVec2d(vehicle_state_.heading());

  // Due to the error of ego heading, only short range distance is meaningful
  static constexpr double kDistanceThreshold = 50.0;
  static constexpr double buffer = 0.1;  // in meters

  //关注的自车前方区域长度
  const double impact_region_length =
      param.length() + buffer + kDistanceThreshold;

  //关注的自车前方矩形区域
  Box2d ego_front_region(center + unit_vec_heading * kDistanceThreshold / 2.0,
                         vehicle_state_.heading(), impact_region_length,
                         param.width() + buffer);

  //跳过虚拟障碍物和与关注的前方矩形区域不重叠的障碍物
  for (const auto& obstacle : obstacles) {
    if (obstacle->IsVirtual() ||
        !ego_front_region.HasOverlap(obstacle->PerceptionBoundingBox())) {
      continue;
    }
    //计算自车与障碍物中心点间的距离并减去了自车矩形框对角线的一半
    double dist = ego_box_.center().DistanceTo(
                      obstacle->PerceptionBoundingBox().center()) -
                  ego_box_.diagonal() / 2.0;
    //获取到与关注区域重叠的最近障碍物障碍物距离
    if (front_clear_distance_ < 0.0 || dist < front_clear_distance_) {
      front_clear_distance_ = dist;
    }
  }
}

modules/map/pnc_map/pnc_map_base.cc
2.LookForwardDistance

double PncMapBase::LookForwardDistance(const double velocity) {
  //8s自车向前行驶的距离速度单位m/s
  auto forward_distance = velocity * FLAGS_look_forward_time_sec;
  //如果行驶距离大于180m,返回250m，否则返回190m
  return forward_distance > FLAGS_look_forward_short_distance
             ? FLAGS_look_forward_long_distance
             : FLAGS_look_forward_short_distance;
}