百度apollo自动驾驶planning代码学习-Apollo\modules\planning\common\EgoInfo类代码详解

概述

EgoInfo类是apollo planning模块下modules\planning\common\ego_info.cc/.h实现
通过ego_info_test.cc给出的示例，可以了解该类的功能及使用方式。

从类名来看，应该是自车信息类。

从代码来看EgoInfo类主要是实现：
首先储存自车信息(包括规划轨迹起始点，车辆状态-定位及底盘反馈信息，前方无障碍物的畅通距离，自车二维边界盒信息等)。
其中无障碍物的畅通距离，自车二维边界盒信息通过成员函数计算得到。
自车信息的更新/清空。

ego_info.h

#pragma once

#include <vector>

#include "cyber/common/macros.h"
#include "modules/common/configs/proto/vehicle_config.pb.h"
#include "modules/common/vehicle_state/proto/vehicle_state.pb.h"
#include "modules/planning/common/obstacle.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/reference_line/reference_line.h"

namespace apollo {
namespace planning {

class EgoInfo {
 public:
  //EgoInfo类构造函数，无参数
  //调用VehicleConfigHelper类的成员函数GetConfig()获取车辆配置，并将
  //配置放入EgoInfo类数据成员ego_vehicle_config_
  EgoInfo();

  //默认析构函数
  ~EgoInfo() = default;

  //成员函数更新自车信息，规划起始点？，自车状态以及自车二维边界盒
  //参数：起始轨迹点，车辆状态
  bool Update(const common::TrajectoryPoint& start_point,
              const common::VehicleState& vehicle_state);

  //清空函数
  void Clear();

  //返回类数据成员 起始点start_point_
  common::TrajectoryPoint start_point() const { return start_point_; }

  //返回类数据成员 车辆状态vehicle_state_
  common::VehicleState vehicle_state() const { return vehicle_state_; }

  //返回类数据成员 前方畅通距离front_clear_distance_
  double front_clear_distance() const { return front_clear_distance_; }

  //返回类数据成员 自车二维边界盒ego_box_
  common::math::Box2d ego_box() const { return ego_box_; }

  //这个函数是计算前方无障碍物的畅通距离
  //参数是障碍物列表Obstacle类型的vector
  void CalculateFrontObstacleClearDistance(
      const std::vector<const Obstacle*>& obstacles);

 private:
  //定义了一个测试？
  FRIEND_TEST(EgoInfoTest, EgoInfoSimpleTest);

  //设定类数据成员 车辆状态vehicle_state_
  void set_vehicle_state(const common::VehicleState& vehicle_state) {
    vehicle_state_ = vehicle_state;
  }

  //设定类数据成员 规划起始点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()));
  }

  //根据车辆状态计算自车二维边界盒
  void CalculateEgoBox(const common::VehicleState& vehicle_state);

  //起始点就是轨迹拼接点(在规划轨迹拼接模式下) 或
  //真是车辆位置点(规划处于非轨迹拼接模式下)
  common::TrajectoryPoint start_point_;

  //数据成员 车辆状态vehicle_state_
  common::VehicleState vehicle_state_;

  //数据成员 前方畅通距离front_clear_distance_
  //默认为300m
  double front_clear_distance_ = FLAGS_default_front_clear_distance;
  //类数据成员 自车配置ego_vehicle_config_ 主要是包含车辆的一些物理参数
  common::VehicleConfig ego_vehicle_config_;
  //类数据成员 自车二维边界盒ego_box_
  common::math::Box2d ego_box_;
};

}  // namespace planning
}  // namespace apollo

ego_info.cc

#include "modules/planning/common/ego_info.h"

#include "cyber/common/log.h"
#include "modules/common/configs/vehicle_config_helper.h"

namespace apollo {
namespace planning {

//Box2d类定义于modules\common\math\box2d.cc
//二维矩形框类，车辆航向方向为长度，其法向为宽度。主要是用来建立边界盒？
using apollo::common::math::Box2d;
//Vec2d二维向量类
using apollo::common::math::Vec2d;

//EgoInfo类构造函数，无参数
//调用VehicleConfigHelper类的成员函数GetConfig()获取车辆配置，并将
//配置放入EgoInfo类数据成员ego_vehicle_config_
EgoInfo::EgoInfo() {
  //ego_vehicle_config_是VehicleConfig类对象
  //VehicleConfig类由modules\common\configs\proto\vehicle_config.proto
  //由.proto文件定义的message类生成c++类
  //VehicleConfig类里包括车辆的品牌，长宽高，轴距，最大加速度等车辆物理参数
  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;
    }
  }
}

}  // namespace planning
}  // namespace apollo

ego_info_test.cc

#include "modules/planning/common/ego_info.h"

#include "gtest/gtest.h"
#include "modules/common/util/point_factory.h"
#include "modules/planning/common/frame.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/reference_line/reference_line_provider.h"

namespace apollo {
namespace planning {

//仅仅定义了一个测试
TEST(EgoInfoTest, EgoInfoSimpleTest) {
  //定义了一个路径点PathPoint，其x,y,z,s,theta,kappa,dkappa,ddkappa分别为括号里的数字
  const auto p = common::util::PointFactory::ToPathPoint(1.23, 3.23, 52.18, 0.0,
                                                         0.1, 0.3, 0.32, 0.4);
  //又定义一个轨迹点TrajectoryPoint类对象tp
  //轨迹点=路径点+速度规划
  common::TrajectoryPoint tp;
  //设定这个轨迹点tp里的路径点信息为p
  tp.mutable_path_point()->CopyFrom(p);
  //定义一个EgoInfo类唯一智能空指针对象ego_info
  auto ego_info = std::make_unique<EgoInfo>();
  //设定ego_info的规划起始点为tp
  ego_info->set_start_point(tp);
  //测试：判断ego_in里的规划起始点x,y,z坐标是否等于p点x,y,z坐标
  EXPECT_DOUBLE_EQ(ego_info->start_point().path_point().x(), p.x());
  EXPECT_DOUBLE_EQ(ego_info->start_point().path_point().y(), p.y());
  EXPECT_DOUBLE_EQ(ego_info->start_point().path_point().z(), p.z());

  //初始定义一个序列号sequence_num为0
  uint32_t sequence_num = 0;
  //定义了一个空的轨迹点 规划起始点planning_start_point
  common::TrajectoryPoint planning_start_point;
  //定义了一个空的车辆状态类对象 vehicle_state
  common::VehicleState vehicle_state;
  //定义了一个参考线提供器类对象reference_line_provider
  ReferenceLineProvider reference_line_provider;

  //又定义了一个空的LocalView类对象
  LocalView dummy_local_view;
  //将上述定义的空对象全部作为参数构造Frame类对象
  Frame frame(sequence_num, dummy_local_view, planning_start_point,
              vehicle_state, &reference_line_provider);
  //最后计算自车的前方畅通距离
  ego_info->CalculateFrontObstacleClearDistance(frame.obstacles());
  //然后是不是这个测试没写完...参数都是空的，也没有再写新的测试，只是测了一下x,y,z有没有传进去?可以参照这个测试进行EgoInfo类对象的构建以及成员函数的调用
}

}  // namespace planning
}  // namespace apollo