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

概述

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

从类名来看，应该是障碍物类，将一个障碍物的所有相关信息封装成这个类。

注：
/nudge在apollo里代表横向上轻轻绕一下避开？
sl_boundary_理解为障碍物的SL边界，其实就是边界盒坐标转化为SL坐标形式？
ObjectDecisionType modules\planning\proto\decision.proto中定义的message类，针对该障碍物对象决策：停车/绕行/减速避让等

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

obstacle.h

#pragma once

#include 
#include 
#include 
#include 
#include 

#include "modules/common/configs/proto/vehicle_config.pb.h"
#include "modules/common/math/box2d.h"
#include "modules/common/math/vec2d.h"
#include "modules/perception/proto/perception_obstacle.pb.h"
#include "modules/planning/common/indexed_list.h"
#include "modules/planning/common/speed/st_boundary.h"
#include "modules/planning/proto/decision.pb.h"
#include "modules/planning/proto/sl_boundary.pb.h"
#include "modules/planning/reference_line/reference_line.h"
#include "modules/prediction/proto/prediction_obstacle.pb.h"

namespace apollo {
namespace planning {

/**
 * @class Obstacle
 * @brief 这是一个连接障碍物路径及其所在path属性的类
 * 一个障碍物的路径属性包括frenet系下s,l的值
 * 障碍物的决策也属于一个路径path的属性
 * 决策分为两种：横向决策和纵向决策
 * 横向决策包括：忽略ignore/轻微绕行nudge
 * 横向决策安全优先级:nudge > ignore
 * 纵向决策包括：停止stop/减速避让yield/跟车follow/超车overtake/忽略ignore
 * 纵向决策的安全优先级：stop > yield >= follow > overtake > ignore
 * ignore决策同时属于纵向和横向决策，其优先级最低
 */
class Obstacle {
 public:
  Obstacle() = default;
  //障碍物类带参构造函数，输入参数id,感知障碍物类对象，障碍物优先级，是否为静态障碍物去构造一个障碍物实例对象
  Obstacle(const std::string& id,
           const perception::PerceptionObstacle& perception_obstacle,
           const prediction::ObstaclePriority::Priority& obstacle_priority,
           const bool is_static);
           
  //Obstacle类的带参构造函数重载，输入参数不同
//输入参数跟上面差不多，就多了一个预测轨迹类对象trajectory         
  Obstacle(const std::string& id,
           const perception::PerceptionObstacle& perception_obstacle,
           const prediction::Trajectory& trajectory,
           const prediction::ObstaclePriority::Priority& obstacle_priority,
           const bool is_static);

//设置/获取障碍物id_，也就是其类成员
  const std::string& Id() const { return id_; }
  void SetId(const std::string& id) { id_ = id; }

//获取障碍物的速度speed_，也就是其数据成员
  double speed() const { return speed_; }

//获取数据成员感知障碍物id perception_id_
  int32_t PerceptionId() const { return perception_id_; }

//返回障碍物是否为静止的标志位，类数据成员is_static_
  bool IsStatic() const { return is_static_; }
 //返回障碍物是否为虚拟的标志位，类数据成员is_virtual_ 
  bool IsVirtual() const { return is_virtual_; }

//根据时间查询预测轨迹点
//输入参数是相对时间relative_time
  common::TrajectoryPoint GetPointAtTime(const double time) const;

//获取障碍物的2维边界盒，输入参数是预测轨迹点？其实就是轨迹点作为几何中心点其x,y,theta信息加上数据成员感知障碍物的长宽构建二维边界盒
  common::math::Box2d GetBoundingBox(
      const common::TrajectoryPoint& point) const;

//返回感知障碍物的边界盒，类数据成员perception_bounding_box_
  const common::math::Box2d& PerceptionBoundingBox() const {
    return perception_bounding_box_;
  }

//返回感知障碍物的多边形，类的数据成员perception_polygon_
  const common::math::Polygon2d& PerceptionPolygon() const {
    return perception_polygon_;
  }

//获取障碍物的预测轨迹，返回类的数据成员trajectory_
  const prediction::Trajectory& Trajectory() const { return trajectory_; }
  common::TrajectoryPoint* AddTrajectoryPoint() {
    return trajectory_.add_trajectory_point();
  }
  //返回障碍物是否有轨迹？
  bool HasTrajectory() const {
    return !(trajectory_.trajectory_point().empty());
  }

//返回感知障碍物对象，属于Obstacle类成员
  const perception::PerceptionObstacle& Perception() const {
    return perception_obstacle_;
  }

  /**
   * @brief 这是一个辅助函数可以创建障碍物来自预测数据
   * 预测原始对于每个障碍物可能有多个轨迹？
   * 这个函数会针对每个轨迹创建一个障碍物？
   * @param predictions 预测结果
   * @return obstacles 障碍物对象列表
   */
//创建障碍物类对象指针列表，输入参数是预测障碍物列表
//其实就是从预测障碍物对象中提取出有效的障碍物和有效的轨迹等信息，塞入障碍物类对象指针列表返回
  static std::list<std::unique_ptr<Obstacle>> CreateObstacles(
      const prediction::PredictionObstacles& predictions);

//创建虚拟障碍物函数
//输入参数id，二维障碍物边界盒
  static std::unique_ptr<Obstacle> CreateStaticVirtualObstacles(
      const std::string& id, const common::math::Box2d& obstacle_box);

//判断是否为有效的感知障碍物，输入参数是感知障碍物类对象
  static bool IsValidPerceptionObstacle(
      const perception::PerceptionObstacle& obstacle);

//判断是否为有效的轨迹点，输入参数是一个轨迹点类型对象
//只要轨迹点没有路径点 或 x/y/z/kappa/s/dkappa/ddkappa/v/a/relative_time里只要有
//一个为nan(not a number)就说明这个轨迹点是无效的，否则有效
  static bool IsValidTrajectoryPoint(const common::TrajectoryPoint& point);

//内联函数，返回障碍物是否为注意等级的障碍物？
  inline bool IsCautionLevelObstacle() const {
    return is_caution_level_obstacle_;
  }

  // const Obstacle* obstacle() const;

  /**
   * return 融合的横向决策
   * Lateral decision is one of {Nudge, Ignore}
   **/
//返回Obstacle类数据成员，针对这个障碍物的横向决策
  const ObjectDecisionType& LateralDecision() const;

  /**
   * @brief return 融合的纵向决策
   * Longitudinal decision is one of {Stop, Yield, Follow, Overtake, Ignore}
   **/
  const ObjectDecisionType& LongitudinalDecision() const;

//返回debug字符串
  std::string DebugString() const;

//获取该感知障碍物的sl边界盒？是感知障碍物边界在参考线上的投影？
  const SLBoundary& PerceptionSLBoundary() const;

//获取数据成员 参考线ST边界
  const STBoundary& reference_line_st_boundary() const;

//获取数据成员 路径ST边界，是指该障碍物所占据的的路径边界？
  const STBoundary& path_st_boundary() const;

//返回数据成员，决策标签？一个字符串的vector,decider_tags_
  const std::vector<std::string>& decider_tags() const;

//返回目标决策类型列表？
  const std::vector<ObjectDecisionType>& decisions() const;

//针对该障碍物增加一个纵向决策，输入参数决策标签字符串decider_tag
//以及目标决策类型对象decision
//其实就是将输入的目标纵向决策类型对象与之前类里储存的进行融合。
//类的数据成员决策列表decisions_和决策标签列表decider_tags_里再增加一个
  void AddLongitudinalDecision(const std::string& decider_tag,
                               const ObjectDecisionType& decision);

//针对该障碍物增加一个横向决策，输入参数决策标签字符串decider_tag
//以及目标决策类型对象decision
//其实就是将输入的目标横向决策类型对象与之前类里储存的进行融合。
//类的数据成员决策列表decisions_和决策标签列表decider_tags_里再增加一个
  void AddLateralDecision(const std::string& decider_tag,
                          const ObjectDecisionType& decision);
  
//检查针对该障碍物是否有横向决策？
  bool HasLateralDecision() const;

//设定路径的ST边界，也就是针对该障碍物的ST边界？
  void set_path_st_boundary(const STBoundary& boundary);

//返回数据成员 路径ST边界被初始化？
  bool is_path_st_boundary_initialized() {
    return path_st_boundary_initialized_;
  }

//设置ST边界类型？
  void SetStBoundaryType(const STBoundary::BoundaryType type);

//擦除类里储存的路径ST边界
  void EraseStBoundary();

//设定道路参考线的ST边界
  void SetReferenceLineStBoundary(const STBoundary& boundary);

//设定道路参考线ST边界类型
  void SetReferenceLineStBoundaryType(const STBoundary::BoundaryType type);

//擦除类里储存的道路参考线ST边界
  void EraseReferenceLineStBoundary();

//检查针对该障碍物是否有纵向决策？
  bool HasLongitudinalDecision() const;

//针对该障碍物的决策是否不可忽略
  bool HasNonIgnoreDecision() const;

  /**
   * @brief
   * 用自车的最小转弯半径去计算到该障碍物的停止距离
   */
  double MinRadiusStopDistance(const common::VehicleParam& vehicle_param) const;

  /**
   * @brief
   * 检查该障碍物是否可以被安全的忽略
   * 目标可以被忽略仅当纵向和横向决策都是可以忽略的时候
   */
  bool IsIgnore() const;
  bool IsLongitudinalIgnore() const;
  bool IsLateralIgnore() const;

//针对障碍物对象建立自车参考线的ST边界，输入参数是道路参考线类对象，以及adc自车的起始s坐标，将障碍物投影到自车的ST图上
  void BuildReferenceLineStBoundary(const ReferenceLine& reference_line,
                                    const double adc_start_s);

//设置障碍物的SL边界也就是Frenet系的横纵坐标的边界类对象sl_boundary
//输入参数就是SLBoundary SL边界类对象
//就是将参数拷贝给类数据成员sl_boundary_ 其实就是障碍物边界点相对于道路参考线的SL坐标
  void SetPerceptionSlBoundary(const SLBoundary& sl_boundary);

  /**
   * @brief 
   * 是否为纵向决策，有ignore，有stop/yield/follow/overtake都是
   **/
  static bool IsLongitudinalDecision(const ObjectDecisionType& decision);

//nudge在apollo里代表横向上轻轻绕一下避开？
//判断目标决策类型对象是横向决策？如果有ignore或nudge就认为是
  static bool IsLateralDecision(const ObjectDecisionType& decision);

//设置该障碍物为阻塞障碍物？
  void SetBlockingObstacle(bool blocking) { is_blocking_obstacle_ = blocking; }

//返回该障碍物是否为阻塞自车的障碍物？
  bool IsBlockingObstacle() const { return is_blocking_obstacle_; }

  /*
   * @brief IsLaneBlocking is only meaningful when IsStatic() == true.
   * 只有当障碍物为静态时，判断车道是否被阻塞
   */
  bool IsLaneBlocking() const { return is_lane_blocking_; }
  void CheckLaneBlocking(const ReferenceLine& reference_line);
  bool IsLaneChangeBlocking() const { return is_lane_change_blocking_; }
  void SetLaneChangeBlocking(const bool is_distance_clear);

 private:
  FRIEND_TEST(MergeLongitudinalDecision, AllDecisions);
  static ObjectDecisionType MergeLongitudinalDecision(
      const ObjectDecisionType& lhs, const ObjectDecisionType& rhs);
  FRIEND_TEST(MergeLateralDecision, AllDecisions);
  static ObjectDecisionType MergeLateralDecision(const ObjectDecisionType& lhs,
                                                 const ObjectDecisionType& rhs);

//建立轨迹的ST边界
//输入参数 参考线类对象，自车起始的frenet系纵坐标s，第三个参数用以存放得到的结果
//这个函数的实现代码没太看明白，大致就是根据参考线，自车的纵向坐标s，用动态感知障碍物对象及其预测轨迹去修改之前的自车参考线ST边界，起始就是用该障碍物信息去修正ST图中搜索的可行域？将动态障碍物投影到自车的ST图上。
  bool BuildTrajectoryStBoundary(const ReferenceLine& reference_line,
                                 const double adc_start_s,
                                 STBoundary* const st_boundary);
  
  //是否为有效的障碍物，主要是看感知障碍物的长宽是否为nan(not a number)或者过于小了，是的话就说明是无效的障碍物
  bool IsValidObstacle(
      const perception::PerceptionObstacle& perception_obstacle);

 private:
  std::string id_;
  int32_t perception_id_ = 0;
  bool is_static_ = false;
  bool is_virtual_ = false;
  double speed_ = 0.0;

  bool path_st_boundary_initialized_ = false;

  prediction::Trajectory trajectory_;
  perception::PerceptionObstacle perception_obstacle_;
  common::math::Box2d perception_bounding_box_;
  common::math::Polygon2d perception_polygon_;

  std::vector<ObjectDecisionType> decisions_;
  std::vector<std::string> decider_tags_;
  SLBoundary sl_boundary_; //其实就是障碍物边界点相对于道路参考线的SL坐标

  STBoundary reference_line_st_boundary_;
  STBoundary path_st_boundary_;

  ObjectDecisionType lateral_decision_;
  ObjectDecisionType longitudinal_decision_;

  // for keep_clear usage only
  bool is_blocking_obstacle_ = false;

  bool is_lane_blocking_ = false;

  bool is_lane_change_blocking_ = false;

  bool is_caution_level_obstacle_ = false;

  double min_radius_stop_distance_ = -1.0;

  struct ObjectTagCaseHash {
    size_t operator()(
        const planning::ObjectDecisionType::ObjectTagCase tag) const {
      return static_cast<size_t>(tag);
    }
  };

  static const std::unordered_map<ObjectDecisionType::ObjectTagCase, int,
                                  ObjectTagCaseHash>
      s_lateral_decision_safety_sorter_;
  static const std::unordered_map<ObjectDecisionType::ObjectTagCase, int,
                                  ObjectTagCaseHash>
      s_longitudinal_decision_safety_sorter_;
};

typedef IndexedList<std::string, Obstacle> IndexedObstacles;
typedef ThreadSafeIndexedList<std::string, Obstacle> ThreadSafeIndexedObstacles;

}  // namespace planning
}  // namespace apollo

obstacle.cc

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

#include 
#include 

#include "cyber/common/log.h"
#include "modules/common/configs/vehicle_config_helper.h"
#include "modules/common/math/linear_interpolation.h"
#include "modules/common/util/map_util.h"
#include "modules/common/util/util.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/common/speed/st_boundary.h"

namespace apollo {
namespace planning {

using apollo::common::VehicleConfigHelper;
using apollo::common::util::FindOrDie;
using apollo::perception::PerceptionObstacle;
using apollo::prediction::ObstaclePriority;

namespace {  //又定义了个namespace防止其他程序引用该类时出现重名重复定义现象。
//定义了3个常量 
//ST边界s的差值阈值 0.2m?
const double kStBoundaryDeltaS = 0.2;        // meters
//ST边界的逃离s的差值 1.0m?
const double kStBoundarySparseDeltaS = 1.0;  // meters
//ST边界的时间差值阈值0.05s
const double kStBoundaryDeltaT = 0.05;       // seconds
}  // namespace

//定义了一个无序map s_longitudinal_decision_safety_sorter_，存放目标场景标签和纵向距离的映射？
//这个map的名字字面意义来看，纵向决策安全分类？
const std::unordered_map<ObjectDecisionType::ObjectTagCase, int,
                         Obstacle::ObjectTagCaseHash>
    Obstacle::s_longitudinal_decision_safety_sorter_ = {
        {ObjectDecisionType::kIgnore, 0}, //忽略？
        {ObjectDecisionType::kOvertake, 100}, //超车
        {ObjectDecisionType::kFollow, 300}, //跟车
        {ObjectDecisionType::kYield, 400}, //避让
        {ObjectDecisionType::kStop, 500}}; //停车

//定义了一个无序map s_lateral_decision_safety_sorter_，存放目标场景标签和横向距离的映射？
//这个map的名字字面意义来看，横向决策安全分类？
const std::unordered_map<ObjectDecisionType::ObjectTagCase, int,
                         Obstacle::ObjectTagCaseHash>
    Obstacle::s_lateral_decision_safety_sorter_ = {
     //横向决策忽略，Nudge是靠边避让？
        {ObjectDecisionType::kIgnore, 0}, {ObjectDecisionType::kNudge, 100}};

//Obstacle障碍物类的带参构造函数
//输入参数：障碍物id，感知障碍物类对象，障碍物优先级类对象，是否为静态障碍物？
Obstacle::Obstacle(const std::string& id,
                   const PerceptionObstacle& perception_obstacle,
                   const ObstaclePriority::Priority& obstacle_priority,
                   const bool is_static)
    //用这些输入参数去初始化Obstacle类自己的数据成员，赋值数据成员id_
    : id_(id), 
    //赋值数据成员感知障碍物perception_id_
      perception_id_(perception_obstacle.id()),
    //赋值数据成员感知障碍物对象perception_obstacle_
      perception_obstacle_(perception_obstacle),
    //赋值数据成员感知障碍物的边界盒perception_bounding_box_
      perception_bounding_box_({perception_obstacle_.position().x(),
                                perception_obstacle_.position().y()},
                               perception_obstacle_.theta(),
                               perception_obstacle_.length(),
                               perception_obstacle_.width()) {
  //将这个障碍物的注意等级先初始设定为CAUTION注意
  is_caution_level_obstacle_ = (obstacle_priority == ObstaclePriority::CAUTION);
  //定义一个2维多边形点vector
  std::vector<common::math::Vec2d> polygon_points;
  //FLAGS_use_navigation_mode是GFLAGS库的用法，FLAGS_代表去相应的XXX.gflags.cc里取
  //出use_navigation_mode的值，默认为false，或者多边形点的数量小于等于2
  //如果用导航模式或感知障碍物的边界多边形点的数目小于等于2，就获取感知障碍物边界盒的所有顶
  //点，顶点就是根据感知的长宽，几何中心，heading角计算而来
  if (FLAGS_use_navigation_mode ||
      perception_obstacle.polygon_point_size() <= 2) {
    perception_bounding_box_.GetAllCorners(&polygon_points);
  } else {
    //既不使用导航模式，感知障碍物的边界多边形点数目又大于2的话
    //把感知障碍物的多边形点放入vector polygon_points
    ACHECK(perception_obstacle.polygon_point_size() > 2)
        << "object " << id << "has less than 3 polygon points";
    for (const auto& point : perception_obstacle.polygon_point()) {
      polygon_points.emplace_back(point.x(), point.y());
    }
  }
  //计算感知障碍物多边形点集的凸包，得到的多边形结果存放在引用变量perception_polygon_感知多边形里
  ACHECK(common::math::Polygon2d::ComputeConvexHull(polygon_points,
                                                    &perception_polygon_))
      << "object[" << id << "] polygon is not a valid convex hull.\n"
      << perception_obstacle.DebugString();

//判断是否为静态 = 是否为静态 或 障碍物优先级为可忽略，就是障碍物可忽略的话也可直接视为静态障碍物
  is_static_ = (is_static || obstacle_priority == ObstaclePriority::IGNORE);
  //是否为虚拟障碍物，若感知障碍物id<0的话则为虚拟障碍物
  is_virtual_ = (perception_obstacle.id() < 0);
  //std::hypot 计算 x 和 y 的平方和的平方根,其实就是计算下感知障碍物速度X方向，Y方向
  //的矢量和
  speed_ = std::hypot(perception_obstacle.velocity().x(),
                      perception_obstacle.velocity().y());
}

//Obstacle类的带参构造函数重载，输入参数不同
//输入参数跟上面差不多，就多了一个预测轨迹类对象trajectory
Obstacle::Obstacle(const std::string& id,
                   const PerceptionObstacle& perception_obstacle,
                   const prediction::Trajectory& trajectory,
                   const ObstaclePriority::Priority& obstacle_priority,
                   const bool is_static)
       //输入参数除预测轨迹，其他参数直接调用上面的构造函数初始化数据成员
    : Obstacle(id, perception_obstacle, obstacle_priority, is_static) {
  //将输入参数预测轨迹trajectory赋值给数据成员trajectory_
  trajectory_ = trajectory;
  //取出预测轨迹的轨迹点trajectory_points
  auto& trajectory_points = *trajectory_.mutable_trajectory_point();
  //初始定义了一个累计纵向距离s cumulative_s
  double cumulative_s = 0.0;
  //如果轨迹点的数目大于0 轨迹点的第一个点的s设置为0，其实就
  //相对第一个轨迹点的相对纵坐标s
  if (trajectory_points.size() > 0) {
    trajectory_points[0].mutable_path_point()->set_s(0.0);
  }
  //从预测轨迹点的第二个点开始遍历设置相对纵坐标s，第一个点上面已经设置了
  for (int i = 1; i < trajectory_points.size(); ++i) {
    //前继点，第i-1个预测轨迹点prev
    const auto& prev = trajectory_points[i - 1];
    //当前点，第i个预测轨迹点cur
    const auto& cur = trajectory_points[i];
    //如果前继点的相对时间 >= 当前点相对时间，报错
    if (prev.relative_time() >= cur.relative_time()) {
      AERROR << "prediction time is not increasing."
             << "current point: " << cur.ShortDebugString()
             << "previous point: " << prev.ShortDebugString();
    }
    //累计的纵向位置s=上次累计的纵向位置s + 第i-1个点到第i个点的距离
    cumulative_s +=
        common::util::DistanceXY(prev.path_point(), cur.path_point());
    //设定第i个点的相对起点的纵坐标s
    trajectory_points[i].mutable_path_point()->set_s(cumulative_s);
  }
}

//根据时间查询预测轨迹点
//输入参数是相对时间relative_time
common::TrajectoryPoint Obstacle::GetPointAtTime(
    const double relative_time) const {
  //首先去数据成员预测轨迹trajectory_上取出所有的预测轨迹点
  const auto& points = trajectory_.trajectory_point();
  //如果预测轨迹点个数小于2，那么轨迹点point的s,t,v,a直接设置为0，x,y,z,theta直接设置为当前值返回这个轨迹点
  if (points.size() < 2) {
    common::TrajectoryPoint point;
    point.mutable_path_point()->set_x(perception_obstacle_.position().x());
    point.mutable_path_point()->set_y(perception_obstacle_.position().y());
    point.mutable_path_point()->set_z(perception_obstacle_.position().z());
    point.mutable_path_point()->set_theta(perception_obstacle_.theta());
    point.mutable_path_point()->set_s(0.0);
    point.mutable_path_point()->set_kappa(0.0);
    point.mutable_path_point()->set_dkappa(0.0);
    point.mutable_path_point()->set_ddkappa(0.0);
    point.set_v(0.0);
    point.set_a(0.0);
    point.set_relative_time(0.0);
    return point;
  } else {
  //如果该障碍物的预测轨迹点数目 >= 2
  //这是函数内部又定义了个比较函数comp，比较函数的参数有轨迹点p，和时间time
  //若p的相对时间小于time就返回p
    auto comp = [](const common::TrajectoryPoint p, const double time) {
      return p.relative_time() < time;
    };

   //std::lower_bound的用法就是遍历预测轨迹点，lower_bound代表反向遍历，找到第一个小于给定时间的轨迹点
   //那正向遍历和反向遍历结果就不一样，正向遍历upper_bound就是找到轨迹点中的第一个点了，反向遍历lower_bound就是找到轨迹点中相对时间刚好小于给定时间的点，下一个点相对时间就要大于给定时间了
   //那么预测轨迹点里相对时间刚好小于输入参数给定相对时间relative_time的轨迹点复制给it_lower
    auto it_lower =
        std::lower_bound(points.begin(), points.end(), relative_time, comp);

//如果it_lower是预测轨迹里的第一个点/最后一个点就直接返回之
    if (it_lower == points.begin()) {
      return *points.begin();
    } else if (it_lower == points.end()) {
      return *points.rbegin();
    }
    //如果it_lower不是预测轨迹里的第一个点/最后一个点就用it_lower和其上一个点线性插值得到relative_time对应的轨迹点信息并返回
    return common::math::InterpolateUsingLinearApproximation(
        *(it_lower - 1), *it_lower, relative_time);
  }
}

//获取障碍物的2维边界盒，输入参数是预测轨迹点？其实就是轨迹点作为几何中心点其x,y,theta信息加上数据成员感知障碍物的长宽构建二维边界盒
common::math::Box2d Obstacle::GetBoundingBox(
    const common::TrajectoryPoint& point) const {
  return common::math::Box2d({point.path_point().x(), point.path_point().y()},
                             point.path_point().theta(),
                             perception_obstacle_.length(),
                             perception_obstacle_.width());
}

//判断是否为有效的感知障碍物，输入参数是感知障碍物类对象
bool Obstacle::IsValidPerceptionObstacle(const PerceptionObstacle& obstacle) {
  //如果感知障碍物的长宽高<=0自然是无效的感知障碍物，直接报错返回
  if (obstacle.length() <= 0.0) {
    AERROR << "invalid obstacle length:" << obstacle.length();
    return false;
  }
  if (obstacle.width() <= 0.0) {
    AERROR << "invalid obstacle width:" << obstacle.width();
    return false;
  }
  if (obstacle.height() <= 0.0) {
    AERROR << "invalid obstacle height:" << obstacle.height();
    return false;
  }
  //如果感知障碍物有速度
  if (obstacle.has_velocity()) {
  //如果感知障碍物的X方向速度 或 Y方向速度只要有一个为nan(not a number)则直接返回false
    if (std::isnan(obstacle.velocity().x()) ||
        std::isnan(obstacle.velocity().y())) {
      AERROR << "invalid obstacle velocity:"
             << obstacle.velocity().DebugString();
      return false;
    }
  }
  //上面都不满足没有返回，那么遍历感知障碍物的多边形点，只要有一个点的x或y坐标为nan(not a number)就直接返回false
  for (auto pt : obstacle.polygon_point()) {
    if (std::isnan(pt.x()) || std::isnan(pt.y())) {
      AERROR << "invalid obstacle polygon point:" << pt.DebugString();
      return false;
    }
  }
  //上面情况都不满足，说明感知障碍物是个有效的障碍物
  return true;
}

//创建障碍物类对象指针列表，输入参数是预测障碍物列表
//其实就是从预测障碍物对象中提取出有效的障碍物和有效的轨迹等信息，塞入障碍物类对下个指针列表返回
std::list<std::unique_ptr<Obstacle>> Obstacle::CreateObstacles(
    const prediction::PredictionObstacles& predictions) {
  //先定义一个空的障碍物类指针列表准备用来存放结果返回
  std::list<std::unique_ptr<Obstacle>> obstacles;
  //遍历预测障碍物列表
  for (const auto& prediction_obstacle : predictions.prediction_obstacle()) {
    //判断预测障碍物的成员感知障碍物是否是有效的感知障碍物，否则报错并跳到下一个障碍物
    if (!IsValidPerceptionObstacle(prediction_obstacle.perception_obstacle())) {
      AERROR << "Invalid perception obstacle: "
             << prediction_obstacle.perception_obstacle().DebugString();
      continue;
    }
    //将第i个被遍历的预测障碍物的id，轨迹，优先级，是否为静态障碍物作为参数调用Obstacle类的构造函数，获得的对象塞到障碍物类对象指针列表obstacles
    const auto perception_id =
        std::to_string(prediction_obstacle.perception_obstacle().id());
    if (prediction_obstacle.trajectory().empty()) {
      obstacles.emplace_back(
          new Obstacle(perception_id, prediction_obstacle.perception_obstacle(),
                       prediction_obstacle.priority().priority(),
                       prediction_obstacle.is_static()));
      continue;
    }

    //遍历第i个预测障碍物的预测轨迹
    int trajectory_index = 0;
    for (const auto& trajectory : prediction_obstacle.trajectory()) {
      bool is_valid_trajectory = true;
      //遍历预测轨迹点
      for (const auto& point : trajectory.trajectory_point()) {
        //若第i个障碍物的预测轨迹点上有一个无效点，就设置is_valid_trajectory为无效轨迹，并且直接跳出针对第i个预测障碍物预测轨迹点的遍历循环
        if (!IsValidTrajectoryPoint(point)) {
          AERROR << "obj:" << perception_id
                 << " TrajectoryPoint: " << trajectory.ShortDebugString()
                 << " is NOT valid.";
          is_valid_trajectory = false;
          break;
        }
      }
      //若第i个预测障碍物的轨迹不是有效的轨迹，则直接跳到下一个障碍物
      if (!is_valid_trajectory) {
        continue;
      }

      //若执行到这里说明第i个障碍物有效，且其预测轨迹是有效的
      //那么就将其id,优先级，预测轨迹，是否为静态障碍物的信息截出来拿去调用Obstacle类的构造函数，得到的障碍物对象塞入障碍物对象列表obstacles待返回
      const std::string obstacle_id =
          absl::StrCat(perception_id, "_", trajectory_index);
      obstacles.emplace_back(
          new Obstacle(obstacle_id, prediction_obstacle.perception_obstacle(),
                       trajectory, prediction_obstacle.priority().priority(),
                       prediction_obstacle.is_static()));
      ++trajectory_index;
    }
  }
  //返回障碍物列表
  return obstacles;
}

//创建虚拟障碍物函数
//输入参数id，二维障碍物边界盒
std::unique_ptr<Obstacle> Obstacle::CreateStaticVirtualObstacles(
    const std::string& id, const common::math::Box2d& obstacle_box) {
  //创建一个虚拟的感知障碍物对象
  perception::PerceptionObstacle perception_obstacle;
  //id需要一个整数，虚拟障碍物是个负id
  size_t negative_id = std::hash<std::string>{}(id);
  // set the first bit to 1 so negative_id became negative number
  //size_t是32位？这里将1左移31位，将32位最高位置1代表负数？然后虚拟障碍物id与之按位或
  negative_id |= (0x1 << 31);
  //感知障碍物设置id，几何中心x,y,heading,vx=0,vy=0,长宽高，类型为未知不可移动
  perception_obstacle.set_id(static_cast<int32_t>(negative_id));
  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);
  //设置感知障碍物的跟踪时间1.0s
  perception_obstacle.set_tracking_time(1.0);

  //定义顶点坐标vector
  std::vector<common::math::Vec2d> corner_points;
  //获取输入参数二维边界盒的所有顶点放入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,感知障碍物对象，障碍物优先级正常，true代表为静态障碍物
  //通过这些信息调用带参构造函数构建一个障碍物对象指针
  auto* obstacle =
      new Obstacle(id, perception_obstacle, ObstaclePriority::NORMAL, true);
  //设置其属性为虚拟障碍物
  obstacle->is_virtual_ = true;
  //将这个障碍物指针对象返回
  return std::unique_ptr<Obstacle>(obstacle);
}

//判断是否为有效的轨迹点，输入参数是一个轨迹点类型对象
//只要轨迹点没有路径点 或 x/y/z/kappa/s/dkappa/ddkappa/v/a/relative_time里只要有
//一个为nan(not a number)就说明这个轨迹点是无效的，否则有效
bool Obstacle::IsValidTrajectoryPoint(const common::TrajectoryPoint& point) {
  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()));
}

//设置障碍物的SL边界也就是Frenet系的横纵坐标的边界类对象sl_boundary
//输入参数就是SLBoundary SL边界类对象
//就是将参数拷贝给类数据成员sl_boundary_ 其实就是障碍物边界点相对于道路参考线的SL坐标
void Obstacle::SetPerceptionSlBoundary(const SLBoundary& sl_boundary) {
  sl_boundary_ = sl_boundary;
}

//使用自车的最小转弯半径计算停车距离？输入参数是车辆参数类对象
//计算的原理大致如博客末尾附图
double Obstacle::MinRadiusStopDistance(
    const common::VehicleParam& vehicle_param) const {
  //如果针对这个障碍物的最小转弯半径的停车距离>0则直接返回？是说明已经设置了吗？
  if (min_radius_stop_distance_ > 0) {
    return min_radius_stop_distance_;
  }
  //定义一个停止距离的缓冲区0.5m
  static constexpr double stop_distance_buffer = 0.5;
  //获取车辆参数的最小转弯半径
  const double min_turn_radius = VehicleConfigHelper::MinSafeTurnRadius();
  //这个是计算出自车到达横向边界上再加上自车宽度一半的横向偏移？
  //sl_boundary_其实就是障碍物边界点相对于道路参考线的SL坐标
  double lateral_diff =
      vehicle_param.width() / 2.0 + std::max(std::fabs(sl_boundary_.start_l()),
                                             std::fabs(sl_boundary_.end_l()));
  //又定义了一个很小的正常数
  const double kEpison = 1e-5;
  lateral_diff = std::min(lateral_diff, min_turn_radius - kEpison);
  //计算的原理大致如博客末尾附图，利用勾股定理计算计算最小转弯半径下的停车距离
  double stop_distance =
      std::sqrt(std::fabs(min_turn_radius * min_turn_radius -
                          (min_turn_radius - lateral_diff) *
                              (min_turn_radius - lateral_diff))) +
      stop_distance_buffer;
  stop_distance -= vehicle_param.front_edge_to_center();
  //对这个停车距离再进行限幅并返回
  stop_distance = std::min(stop_distance, FLAGS_max_stop_distance_obstacle);
  stop_distance = std::max(stop_distance, FLAGS_min_stop_distance_obstacle);
  return stop_distance;
}

//针对障碍物对象建立自车参考线的ST边界，输入参数是道路参考线类对象，以及adc自车的起始s坐标，将障碍物投影到自车的ST图上
//文字不足以描述，详细原理见文末附图
void Obstacle::BuildReferenceLineStBoundary(const ReferenceLine& reference_line,
                                            const double adc_start_s) {
  //获取ADC 自动驾驶车辆的物理参数配置
  const auto& adc_param =
      VehicleConfigHelper::Instance()->GetConfig().vehicle_param();
  //获取车辆物理参数中的宽度
  const double adc_width = adc_param.width();
  //如果障碍物类成员表明其是静态障碍物或障碍物预测轨迹点为空，其实就是个静止障碍物？
  if (is_static_ || trajectory_.trajectory_point().empty()) {
    //定义一个存放STPoint点对的vetor point_pairs
    std::vector<std::pair<STPoint, STPoint>> point_pairs;
    //sl_boundary_其实就是障碍物边界点相对于道路参考线的SL坐标
    //获取障碍物在参考线上对应的起始s坐标，其实就是障碍物的尾部边界点？
    double start_s = sl_boundary_.start_s();
    //获取障碍物在参考线上对应的终点s坐标，其实就是障碍物的头部边界点？
    double end_s = sl_boundary_.end_s();
    //如果终点s-起点s<阈值0.2m，那么终点s=起点s+0.2m
    if (end_s - start_s < kStBoundaryDeltaS) {
      end_s = start_s + kStBoundaryDeltaS;
    }
    //参考线道路被阻塞？输入参数是感知障碍物边界盒，自车的宽度
    //检查是否有边界盒堵塞了路面。标准是检查路面上的剩余空间是否大于自车宽度。
    if (!reference_line.IsBlockRoad(perception_bounding_box_, adc_width)) {
      return;
    }
    //往ST边界里塞ST点对，起点t 0.0
    //S分别塞入(障碍物SL边界起始点s-自车的起始s，0.0) (SL边界终点s-自车的起始s)
    //其实就是这个障碍物的S边界上下限距离自车起始点s的距离
    //相对时间0对应的S边界就是自车到障碍物头部和尾部纵向位置的差值？这个边界其实还是
    //障碍物的头部尾部的相对纵向位置s投影到自车参考线上
    //其实就是把障碍物投影到自车的ST图上？
    //相对时间0也就是当下，直接把现在障碍物头部尾部所处位置直接放入ST图
    point_pairs.emplace_back(STPoint(start_s - adc_start_s, 0.0),
                             STPoint(end_s - adc_start_s, 0.0)); 
    //因为这是一个静态障碍物(在这个大if里说明是静态障碍物) ，障碍物会阻塞参考线
    //s轴上相对自车位置的s坐标8s(最大ST规划时间？)
    //认为这8s该障碍物头部尾部会一直占据start_s到end_s位置          
    point_pairs.emplace_back(STPoint(start_s - adc_start_s, FLAGS_st_max_t),
                             STPoint(end_s - adc_start_s, FLAGS_st_max_t));
    //障碍物类成员参考线ST边界就为这个障碍物相对纵向位置占据8s
    reference_line_st_boundary_ = STBoundary(point_pairs);
  } else { //不是静态障碍物的话，调用建立轨迹ST边界函数，输入参数是道路参考线，自车的起始s，求得的st边界放入reference_line_st_boundary_，起始就是对于这个障碍物对象将其的影响考虑成对参考线ST边界的影响上，其实就是将动态障碍物映射到ST图上？
    if (BuildTrajectoryStBoundary(reference_line, adc_start_s,
                                  &reference_line_st_boundary_)) {
      ADEBUG << "Found st_boundary for obstacle " << id_;
      ADEBUG << "st_boundary: min_t = " << reference_line_st_boundary_.min_t()
             << ", max_t = " << reference_line_st_boundary_.max_t()
             << ", min_s = " << reference_line_st_boundary_.min_s()
             << ", max_s = " << reference_line_st_boundary_.max_s();
    } else {
      ADEBUG << "No st_boundary for obstacle " << id_;
    }
  }
}

//建立轨迹的ST边界
//输入参数 参考线类对象，自车起始的frenet系纵坐标s，第三个参数用以存放得到的结果
//这个函数的实现代码没太看明白，大致就是根据参考线，自车的纵向坐标s，用动态感知障碍物对象及其预测轨迹去修改之前的自车参考线ST边界，起始就是用该障碍物信息去修正ST图中搜索的可行域？将动态障碍物投影到自车的ST图上。
bool Obstacle::BuildTrajectoryStBoundary(const ReferenceLine& reference_line,
                                         const double adc_start_s,
                                         STBoundary* const st_boundary) {
  //如果不是有效的感知障碍物直接返回false
  if (!IsValidObstacle(perception_obstacle_)) {
    AERROR << "Fail to build trajectory st boundary because object is not "
              "valid. PerceptionObstacle: "
           << perception_obstacle_.DebugString();
    return false;
  }
  //获取这个障碍物的长宽，以及预测轨迹点
  const double object_width = perception_obstacle_.width();
  const double object_length = perception_obstacle_.length();
  const auto& trajectory_points = trajectory_.trajectory_point();
  //如果预测轨迹点为空直接返回false
  if (trajectory_points.empty()) {
    AWARN << "object " << id_ << " has no trajectory points";
    return false;
  }
  //获取自车的物理参数，长度，长度一半，宽度
  const auto& adc_param =
      VehicleConfigHelper::Instance()->GetConfig().vehicle_param();
  const double adc_length = adc_param.length();
  const double adc_half_length = adc_length / 2.0;
  const double adc_width = adc_param.width();
  //这里创建了两个边界盒对象最小盒，最大盒？没看到在哪里用？
  //Box2d构造函数，center中心点，heading,length,width
  common::math::Box2d min_box({0, 0}, 1.0, 1.0, 1.0);
  common::math::Box2d max_box({0, 0}, 1.0, 1.0, 1.0);
  //ST点，就是纵向位置s和时间t构成的点(s,t)
  //又创建了一个ST点对的vector polygon_points,多边形点？
  std::vector<std::pair<STPoint, STPoint>> polygon_points;

  //又创建了一个SL边界类对象，last_sl_boundary上一次的sl边界？
  SLBoundary last_sl_boundary;
  //初始定义了上次的索引为0 last_index
  int last_index = 0;

  //从障碍物预测轨迹的第二个点开始遍历
  for (int i = 1; i < trajectory_points.size(); ++i) {
    ADEBUG << "last_sl_boundary: " << last_sl_boundary.ShortDebugString();

    //traj轨迹 = path路径 + 速度规划
    //障碍物第i-1个预测轨迹点
    const auto& first_traj_point = trajectory_points[i - 1];
    //障碍物第i个预测轨迹点
    const auto& second_traj_point = trajectory_points[i];
    //障碍物第i-1个预测轨迹点上的路径点
    const auto& first_point = first_traj_point.path_point();
    //障碍物第i个预测轨迹点上的路径点
    const auto& second_point = second_traj_point.path_point();

    //定义了一个物体移动边界盒长度 = 物体本身长度 + 两个轨迹点之间的长度，又准备搞障碍物第
    //i个轨迹点的边界盒了
    double object_moving_box_length =
        object_length + common::util::DistanceXY(first_point, second_point);

    //定义该障碍物(Obstacle类实例)第i个轨迹点的边界盒的中心，就是第i个，i-1轨迹点的中点
    common::math::Vec2d center((first_point.x() + second_point.x()) / 2.0,
                               (first_point.y() + second_point.y()) / 2.0);
    //构建一个2维边界盒对象 object_moving_box物体移动边界盒，center,heading,length,width,heading
    common::math::Box2d object_moving_box(
        center, first_point.theta(), object_moving_box_length, object_width);
    //又定义了SL边界类对象 object_boundary 目标边界？
    SLBoundary object_boundary;
    //Apollo原注释
    //注意：这种方法在参考线不是直的时候有误差，需要双回路来cover所有的corner cases。
    //粗糙的跳过距离上次sl边界盒过近的点？
    
    //这里就是计算障碍物第i预测轨迹点和第i-1个点之间的距离
    //上一个点的索引并不是单纯的i-1而是由last_index控制，如果可以忽略的情况下last_index并不会递增
    const double distance_xy =
        common::util::DistanceXY(trajectory_points[last_index].path_point(),
                                 trajectory_points[i].path_point());
    //大致意思是障碍物从i-1个点挪动到第i个轨迹点，横向上变化量假设最恶劣变化distance_xy两点间直线距离，都没有超过上一次的SL边界，就直接跳到下一个轨迹点？这块原理不是特别清楚？
    if (last_sl_boundary.start_l() > distance_xy ||
        last_sl_boundary.end_l() < -distance_xy) {
      continue;
    }

   //上一次SL边界的纵向中点
    const double mid_s =
        (last_sl_boundary.start_s() + last_sl_boundary.end_s()) / 2.0;
    const double start_s = std::fmax(0.0, mid_s - 2.0 * distance_xy);
    const double end_s = (i == 1) ? reference_line.Length()
                                  : std::fmin(reference_line.Length(),
                                              mid_s + 2.0 * distance_xy);

    if (!reference_line.GetApproximateSLBoundary(object_moving_box, start_s,
                                                 end_s, &object_boundary)) {
      AERROR << "failed to calculate boundary";
      return false;
    }

    // update history record
    last_sl_boundary = object_boundary;
    last_index = i;

    //跳过障碍物，如果它在道路参考线的一边的话，在横向上相对参考线的偏移大于
    //自车车宽一半+障碍物车长*0.4那么就忽略这个轨迹点
    static constexpr double kSkipLDistanceFactor = 0.4;
    const double skip_l_distance =
        (object_boundary.end_s() - object_boundary.start_s()) *
            kSkipLDistanceFactor +
        adc_width / 2.0;
//跳过障碍物，如果它在道路参考线的一边的话，在横向上相对参考线的偏移大于
    //自车车宽一半+障碍物车长*0.4那么就忽略这个轨迹点
    if (!IsCautionLevelObstacle() &&
        (std::fmin(object_boundary.start_l(), object_boundary.end_l()) >
             skip_l_distance ||
         std::fmax(object_boundary.start_l(), object_boundary.end_l()) <
             -skip_l_distance)) {
      continue;
    }

//如果障碍物的level不是Caution需要注意这个级别或者障碍物头部都在自车后方，这个轨迹点也可忽略
    if (!IsCautionLevelObstacle() && object_boundary.end_s() < 0) {
      // skip if behind reference line
      continue;
    }
    static constexpr double kSparseMappingS = 20.0;
    const double st_boundary_delta_s =
        (std::fabs(object_boundary.start_s() - adc_start_s) > kSparseMappingS)
            ? kStBoundarySparseDeltaS
            : kStBoundaryDeltaS;
    const double object_s_diff =
        object_boundary.end_s() - object_boundary.start_s();
    if (object_s_diff < st_boundary_delta_s) {
      continue;
    }
    const double delta_t =
        second_traj_point.relative_time() - first_traj_point.relative_time();
    double low_s = std::max(object_boundary.start_s() - adc_half_length, 0.0);
    bool has_low = false;
    double high_s =
        std::min(object_boundary.end_s() + adc_half_length, FLAGS_st_max_s);
    bool has_high = false;
    while (low_s + st_boundary_delta_s < high_s && !(has_low && has_high)) {
      if (!has_low) {
        auto low_ref = reference_line.GetReferencePoint(low_s);
        has_low = object_moving_box.HasOverlap(
            {low_ref, low_ref.heading(), adc_length,
             adc_width + FLAGS_nonstatic_obstacle_nudge_l_buffer});
        low_s += st_boundary_delta_s;
      }
      if (!has_high) {
        auto high_ref = reference_line.GetReferencePoint(high_s);
        has_high = object_moving_box.HasOverlap(
            {high_ref, high_ref.heading(), adc_length,
             adc_width + FLAGS_nonstatic_obstacle_nudge_l_buffer});
        high_s -= st_boundary_delta_s;
      }
    }
    if (has_low && has_high) {
      low_s -= st_boundary_delta_s;
      high_s += st_boundary_delta_s;
      double low_t =
          (first_traj_point.relative_time() +
           std::fabs((low_s - object_boundary.start_s()) / object_s_diff) *
               delta_t);
      polygon_points.emplace_back(
          std::make_pair(STPoint{low_s - adc_start_s, low_t},
                         STPoint{high_s - adc_start_s, low_t}));
      double high_t =
          (first_traj_point.relative_time() +
           std::fabs((high_s - object_boundary.start_s()) / object_s_diff) *
               delta_t);
      if (high_t - low_t > 0.05) {
        polygon_points.emplace_back(
            std::make_pair(STPoint{low_s - adc_start_s, high_t},
                           STPoint{high_s - adc_start_s, high_t}));
      }
    }
  }
  if (!polygon_points.empty()) {
    std::sort(polygon_points.begin(), polygon_points.end(),
              [](const std::pair<STPoint, STPoint>& a,
                 const std::pair<STPoint, STPoint>& b) {
                return a.first.t() < b.first.t();
              });
    auto last = std::unique(polygon_points.begin(), polygon_points.end(),
                            [](const std::pair<STPoint, STPoint>& a,
                               const std::pair<STPoint, STPoint>& b) {
                              return std::fabs(a.first.t() - b.first.t()) <
                                     kStBoundaryDeltaT;
                            });
    polygon_points.erase(last, polygon_points.end());
    if (polygon_points.size() > 2) {
      *st_boundary = STBoundary(polygon_points);
    }
  } else {
    return false;
  }
  return true;
}

//获取数据成员 参考线ST边界
const STBoundary& Obstacle::reference_line_st_boundary() const {
  return reference_line_st_boundary_;
}
//获取数据成员 路径ST边界
const STBoundary& Obstacle::path_st_boundary() const {
  return path_st_boundary_;
}

//返回数据成员，决策标签？一个字符串的vector,decider_tags_
const std::vector<std::string>& Obstacle::decider_tags() const {
  return decider_tags_;
}

//返回目标决策类型列表？
const std::vector<ObjectDecisionType>& Obstacle::decisions() const {
  return decisions_;
}

//nudge在apollo里代表横向上轻轻绕一下避开？
//判断目标决策类型对象是横向决策？如果有ignore或nudge就认为是
bool Obstacle::IsLateralDecision(const ObjectDecisionType& decision) {
  return decision.has_ignore() || decision.has_nudge();
}

//是否为纵向决策
//有ignore，有stop/yield/follow/overtake都是
bool Obstacle::IsLongitudinalDecision(const ObjectDecisionType& decision) {
  return decision.has_ignore() || decision.has_stop() || decision.has_yield() ||
         decision.has_follow() || decision.has_overtake();
}

//融合两个纵向决策类型对象，保留更保守的那个
//停车/跟车/减速避让都保留纵向距离更大的那个
//超车保留纵向距离更大的那个
ObjectDecisionType Obstacle::MergeLongitudinalDecision(
    const ObjectDecisionType& lhs, const ObjectDecisionType& rhs) {
  if (lhs.object_tag_case() == ObjectDecisionType::OBJECT_TAG_NOT_SET) {
    return rhs;
  }
  if (rhs.object_tag_case() == ObjectDecisionType::OBJECT_TAG_NOT_SET) {
    return lhs;
  }
  //portobuf库FindOrDie：查找map容器中指定key的value值地址，否则抛出FatalException异常或终止进程
  const auto lhs_val =
      FindOrDie(s_longitudinal_decision_safety_sorter_, lhs.object_tag_case());
  const auto rhs_val =
      FindOrDie(s_longitudinal_decision_safety_sorter_, rhs.object_tag_case());
  if (lhs_val < rhs_val) {
    return rhs;
  } else if (lhs_val > rhs_val) {
    return lhs;
  } else {
    if (lhs.has_ignore()) {
      return rhs;
    } else if (lhs.has_stop()) {
      return lhs.stop().distance_s() < rhs.stop().distance_s() ? lhs : rhs;
    } else if (lhs.has_yield()) {
      return lhs.yield().distance_s() < rhs.yield().distance_s() ? lhs : rhs;
    } else if (lhs.has_follow()) {
      return lhs.follow().distance_s() < rhs.follow().distance_s() ? lhs : rhs;
    } else if (lhs.has_overtake()) {
      return lhs.overtake().distance_s() > rhs.overtake().distance_s() ? lhs
                                                                       : rhs;
    } else {
      DCHECK(false) << "Unknown decision";
    }
  }
  return lhs;  // stop compiler complaining
}

//返回Obstacle类数据成员，针对这个障碍物的纵向决策
const ObjectDecisionType& Obstacle::LongitudinalDecision() const {
  return longitudinal_decision_;
}
//返回Obstacle类数据成员，针对这个障碍物的横向决策
const ObjectDecisionType& Obstacle::LateralDecision() const {
  return lateral_decision_;
}

//障碍物可被忽略？调用函数判断横/纵向均可被忽略？
bool Obstacle::IsIgnore() const {
  return IsLongitudinalIgnore() && IsLateralIgnore();
}

//判断该障碍物纵向决策是否为可忽略？
bool Obstacle::IsLongitudinalIgnore() const {
  return longitudinal_decision_.has_ignore();
}

//判断该障碍物横向决策是否为可忽略？
bool Obstacle::IsLateralIgnore() const {
  return lateral_decision_.has_ignore();
}

//融合两个横向的目标决策类型对象，保留避让nudge距离大的那个横向目标决策类型对象并返回
ObjectDecisionType Obstacle::MergeLateralDecision(
    const ObjectDecisionType& lhs, const ObjectDecisionType& rhs) {
  if (lhs.object_tag_case() == ObjectDecisionType::OBJECT_TAG_NOT_SET) {
    return rhs;
  }
  if (rhs.object_tag_case() == ObjectDecisionType::OBJECT_TAG_NOT_SET) {
    return lhs;
  }
  const auto lhs_val =
      FindOrDie(s_lateral_decision_safety_sorter_, lhs.object_tag_case());
  const auto rhs_val =
      FindOrDie(s_lateral_decision_safety_sorter_, rhs.object_tag_case());
  if (lhs_val < rhs_val) {
    return rhs;
  } else if (lhs_val > rhs_val) {
    return lhs;
  } else {
    if (lhs.has_ignore()) {
      return rhs;
    } else if (lhs.has_nudge()) {
      DCHECK(lhs.nudge().type() == rhs.nudge().type())
          << "could not merge left nudge and right nudge";
      return std::fabs(lhs.nudge().distance_l()) >
                     std::fabs(rhs.nudge().distance_l())
                 ? lhs
                 : rhs;
    }
  }
  DCHECK(false) << "Does not have rule to merge decision: "
                << lhs.ShortDebugString()
                << " and decision: " << rhs.ShortDebugString();
  return lhs;
}

//检查针对该障碍物是否有横向决策？
bool Obstacle::HasLateralDecision() const {
  return lateral_decision_.object_tag_case() !=
         ObjectDecisionType::OBJECT_TAG_NOT_SET;
}

//检查针对该障碍物是否有纵向决策？
bool Obstacle::HasLongitudinalDecision() const {
  return longitudinal_decision_.object_tag_case() !=
         ObjectDecisionType::OBJECT_TAG_NOT_SET;
}

//针对该障碍物的决策是否不可忽略
bool Obstacle::HasNonIgnoreDecision() const {
  return (HasLateralDecision() && !IsLateralIgnore()) ||
         (HasLongitudinalDecision() && !IsLongitudinalIgnore());
}

//针对该障碍物增加一个纵向决策，输入参数决策标签字符串decider_tag
//以及目标决策类型对象decision
//其实就是将输入的目标纵向决策类型对象与之前类里储存的进行融合。
//类的数据成员决策列表decisions_和决策标签列表decider_tags_里再增加一个
void Obstacle::AddLongitudinalDecision(const std::string& decider_tag,
                                       const ObjectDecisionType& decision) {
  DCHECK(IsLongitudinalDecision(decision))
      << "Decision: " << decision.ShortDebugString()
      << " is not a longitudinal decision";
  longitudinal_decision_ =
      MergeLongitudinalDecision(longitudinal_decision_, decision);
  ADEBUG << decider_tag << " added obstacle " << Id()
         << " longitudinal decision: " << decision.ShortDebugString()
         << ". The merged decision is: "
         << longitudinal_decision_.ShortDebugString();
  decisions_.push_back(decision);
  decider_tags_.push_back(decider_tag);
}

//针对该障碍物增加一个横向决策，输入参数决策标签字符串decider_tag
//以及目标决策类型对象decision
//其实就是将输入的目标横向决策类型对象与之前类里储存的进行融合。
//类的数据成员决策列表decisions_和决策标签列表decider_tags_里再增加一个
void Obstacle::AddLateralDecision(const std::string& decider_tag,
                                  const ObjectDecisionType& decision) {
  DCHECK(IsLateralDecision(decision))
      << "Decision: " << decision.ShortDebugString()
      << " is not a lateral decision";
  lateral_decision_ = MergeLateralDecision(lateral_decision_, decision);
  ADEBUG << decider_tag << " added obstacle " << Id()
         << " a lateral decision: " << decision.ShortDebugString()
         << ". The merged decision is: "
         << lateral_decision_.ShortDebugString();
  decisions_.push_back(decision);
  decider_tags_.push_back(decider_tag);
}

//返回debug字符串
std::string Obstacle::DebugString() const {
  std::stringstream ss;
  ss << "Obstacle id: " << id_;
  for (size_t i = 0; i < decisions_.size(); ++i) {
    ss << " decision: " << decisions_[i].DebugString() << ", made by "
       << decider_tags_[i];
  }
  if (lateral_decision_.object_tag_case() !=
      ObjectDecisionType::OBJECT_TAG_NOT_SET) {
    ss << "lateral decision: " << lateral_decision_.ShortDebugString();
  }
  if (longitudinal_decision_.object_tag_case() !=
      ObjectDecisionType::OBJECT_TAG_NOT_SET) {
    ss << "longitudinal decision: "
       << longitudinal_decision_.ShortDebugString();
  }
  return ss.str();
}

//获取针对该感知障碍物的SL边界盒？
//sl_boundary_其实就是障碍物边界点相对于道路参考线的SL坐标
const SLBoundary& Obstacle::PerceptionSLBoundary() const {
  return sl_boundary_;
}

//设定路径的ST边界，也就是针对该障碍物的ST边界？
void Obstacle::set_path_st_boundary(const STBoundary& boundary) {
  path_st_boundary_ = boundary;
  path_st_boundary_initialized_ = true;
}

//设定ST边界类型，设置为输入的类型
void Obstacle::SetStBoundaryType(const STBoundary::BoundaryType type) {
  path_st_boundary_.SetBoundaryType(type);
}

//擦除类里储存的路径ST边界
void Obstacle::EraseStBoundary() { path_st_boundary_ = STBoundary(); }

//设定道路参考线的ST边界
void Obstacle::SetReferenceLineStBoundary(const STBoundary& boundary) {
  reference_line_st_boundary_ = boundary;
}

//设定道路参考线ST边界类型
void Obstacle::SetReferenceLineStBoundaryType(
    const STBoundary::BoundaryType type) {
  reference_line_st_boundary_.SetBoundaryType(type);
}

//擦除类里储存的道路参考线ST边界
void Obstacle::EraseReferenceLineStBoundary() {
  reference_line_st_boundary_ = STBoundary();
}

//是否为有效的障碍物，主要是看感知障碍物的长宽是否为nan(not a number)或者过于小了，是的话就说明是无效的障碍物
bool Obstacle::IsValidObstacle(
    const perception::PerceptionObstacle& perception_obstacle) {
  const double object_width = perception_obstacle.width();
  const double object_length = perception_obstacle.length();

  const double kMinObjectDimension = 1.0e-6;
  return !std::isnan(object_width) && !std::isnan(object_length) &&
         object_width > kMinObjectDimension &&
         object_length > kMinObjectDimension;
}

//检查车道被该障碍物阻塞？输入参数是道路参考线类对象
void Obstacle::CheckLaneBlocking(const ReferenceLine& reference_line) {
  //如果障碍物不是静止，就没阻塞，并返回
  if (!IsStatic()) {
    is_lane_blocking_ = false;
    return;
  }
  //如果执行到这里说明障碍物已经是是静态障碍物了
  DCHECK(sl_boundary_.has_start_s());
  DCHECK(sl_boundary_.has_end_s());
  DCHECK(sl_boundary_.has_start_l());
  DCHECK(sl_boundary_.has_end_l());

//sl_boundary_其实就是障碍物边界点相对于道路参考线的SL坐标
//如果根据该障碍物的SL边界
//SL边界是相对道路参考线的，若求出的SL边界下限L坐标和上限L坐标异号，说明障碍物SL边界盒占据了道路参考线的两边，则认为是阻塞了
  if (sl_boundary_.start_l() * sl_boundary_.end_l() < 0.0) {
    is_lane_blocking_ = true;
    return;
  }

  //如果上面没有返回，说明障碍物是在道路参考线的一侧，且是静态障碍物
  //驾驶宽度 = 道路参考线计算刨去障碍物边界盒SL边界后左右宽度中更大的那个,也就是障碍物的左边界
  const double driving_width = reference_line.GetDrivingWidth(sl_boundary_);
  //获取车辆物理参数
  auto vehicle_param = common::VehicleConfigHelper::GetConfig().vehicle_param();

  //根据道路参考线判断sl_boundary_也就是该障碍物的SL边界盒在车道内？且驾驶宽度<小于车辆的宽度+静态横向避让缓冲距离0.3，道路阻塞的标志位is_lane_blocking_为true
  if (reference_line.IsOnLane(sl_boundary_) &&
      driving_width <
          vehicle_param.width() + FLAGS_static_obstacle_nudge_l_buffer) {
    is_lane_blocking_ = true;
    return;
  }

//上面都不满足的话，该障碍物就没有阻塞车道
  is_lane_blocking_ = false;
}

//设置数据成员，该障碍物阻塞换道？用输入参数 距离过近is_distance_clear拷贝给数据成员
//is_lane_change_blocking_
void Obstacle::SetLaneChangeBlocking(const bool is_distance_clear) {
  is_lane_change_blocking_ = is_distance_clear;
}

}  // namespace planning
}  // namespace apollo

附录

最小转弯半径避让停车距离计算函数

将障碍物投影到自车ST图(未完全看懂apollo实现细节)
这里的adc_start_s是指自车边界盒尾部的s坐标?障碍物占据的S上下边界都是相对自车尾部边界的纵向距离？真正的停车距离还要考虑一个自身车长？这块具体怎么在ST图中应用，看完ST图相关代码应该就清楚了。