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

wujiangzhu_xjtu

已于 2022-05-07 22:00:38 修改

阅读量723

点赞数 1

分类专栏： Apollo学习笔记自动驾驶文章标签：自动驾驶人工智能 c++

于 2022-05-04 23:02:30 首次发布

本文链接：https://blog.csdn.net/weixin_39199083/article/details/124575643

版权

自动驾驶同时被 2 个专栏收录

86 篇文章 101 订阅

订阅专栏

Apollo学习笔记

70 篇文章 244 订阅

订阅专栏

概述

DecisionData类是apollo planning模块下common/decision_data.cc/.h实现

从类名来看，应该是存放行为决策的数据结构类。

从代码来看DecisionData类主要是实现：

储存所有的预测障碍物，按动态/静态/所有障碍物分类储存成列表，并且建立id和障碍物对象的映射map，然后可以通过id/类型来查询障碍物；
结合道路参考线信息，可以创建虚拟障碍物对象，并储存到相应的数据成员障碍物列表中。

decision_data.h

#pragma once

#include "modules/common/math/box2d.h"
#include "modules/planning/common/obstacle.h"
#include "modules/planning/reference_line/reference_line.h"
#include "modules/prediction/proto/prediction_obstacle.pb.h"

namespace apollo {
namespace planning {
//先定义了个虚拟障碍物枚举类型？
enum class VirtualObjectType {
  DESTINATION = 0,   //目标点
  CROSSWALK = 1,   //人行道
  TRAFFIC_LIGHT = 2,  //交通灯
  CLEAR_ZONE = 3,  //禁停区？
  REROUTE = 4,  //重新routing?
  DECISION_JUMP = 5,  //决策
  PRIORITY = 6 //优先行驶？
};

//这个struct是当class在用
//类里重载小括号()通常是为了将对象实例当作函数来使用，这个函数又可以像对象一样传来传去
//同时这个函数还可以像对象一样携带内部状态和信息
//这个函数是实现类型转换？
struct EnumClassHash {
  template <typename T>  //这里是一个函数模板，T可以为任意类型
  size_t operator()(T t) const { //size_t为函数模板的返回类型，类似于无符号整型
                                 //unsigned int，其能表示的数范围取决于机器
    //static_cast
    return static_cast<size_t>(t);
  }
};
//定义了一个DecisionData决策数据类
class DecisionData {
 public:
  //带参构造函数
  //参数：prediction_obstacles预测障碍物信息，reference_line道路参考线信息
  //用所有预测障碍物，以及道路参考线对象去构造决策数据类对象
  DecisionData(const prediction::PredictionObstacles& prediction_obstacles,
               const ReferenceLine& reference_line);
  ~DecisionData() = default;

 public:
  //根据id获取障碍物Obstacle类对象
  Obstacle* GetObstacleById(const std::string& id);
  //根据障碍物类型获取障碍物对象，参数是虚拟障碍物类型
  std::vector<Obstacle*> GetObstacleByType(const VirtualObjectType& type);
  //根据类型获取障碍物id，函数输出是一个string类型unordered_set无序关联容器，理解为一
  //个字符串列表，里面存放所有这类障碍物的id，输入参数是虚拟障碍物类型
  //VirtualObjectType
  std::unordered_set<std::string> GetObstacleIdByType(
      const VirtualObjectType& type);
      
  //获取静态障碍物函数，返回是一个Obstacle的vector就是障碍物列表
  //直接返回DecisionData决策数据类的数据成员static_obstacle_
  const std::vector<Obstacle*>& GetStaticObstacle() const;
  //获取动态障碍物函数，返回是一个Obstacle的vector就是障碍物列表
  //直接返回DecisionData决策数据类的数据成员dynamic_obstacle_
  const std::vector<Obstacle*>& GetDynamicObstacle() const;
  //获取虚拟障碍物函数，返回是一个Obstacle的vector就是障碍物列表
  //直接返回DecisionData决策数据类的数据成员virtual_obstacle_
  const std::vector<Obstacle*>& GetVirtualObstacle() const;
  //获取实际障碍物函数(相对虚拟障碍物而言)，返回是一个Obstacle的vector就是障碍物列表
  //直接返回DecisionData决策数据类的数据成员practical_obstacle_
  const std::vector<Obstacle*>& GetPracticalObstacle() const;
  //获取全部障碍物函数，返回是一个Obstacle的vector就是障碍物列表
  //直接返回DecisionData决策数据类的数据成员all_obstacle_
  const std::vector<Obstacle*>& GetAllObstacle() const;

 public:
  //创建虚拟障碍物函数
  //参数 ReferencePoint参考点，可以获取其x,y,s,heading,v,kappa,dkappa等信息
		//这个point是虚拟障碍物的位置？
  //参数 虚拟障碍物类型type
  //参数 障碍物id
  bool CreateVirtualObstacle(const ReferencePoint& point,
                             const VirtualObjectType& type,
                             std::string* const id);
  //重载创建虚拟障碍物函数，参数是障碍物frenet系s坐标，虚拟障碍物类型，以及障碍物id
  //过程于与上方类似，最后也是调用CreateVirtualObstacle(box, type, id);
  //通过box，虚拟障碍物类型，障碍物id来创建虚拟障碍物
  bool CreateVirtualObstacle(const double point_s,
                             const VirtualObjectType& type,
                             std::string* const id);

 private:
  //判断是否是有效的轨迹，输入的参数就是轨迹
  bool IsValidTrajectory(const prediction::Trajectory& trajectory);
  //检查轨迹点是否有效？输入参数是一个轨迹点
  bool IsValidTrajectoryPoint(const common::TrajectoryPoint& point);
  //通过二维障碍物盒box，虚拟障碍物类型，障碍物id来创建虚拟障碍物
  bool CreateVirtualObstacle(const common::math::Box2d& obstacle_box,
                             const VirtualObjectType& type,
                             std::string* const id);

 private:
  std::vector<Obstacle*> static_obstacle_;//数据成员 静态障碍物列表
  std::vector<Obstacle*> dynamic_obstacle_;//数据成员 动态障碍物列表
  std::vector<Obstacle*> virtual_obstacle_;//数据成员 虚拟障碍物列表
  std::vector<Obstacle*> practical_obstacle_;//数据成员 实际障碍物列表
  std::vector<Obstacle*> all_obstacle_;//数据成员 所有障碍物列表

 private:
  const ReferenceLine& reference_line_; //数据成员 道路参考线类对象
  std::list<std::unique_ptr<Obstacle>> obstacles_;//数据成员 障碍物列表
  //数据成员 障碍物无序map是存储多对id和障碍物对象的映射关系
  std::unordered_map<std::string, Obstacle*> obstacle_map_; 
  //虚拟障碍物无序map是存储多对id和虚拟障碍物对象的映射关系？                                            
  std::unordered_map<VirtualObjectType, std::unordered_set<std::string>,
                     EnumClassHash>
      virtual_obstacle_id_map_;
  std::mutex mutex_;
  std::mutex transaction_mutex_;
};

}  // namespace planning
}  // namespace apollo

decision_data.cc

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

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

namespace apollo {
namespace planning {

//这项检查将移至planning的一开始?apollo原注释
//检查轨迹点是否有效？输入参数是一个轨迹点
bool DecisionData::IsValidTrajectoryPoint(
    const common::TrajectoryPoint& point) {
  //如果轨迹点无path_point路径点，或x/y/z/kappa/s...只要有一个是nan(not a number)
  //只要有一个是nan则轨迹点无效，返回false
  return !((!point.has_path_point()) || std::isnan(point.path_point().x()) ||
           std::isnan(point.path_point().y()) ||
           std::isnan(point.path_point().z()) ||
           std::isnan(point.path_point().kappa()) ||
           std::isnan(point.path_point().s()) ||
           std::isnan(point.path_point().dkappa()) ||
           std::isnan(point.path_point().ddkappa()) || std::isnan(point.v()) ||
           std::isnan(point.a()) || std::isnan(point.relative_time()));
}

//判断是否是有效的轨迹，输入的参数就是轨迹
bool DecisionData::IsValidTrajectory(const prediction::Trajectory& trajectory) {
  //遍历轨迹上的每个轨迹点，判断每一个轨迹点是否为有效的轨迹点，只要有一个点无效轨迹就
  //无效，报error返回false
  for (const auto& point : trajectory.trajectory_point()) {
    if (!IsValidTrajectoryPoint(point)) {
      AERROR << " TrajectoryPoint: " << trajectory.ShortDebugString()
             << " is NOT valid.";
      return false;
    }
  }
  return true;
}

//带参构造函数
//参数：prediction_obstacles预测障碍物信息，reference_line道路参考线信息
//用所有预测障碍物，以及道路参考线对象去构造决策数据类对象
DecisionData::DecisionData(
    const prediction::PredictionObstacles& prediction_obstacles,
    const ReferenceLine& reference_line)
    : reference_line_(reference_line) { //冒号后面是类对象成员初始化方式
    							//用reference_line初始化成员reference_line_
  //遍历预测障碍物
  for (const auto& prediction_obstacle :
       prediction_obstacles.prediction_obstacle()) {
    //获取第i个预测障碍物的感知id存放到perception_id
    const std::string perception_id =
        std::to_string(prediction_obstacle.perception_obstacle().id());
    //如果第i个预测障碍物的轨迹为空，是说明是静态的障碍物？塞到障碍物列表后直接跳到下一
    //个障碍物循环，这个if里主要是塞入静态障碍物
    if (prediction_obstacle.trajectory().empty()) {
      //类成员obstacles_在decision_data.h里被声明，就是一个障碍物类型指针的list
      //将id和预测障碍物塞入障碍物列表obstacles_
      obstacles_.emplace_back(new Obstacle(
          perception_id, prediction_obstacle.perception_obstacle()));
      //all_obstacle_也是障碍物的vector，也将刚刚无轨迹的障碍物对象塞入
      all_obstacle_.emplace_back(obstacles_.back().get());
      //practical_obstacle_实际障碍物vector，也将刚刚无轨迹的障碍物对象塞入
      practical_obstacle_.emplace_back(obstacles_.back().get());
      //static_obstacle_静态障碍物vector，也将刚刚无轨迹的障碍物对象塞入
      static_obstacle_.emplace_back(obstacles_.back().get());
      //unordered_map obstacle_map_关键字障碍物对应的id映射到刚刚的障碍物对象
      obstacle_map_[perception_id] = obstacles_.back().get();
      continue;
    }
    //轨迹下标？trajectory_index
    int trajectory_index = 0;
    //针对第i个预测障碍物，遍历预测障碍物的轨迹
    for (const auto& trajectory : prediction_obstacle.trajectory()) {
      //判断对第i个预测障碍物，第j个轨迹是否有效，否则报错跳到下一条轨迹
      if (!IsValidTrajectory(trajectory)) {
        AERROR << "obj:" << perception_id;
        continue;
      }
      //定义障碍物id为"感知障碍物id + 预测轨迹下标j"
      const std::string obstacle_id =
          absl::StrCat(perception_id, "_", trajectory_index);
      //障碍物列表obstacles_塞入障碍物id，第i个预测障碍物，及第j条轨迹
      obstacles_.emplace_back(new Obstacle(
          obstacle_id, prediction_obstacle.perception_obstacle(), trajectory));
      //下面几个vector是障碍物列表，执行到此处说明是动态障碍物，塞入
      all_obstacle_.emplace_back(obstacles_.back().get());
      practical_obstacle_.emplace_back(obstacles_.back().get());
      dynamic_obstacle_.emplace_back(obstacles_.back().get());
      obstacle_map_[obstacle_id] = obstacles_.back().get();
      ++trajectory_index;
    }
  }
}

//根据id获取障碍物Obstacle类对象
Obstacle* DecisionData::GetObstacleById(const std::string& id) {
  std::lock_guard<std::mutex> lock(mutex_);
  //就是用Id关键字去unordered_map里查其对应的value
  return common::util::FindPtrOrNull(obstacle_map_, id);
}

//根据障碍物类型获取障碍物对象，参数是虚拟障碍物类型
std::vector<Obstacle*> DecisionData::GetObstacleByType(
    const VirtualObjectType& type) {
  std::lock_guard<std::mutex> lock(transaction_mutex_);
  //先调用根据障碍物类型获取这类所有障碍物id函数，获取多个id放入unordered_set无序关联
  //容器ids里
  std::unordered_set<std::string> ids = GetObstacleIdByType(type);
  //如果ids为空，返回一个空的Obstacle指针vector，障碍物指针列表。
  if (ids.empty()) {
    return std::vector<Obstacle*>();
  }
  //又定义了一个障碍物指针列表ret
  std::vector<Obstacle*> ret;
  //遍历id列表ids，再调用根据id获取障碍物对象的函数
  for (const std::string& id : ids) {
    //将ids中每个id对应的障碍物塞入障碍物指针列表ret
    ret.emplace_back(GetObstacleById(id));
    //经过上面的塞入emplace操作后，若ret列表里的最后一个障碍物仍为空，则报错，说明
    //上一步中根据id没有找到障碍物
    if (ret.back() == nullptr) {
      AERROR << "Ignore. can't find obstacle by id: " << id;
      ret.pop_back();
    }
  }
  //返回障碍物指针列表ret
  return ret;
}

//根据类型获取障碍物id，函数输出是一个string类型unordered_set无序关联容器，理解为一个
//字符串列表，里面存放所有这类障碍物的id，输入参数是虚拟障碍物类型VirtualObjectType
std::unordered_set<std::string> DecisionData::GetObstacleIdByType(
    const VirtualObjectType& type) {
  std::lock_guard<std::mutex> lock(mutex_);
  //实际上就是去决策数据类DecisionData的数据成员virtual_obstacle_id_map_里根据
  //key关键字type,去查找相应的value值也就是障碍物id，返回的是一个id的集合
  //这个unordered_map里存放的是 虚拟障碍物类型-id(key-value)键值对，就是多个映射关系
  return common::util::FindWithDefault(virtual_obstacle_id_map_, type, {});
}

//获取静态障碍物函数，返回是一个Obstacle的vector就是障碍物列表
//直接返回DecisionData决策数据类的数据成员static_obstacle_
//在带参构造函数里已经对该成员赋值过了
const std::vector<Obstacle*>& DecisionData::GetStaticObstacle() const {
  return static_obstacle_;
}

//获取动态障碍物函数，返回是一个Obstacle的vector就是障碍物列表
//直接返回DecisionData决策数据类的数据成员dynamic_obstacle_
//在带参构造函数里已经对该成员赋值过了
const std::vector<Obstacle*>& DecisionData::GetDynamicObstacle() const {
  return dynamic_obstacle_;
}

//获取虚拟障碍物函数，返回是一个Obstacle的vector就是障碍物列表
//直接返回DecisionData决策数据类的数据成员virtual_obstacle_
//会通过下方CreateVirtualObstacle函数里对该成员赋值
const std::vector<Obstacle*>& DecisionData::GetVirtualObstacle() const {
  return virtual_obstacle_;
}

//获取实际障碍物函数(相对虚拟障碍物而言)，返回是一个Obstacle的vector就是障碍物列表
//直接返回DecisionData决策数据类的数据成员practical_obstacle_
//在带参构造函数里已经对该成员赋值过了
const std::vector<Obstacle*>& DecisionData::GetPracticalObstacle() const {
  return practical_obstacle_;
}

//获取全部障碍物函数，返回是一个Obstacle的vector就是障碍物列表
//直接返回DecisionData决策数据类的数据成员all_obstacle_
//在带参构造函数里已经对该成员赋值过了(实际障碍物)，同样会通过下方
//CreateVirtualObstacle函数里对该成员赋值(虚拟障碍物)
const std::vector<Obstacle*>& DecisionData::GetAllObstacle() const {
  return all_obstacle_;
}

//创建虚拟障碍物函数
//参数 ReferencePoint参考点，可以获取其x,y,s,heading,v,kappa,dkappa等信息
		//这个point是虚拟障碍物的位置？
//参数 虚拟障碍物类型type
//参数 障碍物id
bool DecisionData::CreateVirtualObstacle(const ReferencePoint& point,
                                         const VirtualObjectType& type,
                                         std::string* const id) {
  //不同的类型应当建立不同的盒子box
  common::SLPoint sl_point; //定义了一个sl点(s,l),frenet系下的SL坐标
  //将参数参考点转换到frenet系下SL坐标，如果失败则报错返回，转化结果存放到sl_point
  if (!reference_line_.XYToSL(point, &sl_point)) {
    return false;
  }
  //定义障碍物的s坐标obstacle_s为sl_point的s
  double obstacle_s = sl_point.s();
  //则障碍物盒子的中心的s坐标box_center_s = 障碍物s坐标 + 虚拟障碍物停止墙长度的一半
  //虚拟障碍物停止墙FLAGS_virtual_stop_wall_length，google gflags用法
  //FLAGS_表明去相应的XXX_gflags.cc下面取出变量virtual_stop_wall_length的值
  //modules\planning\common\planning_gflags.cc下取出virtual_stop_wall_length值为
  //0.1m
  const double box_center_s = obstacle_s + FLAGS_virtual_stop_wall_length / 2.0;
  //然后中心点就直接通过参考线去查相应的s坐标对应的pathpoint路径点
  auto box_center = reference_line_.GetReferencePoint(box_center_s);
  //障碍物的heading直接用障碍物的s坐标去参考线上查对应的参考点的heading
  double heading = reference_line_.GetReferencePoint(obstacle_s).heading();
  //初始化定义车道左边宽度，车道右边宽度为0.0
  double lane_left_width = 0.0;
  double lane_right_width = 0.0;
  //用数据成员reference_line_参考线上通过障碍物的frenet系s坐标去查询那一点对应的车道
  //障碍物点到车道左边缘的宽度，以及障碍物点到车道右边缘的宽度，放入引用变量
  //lane_left_width,lane_right_width中
  reference_line_.GetLaneWidth(obstacle_s, &lane_left_width, &lane_right_width);
  //建立一个二维盒子类对象box，参数为盒子中心box_center，障碍物点的heading，虚拟墙的
  //长度，车道的宽度
  common::math::Box2d box(box_center, heading, FLAGS_virtual_stop_wall_length,
                          lane_left_width + lane_right_width);
  //再通过二维box,虚拟障碍物类型，障碍物id去创建虚拟障碍物
  //调用了一个另外一个重载的创建虚拟障碍物函数，用上述三个参数去创建
  return CreateVirtualObstacle(box, type, id);
}

//重载创建虚拟障碍物函数，参数是障碍物frenet系s坐标，虚拟障碍物类型，以及障碍物id
//过程于与上方类似，最后也是调用CreateVirtualObstacle(box, type, id);
//通过box，虚拟障碍物类型，障碍物id来创建虚拟障碍物
bool DecisionData::CreateVirtualObstacle(const double point_s,
                                         const VirtualObjectType& type,
                                         std::string* const id) {
  // should build different box by type;
  const double box_center_s = point_s + FLAGS_virtual_stop_wall_length / 2.0;
  auto box_center = reference_line_.GetReferencePoint(box_center_s);
  double heading = reference_line_.GetReferencePoint(point_s).heading();
  double lane_left_width = 0.0;
  double lane_right_width = 0.0;
  reference_line_.GetLaneWidth(point_s, &lane_left_width, &lane_right_width);
  common::math::Box2d box(box_center, heading, FLAGS_virtual_stop_wall_length,
                          lane_left_width + lane_right_width);
  return CreateVirtualObstacle(box, type, id);
}

//创建虚拟障碍物
//CreateVirtualObstacle(box, type, id);
//通过二维障碍物盒box，虚拟障碍物类型，障碍物id来创建虚拟障碍物
//最后将障碍物对象塞入DecisionData数据决策类的成员obstacles_，virtual_obstacle_等
//障碍物列表中
bool DecisionData::CreateVirtualObstacle(
    const common::math::Box2d& obstacle_box, const VirtualObjectType& type,
    std::string* const id) {
  std::lock_guard<std::mutex> transaction_lock(transaction_mutex_);
  std::lock_guard<std::mutex> lock(mutex_);

  perception::PerceptionObstacle perception_obstacle;
  // TODO(All) please chagne is_virtual in obstacle
  perception_obstacle.set_id(virtual_obstacle_.size() + 1);
  perception_obstacle.mutable_position()->set_x(obstacle_box.center().x());
  perception_obstacle.mutable_position()->set_y(obstacle_box.center().y());
  perception_obstacle.set_theta(obstacle_box.heading());
  perception_obstacle.mutable_velocity()->set_x(0);
  perception_obstacle.mutable_velocity()->set_y(0);
  perception_obstacle.set_length(obstacle_box.length());
  perception_obstacle.set_width(obstacle_box.width());
  perception_obstacle.set_height(FLAGS_virtual_stop_wall_height);
  perception_obstacle.set_type(
      perception::PerceptionObstacle::UNKNOWN_UNMOVABLE);
  perception_obstacle.set_tracking_time(1.0);

  std::vector<common::math::Vec2d> corner_points;
  obstacle_box.GetAllCorners(&corner_points);
  for (const auto& corner_point : corner_points) {
    auto* point = perception_obstacle.add_polygon_point();
    point->set_x(corner_point.x());
    point->set_y(corner_point.y());
  }
  *id = std::to_string(perception_obstacle.id());
  obstacles_.emplace_back(new Obstacle(*id, perception_obstacle));
  all_obstacle_.emplace_back(obstacles_.back().get());
  virtual_obstacle_.emplace_back(obstacles_.back().get());
  // would be changed if some virtual type is not static one
  static_obstacle_.emplace_back(obstacles_.back().get());

  virtual_obstacle_id_map_[type].insert(*id);
  obstacle_map_[*id] = obstacles_.back().get();
  return true;
}

}  // namespace planning
}  // namespace apollo