Apollo：EgoInfo类详解

EgoInfo类是apollo planning模块下modules\planning\common\ego_info.cc/.h实现

通过ego_info_test.cc给出的测试示例，可以了解该类的功能及使用方式。

从类名来看，应该是自车信息类，包含了自车信息，例如：当前位置点，车辆状态，外围Box等。

成员	－	说明	其他｜
common::TrajectoryPoint	start_point_;	起始点(在轨迹拼接模式下就是轨迹拼接点)　或者车辆位置点(规划处于非轨迹拼接模式下)
common::VehicleState	vehicle_state_;	车辆状态
double	front_clear_distance_ = FLAGS_default_front_clear_distance;	前方畅通距离，默认为300m
common::VehicleConfig	ego_vehicle_config_	自车配置，主要是包括车辆的品牌，长宽高，轴距，最大加速度等车辆物理参数	vehicle_config.proto
common::math::Box2d	ego_box_	自车二维边界框，车辆航向方向为长度，其法向为宽度	定义在modules\common\math\box2d.cc

  //设定类数据成员 规划起始点start_point_
  void set_start_point(const common::TrajectoryPoint& start_point) {
    start_point_ = start_point;
    //设定车辆参数时考虑车辆物理参数的限制
    const auto& param = ego_vehicle_config_.vehicle_param();
    start_point_.set_a(
        std::fmax(std::fmin(start_point_.a(), param.max_acceleration()),
                  param.max_deceleration()));
  }

set_start_point

EgoInfo::EgoInfo() {
//调用VehicleConfigHelper类的成员函数GetConfig()获取车辆配置，并将
//配置放入EgoInfo类数据成员ego_vehicle_config_
  ego_vehicle_config_ = common::VehicleConfigHelper::GetConfig();
}

//成员函数更新自车信息，起始点，自车状态以及自车二维边界盒
//参数：起始轨迹点，车辆状态
bool EgoInfo::Update(const common::TrajectoryPoint& start_point,
                     const common::VehicleState& vehicle_state) {
  //用参数start_point设定数据成员起始点start_point_
  set_start_point(start_point);
  //用参数vehicle_state设定数据成员起始点vehicle_state_
  set_vehicle_state(vehicle_state);
  //用参数vehicle_state计算自车的二维边界盒
  CalculateEgoBox(vehicle_state);
  return true;
}


//计算二维自车边界盒
//参数：车辆状态
void EgoInfo::CalculateEgoBox(const common::VehicleState& vehicle_state) {
  //首先获取自车配置里的车辆参数先放到变量param中
  const auto& param = ego_vehicle_config_.vehicle_param();
  ADEBUG << "param: " << param.DebugString();

  //vec_to_center是整辆车的几何中心点在车体系下坐标，因为假设车体系后轴中心坐标(0,0)
  //front_edge_to_center里的center是后轴中心车体系坐标
  //假设车辆后轴中心为(0,0)，那么车辆前边缘到后轴中心距离就是front_edge_to_center为正，坐标也为front_edge_to_center
  //车辆后边缘到后轴中心距离就是back_edge_to_center，但是方向是负的为负，坐标是-back_edge_to_center
  //所以在前后方向几何中点为(front_edge_to_center-back_edge_to_center)/2.0
  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);

  //车辆的位置坐标position就从车辆状态里获取后轴中心大地XY坐标
  Vec2d position(vehicle_state.x(), vehicle_state.y());
  //将车辆的几何中心的车体系坐标通过heading角旋转，大地坐标XY平移转换到大地坐标系下
  //center是车辆几何中心的大地坐标
  Vec2d center(position + vec_to_center.rotate(vehicle_state.heading()));

  //调用Box2d()构造函数，通过车辆几何中心，heading航向角，车辆的长度，宽度，构建
  //二维边界盒对象ego_box_ 
  ego_box_ =
      Box2d(center, vehicle_state.heading(), param.length(), param.width());
}


//清空函数
void EgoInfo::Clear() {
  //类成员起始点清空
  start_point_.Clear();
  //类成员车辆状态清空
  vehicle_state_.Clear();
  //类成员自车前方畅通距离恢复配置文件中的默认值
  //FLAGS_取出modules\planning\common\planning_gflags.cc里定义
  //default_front_clear_distance的值300m
  front_clear_distance_ = FLAGS_default_front_clear_distance;
}

// Apollo原注释 TODO(all):移除这个函数以及与"front_clear_distance"相关的部分
// 以下两种情况似乎没有意义：
// 1. 航向没有必要与道路对齐
// 2. 道路没有必要笔直
//这个函数是计算前方障碍物畅通距离
//参数是障碍物列表Obstacle类型的vector
void EgoInfo::CalculateFrontObstacleClearDistance(
    const std::vector<const Obstacle*>& obstacles) {
  //车辆位置也就是后轴中心的大地XY坐标放入二维向量position
  Vec2d position(vehicle_state_.x(), vehicle_state_.y());

  //获取车辆的物理参数放入param 
  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);

  //将车辆几何中心的大地坐标经过heading角旋转，position平移转换到大地XY坐标系下
  Vec2d center(position + vec_to_center.rotate(vehicle_state_.heading()));
  //车辆航向方向的单位向量(cos(heading),sin(heading))
  Vec2d unit_vec_heading = Vec2d::CreateUnitVec2d(vehicle_state_.heading());

  //由于车辆航向角的误差，只有短距离的范围才有意义
  //定义了一个距离阈值50m
  static constexpr double kDistanceThreshold = 50.0;
  //定义了一个buffer缓冲距离 0.1m
  static constexpr double buffer = 0.1;  // in meters
  //影响区域长度 = 车辆长度 + 缓冲距离 + 距离阈值
  const double impact_region_length =
      param.length() + buffer + kDistanceThreshold;

  //这个是定义了一个Box2d二维边界盒类对象ego_front_region
  //这个对象ego_front_region用括号里的参数去初始化,输入了4个参数
  //所以调用的构造函数原型
  //Box2d(const Vec2d &center, const double heading, const double length,
  //      const double width);
  //center + unit_vec_heading * kDistanceThreshold / 2.0意义是从自车几何中心
           //朝前方heading方向走kDistanceThreshold距离一半的点坐标作为边界盒的中心
  //heading自车heading角
  //影响区域长度impact_region_length作为边界盒的长度
  //车辆的宽度+0.1m缓冲作为边界盒的宽度
  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) {
    //如果障碍物是虚拟的或者自车前方区域50m边界盒与障碍物边界盒没有重叠
    //直接看下一个障碍物
    if (obstacle->IsVirtual() ||
        !ego_front_region.HasOverlap(obstacle->PerceptionBoundingBox())) {
      continue;
    }
    //如果执行到此步，说明自车前方区域50m边界盒与障碍物边界盒有重叠且障碍物不是虚拟
    //计算障碍物边界盒边缘到自车边界盒中心的距离减去自车边界盒对角线一半(按照最恶劣的情况)
    //减去的最大，就是考虑自车边缘离障碍物边缘剩余距离最恶劣的情况
    //dist就是自车边缘距障碍物边缘最近的距离
    double dist = ego_box_.center().DistanceTo(
                      obstacle->PerceptionBoundingBox().center()) -
                  ego_box_.diagonal() / 2.0;

    //如果前方畅通距离<0或 自车边缘距障碍物边缘最近的距离<前方畅通距离 
    //找到一个更近障碍物挡住了，则类成员前方畅通距离变为到这个更近障碍物的dist
    if (front_clear_distance_ < 0.0 || dist < front_clear_distance_) {
      front_clear_distance_ = dist;
    }
  }
}