概述

PathTimeGraph类是apollo planning模块下modules\planning\lattice\behavior\path_time_graph.cc/.h实现

从类名来看，应该是路径-时间图类？

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

主要实现自车ST图的构建,以及对应每一个规划离散路径点的横向边界设置(根据车道宽度以及障碍物信息)
1.储存ST图的s轴,t轴的范围，车辆初始状态，参考线信息，障碍物列表等；
2.储存ST图需要考虑的障碍物列表；
3.获取时刻t，参考线上障碍物纵向边界[s0_lower,s0_upper],[s1_lower,s1_upper],…
4.获取指定障碍物从开始阻塞参考线到结束阻塞时间段里一系列得(s,t)，对其进行二次时间采样变成时间间隔为1.0s(t_min_density)得一系列(s,t)点？
5.根据输入得障碍物id判断障碍物是否在ST图上
6.获取考虑车道宽度和静态障碍物的自车横向边界数组[l0_lower,l0_upper],[l1_lower,l1_upper],…
7.将动静态障碍物投影到ST图上，其实就是将各时刻障碍物id和障碍物的sl边界对应关系储存在类成员path_time_obstacle_map_映射map
8.根据障碍物更新横向边界(静态障碍物留了0.1m的buffer，动态障碍物留了0.3m的buffer)

path_time_graph.h

#pragma once

#include <string>
#include <unordered_map>
#include <utility>
#include <vector>

#include "modules/common/math/polygon2d.h"
#include "modules/common/proto/geometry.pb.h"
#include "modules/planning/common/frame.h"
#include "modules/planning/common/obstacle.h"
#include "modules/planning/common/reference_line_info.h"
#include "modules/planning/common/speed/st_boundary.h"
#include "modules/planning/common/speed/st_point.h"
#include "modules/planning/reference_line/reference_line.h"

namespace apollo {
namespace planning {

class PathTimeGraph {
 public:
 
//构造函数
//输入参数：障碍物列表obstacles
//输入参数：离散参考路径点？discretized_ref_points
//输入参数：参考线信息类指针ptr_reference_line_info
//输入参数：纵向起点s_start
//输入参数：纵向终点s_end
//输入参数：时间起点t_start
//输入参数：时间终点t_end
//输入参数：init_d代表初始得自车车辆横向偏移？
//将输入参数用于初始化类数据成员，代表该类里储存携带的状态信息
  PathTimeGraph(const std::vector<const Obstacle*>& obstacles,
                const std::vector<common::PathPoint>& discretized_ref_points,
                const ReferenceLineInfo* ptr_reference_line_info,
                const double s_start, const double s_end, const double t_start,
                const double t_end, const std::array<double, 3>& init_d);

//获取路径时间障碍物列表，直接返回类成员path_time_obstacles_？
//所有ST图需要考虑的障碍物的列表？
  const std::vector<STBoundary>& GetPathTimeObstacles() const;

//获取路径时间障碍物？根据id去类成员path_time_obstacle_map_中查找输入id对应的障碍物ST边界，查询结果放入输入的第二个参数path_time_obstacle指针里
  bool GetPathTimeObstacle(const std::string& obstacle_id,
                           STBoundary* path_time_obstacle);

//获取路径阻塞的间隔(我理解就是在t时刻阻塞参考线的障碍物的在ST图上投影的线段[s_lower,s_upper]？可能有多个障碍物阻塞参考线，也就返回多个线段)，输入参数时时间t
  std::vector<std::pair<double, double>> GetPathBlockingIntervals(
      const double t) const;


//获取整个ST图时间轴发范围的路径阻塞纵向间隔，返回包含多个[s_lower,s_upper]的数组
//输入参数：t_start ST图时间起点
//输入参数：t_end ST图时间终点
//输入参数：t_resolution ST图时间精度
  std::vector<std::vector<std::pair<double, double>>> GetPathBlockingIntervals(
      const double t_start, const double t_end, const double t_resolution);

//获取ST图路径s的范围？直接返回类成员path_range_
  std::pair<double, double> get_path_range() const;

//获取ST图的时间范围，直接返回类成员time_range_
  std::pair<double, double> get_time_range() const;

//获取障碍物周围的点？返回的是ST点的数组
//输入参数：障碍物id,s的距离s_dist(通常很小调用时代入1e-6)?最小的时间密度t_min_density(通常代入配置文件中定义的1.0s)?
//获取输入障碍物从开始阻塞参考线到结束阻塞时间段里一系列得(s,t)，对其进行二次时间采样变成时间间隔为1.0s(t_min_density)得一系列(s,t)点？
  std::vector<STPoint> GetObstacleSurroundingPoints(
      const std::string& obstacle_id, const double s_dist,
      const double t_density) const;

//根据输入得障碍物id判断障碍物是否在ST图上
//其实就是去类成员path_time_obstacle_map_里看能不能找到这个障碍物id
//如果找不到就会返回最后一个说明就没找到，障碍物就不在ST图上
  bool IsObstacleInGraph(const std::string& obstacle_id);

//获取横向得边界
//输入参数：ST图得s得起点s_start和终点s_end
//输入参数：s得采样精度s_resolution，在调用时该参数是一个从planning_gflags.cc里定义得参数为1.0m
//返回的是s从ST图start~end每间隔1.0m采样的横向上下边界{[d_lower,d_upper]...}
//首先会用车道宽度去设置横向边界，然后遍历所有的ST图的静态障碍物去更新这个边界数组，最后将这个边界转化成车辆后轴中心位置横向偏移的边界(加上或减去车宽一半即可)
  std::vector<std::pair<double, double>> GetLateralBounds(
      const double s_start, const double s_end, const double s_resolution);

 private:
 //安装障碍物列表
//输入参数：障碍物列表obstacles
//输入参数：离散参考点数组discretized_ref_points
//将需要考虑的动静态障碍物投影到ST图上，并将相应的信息存到类成员里path_time_obstacle_map_，
  void SetupObstacles(
      const std::vector<const Obstacle*>& obstacles,
      const std::vector<common::PathPoint>& discretized_ref_points);

//计算障碍物的的边界
//输入参数：障碍物的边界多边形点vertices
//输入参数：参考线的离散路径点，为了将障碍物边界投影到参考线上
  SLBoundary ComputeObstacleBoundary(
      const std::vector<common::math::Vec2d>& vertices,
      const std::vector<common::PathPoint>& discretized_ref_points) const;

//设置路径时间点？输入参数障碍物id和s,t
//其实就是设置障碍物边界点在ST图上的投影，对应的id对应的t时障碍物对应的s
  STPoint SetPathTimePoint(const std::string& obstacle_id, const double s,
                           const double t) const;

//设置静态的障碍物
//输入参数：单个障碍物对象obstacle
//输入参数：离散参考点数组discretized_ref_points，这个是参考线轨迹点，就是为了求解障碍物在参考线上的投影的sl边界
//其实就是path_time_obstacle_map_里储存了静障碍物id及其在ST图上的投影映射关系
//这里是将静止障碍物投影到ST图上
//静态障碍物投影到ST图上的原理见附录
  void SetStaticObstacle(
      const Obstacle* obstacle,
      const std::vector<common::PathPoint>& discretized_ref_points);

//设置动态的障碍物，将动态障碍物投影到ST图上
//输入参数：动态障碍物对象obstacle
//输入参数：参考线路径点数组discretized_ref_points，这个是为了求解动态障碍物边界在参考线上投影的SL坐标
//动态障碍物投影到ST图上的原理见附录
  void SetDynamicObstacle(
      const Obstacle* obstacle,
      const std::vector<common::PathPoint>& discretized_ref_points);

//根据障碍物更新横向边界
//输入参数:障碍物的SL边界sl_boundary，包括start_s,end_s,start_l,end_l
//输入参数：参考线的离散路径点数组
//输入参数：ST图的s轴的起点，终点
//输入参数：bounds用于存放更新后的横向边界,输入进来时存放着更新前的自车横向边界，即自车后轴中心的横向坐标d坐标的上下限
  void UpdateLateralBoundsByObstacle(
      const SLBoundary& sl_boundary,
      const std::vector<double>& discretized_path, const double s_start,
      const double s_end, std::vector<std::pair<double, double>>* const bounds);

 private:
  std::pair<double, double> time_range_;
  std::pair<double, double> path_range_;
  const ReferenceLineInfo* ptr_reference_line_info_;
  std::array<double, 3> init_d_;

  std::unordered_map<std::string, STBoundary> path_time_obstacle_map_;
  std::vector<STBoundary> path_time_obstacles_;
  std::vector<SLBoundary> static_obs_sl_boundaries_;
};

}  // namespace planning
}  // namespace apollo

path_time_graph.cc

#include "modules/planning/lattice/behavior/path_time_graph.h"

#include <algorithm>
#include <limits>

#include "modules/common/configs/vehicle_config_helper.h"
#include "modules/common/math/linear_interpolation.h"
#include "modules/common/math/path_matcher.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/proto/sl_boundary.pb.h"

namespace apollo {
namespace planning {

using apollo::common::PathPoint;
using apollo::common::TrajectoryPoint;
using apollo::common::math::Box2d;
using apollo::common::math::lerp;
using apollo::common::math::PathMatcher;
using apollo::common::math::Polygon2d;

//构造函数
//输入参数：障碍物列表obstacles
//输入参数：离散参考路径点？discretized_ref_points
//输入参数：参考线信息类指针ptr_reference_line_info
//输入参数：纵向起点s_start
//输入参数：纵向终点s_end
//输入参数：时间起点t_start
//输入参数：时间终点t_end
//输入参数：init_d代表初始得自车车辆横向偏移？
//将输入参数用于初始化类数据成员，代表该类里储存携带的状态信息
PathTimeGraph::PathTimeGraph(
    const std::vector<const Obstacle*>& obstacles,
    const std::vector<PathPoint>& discretized_ref_points,
    const ReferenceLineInfo* ptr_reference_line_info, const double s_start,
    const double s_end, const double t_start, const double t_end,
    const std::array<double, 3>& init_d) {
 //主要就是将输入参数赋给类成员
  CHECK_LT(s_start, s_end);
  CHECK_LT(t_start, t_end);
  path_range_.first = s_start;
  path_range_.second = s_end;
  time_range_.first = t_start;
  time_range_.second = t_end;
  ptr_reference_line_info_ = ptr_reference_line_info;
  init_d_ = init_d;

//安装障碍物列表？输入参数是障碍物列表和离散参考点
//将需要考虑的动静态障碍物投影到ST图上，并将相应的信息存到类成员里path_time_obstacle_map_，
  SetupObstacles(obstacles, discretized_ref_points);
}

//计算障碍物的的边界
//输入参数：障碍物的边界多边形点vertices
//输入参数：参考线的离散路径点，为了将障碍物边界投影到参考线上
SLBoundary PathTimeGraph::ComputeObstacleBoundary(
    const std::vector<common::math::Vec2d>& vertices,
    const std::vector<PathPoint>& discretized_ref_points) const {
  //初始定义start_s，end_s，start_l，end_l为double 能表示的最大或最小的数
  double start_s(std::numeric_limits<double>::max());
  double end_s(std::numeric_limits<double>::lowest());
  double start_l(std::numeric_limits<double>::max());
  double end_l(std::numeric_limits<double>::lowest());

//遍历障碍物的多边形点
  for (const auto& point : vertices) {
    //把遍历的障碍物多边形点xy坐标投影到参考线的frenet系的sl坐标sl_point 
    auto sl_point = PathMatcher::GetPathFrenetCoordinate(discretized_ref_points,
                                                         point.x(), point.y());
    //用上面获得的sl坐标赋给start_s，end_s，start_l，end_l
    //也就是障碍物的sl边界
    start_s = std::fmin(start_s, sl_point.first);
    end_s = std::fmax(end_s, sl_point.first);
    start_l = std::fmin(start_l, sl_point.second);
    end_l = std::fmax(end_l, sl_point.second);
  }

//将上面获得的start_s，end_s，start_l，end_l塞入SL边界类对象里sl_boundary
  SLBoundary sl_boundary;
  sl_boundary.set_start_s(start_s);
  sl_boundary.set_end_s(end_s);
  sl_boundary.set_start_l(start_l);
  sl_boundary.set_end_l(end_l);

//返回sl边界对象
  return sl_boundary;
}

//安装障碍物列表
//输入参数：障碍物列表obstacles
//输入参数：离散参考点数组discretized_ref_points
//将需要考虑的动静态障碍物投影到ST图上，并将相应的信息存到类成员里path_time_obstacle_map_，
void PathTimeGraph::SetupObstacles(
    const std::vector<const Obstacle*>& obstacles,
    const std::vector<PathPoint>& discretized_ref_points) {
  //遍历输入的障碍物列表
  for (const Obstacle* obstacle : obstacles) {
    //如果障碍物是虚拟的就直接跳到下一个障碍物
    if (obstacle->IsVirtual()) {
      continue;
    }
    //如果障碍物没有轨迹，就设置静态的障碍物物？将静态的障碍物投影到ST图上
    //path_time_obstacle_map_里储存了障碍物id及其在ST图上的投影映射关系
    if (!obstacle->HasTrajectory()) {
      SetStaticObstacle(obstacle, discretized_ref_points);
    } else {
      //如果障碍物有轨迹，就将该动态的障碍物投影到ST图上
      SetDynamicObstacle(obstacle, discretized_ref_points);
    }
  }
//按照障碍物的s对成员static_obs_sl_boundaries_静态障碍物sl边界列表排序？
  std::sort(static_obs_sl_boundaries_.begin(), static_obs_sl_boundaries_.end(),
            [](const SLBoundary& sl0, const SLBoundary& sl1) {
              return sl0.start_s() < sl1.start_s();
            });
//遍历id和障碍物ST图投影映射map，将这些ST图上的障碍物塞到类成员path_time_obstacles_里
  for (auto& path_time_obstacle : path_time_obstacle_map_) {
    path_time_obstacles_.push_back(path_time_obstacle.second);
  }
}

//设置静态的障碍物
//输入参数：单个障碍物对象obstacle
//输入参数：离散参考点数组discretized_ref_points，这个是参考线轨迹点，就是为了求解障碍物在参考线上的投影的sl边界
//其实就是path_time_obstacle_map_里储存了静障碍物id及其在ST图上的投影映射关系
//这里是将静止障碍物投影到ST图上
//静态障碍物投影到ST图上的原理见附录
void PathTimeGraph::SetStaticObstacle(
    const Obstacle* obstacle,
    const std::vector<PathPoint>& discretized_ref_points) {
  //获取障碍物对象边框的感知多边形polygon 
  const Polygon2d& polygon = obstacle->PerceptionPolygon();

//定义障碍物idobstacle_id 为输入的障碍物的id
  std::string obstacle_id = obstacle->Id();
  //根据障碍物的感知边框多边形边界计算障碍物的sl边界，start_s,end_s,start_l,end_l
  SLBoundary sl_boundary =
      ComputeObstacleBoundary(polygon.GetAllVertices(), discretized_ref_points);

//获取参考线车道中心线左边宽度，右边宽度
//车道宽度默认值是4.0m
  double left_width = FLAGS_default_reference_line_width * 0.5;
  double right_width = FLAGS_default_reference_line_width * 0.5;
  //获取类成员参考线信息成员对象里的参考线该障碍物所处的车道宽度，放入left_width，right_width
  ptr_reference_line_info_->reference_line().GetLaneWidth(
      sl_boundary.start_s(), &left_width, &right_width);
  //如果障碍物在横纵向有一个不在参考线范围内，就直接返回
  if (sl_boundary.start_s() > path_range_.second ||
      sl_boundary.end_s() < path_range_.first ||
      sl_boundary.start_l() > left_width ||
      sl_boundary.end_l() < -right_width) {
    ADEBUG << "Obstacle [" << obstacle_id << "] is out of range.";
    return;
  }

//设置路径时间障碍物map里障碍物id为输入的障碍物的id
  path_time_obstacle_map_[obstacle_id].set_id(obstacle_id);
 //设置路径时间障碍物map里对应障碍物id映射的障碍物的SL边界的纵向起始点以及对应时间0.0
 //ST图该静态障碍物平行四边形的底部左边点
  path_time_obstacle_map_[obstacle_id].set_bottom_left_point(
      SetPathTimePoint(obstacle_id, sl_boundary.start_s(), 0.0));
  
//设置路径时间障碍物map里对应障碍物id映射的障碍物的SL边界的纵向起点以及对应时间8.0 
//其实应为这个函数是设置静态障碍物，所以认为这个障碍物边界起始点在接下来8s内都会静止在start_s处 
//ST图该静态障碍物平行四边形的底部右边点
path_time_obstacle_map_[obstacle_id].set_bottom_right_point(SetPathTimePoint(
      obstacle_id, sl_boundary.start_s(), FLAGS_trajectory_time_length));
  
  //同理，该障碍物的在0.0s对应的end_s就是当前SL边界的end_s
  //ST图该静态障碍物平行四边形的顶部左边点
  path_time_obstacle_map_[obstacle_id].set_upper_left_point(
      SetPathTimePoint(obstacle_id, sl_boundary.end_s(), 0.0));
  //ST图该静态障碍物平行四边形的顶部右边点
  path_time_obstacle_map_[obstacle_id].set_upper_right_point(SetPathTimePoint(
      obstacle_id, sl_boundary.end_s(), FLAGS_trajectory_time_length));
  static_obs_sl_boundaries_.push_back(std::move(sl_boundary));
  ADEBUG << "ST-Graph mapping static obstacle: " << obstacle_id
         << ", start_s : " << sl_boundary.start_s()
         << ", end_s : " << sl_boundary.end_s()
         << ", start_l : " << sl_boundary.start_l()
         << ", end_l : " << sl_boundary.end_l();
}

//设置动态的障碍物，将动态障碍物投影到ST图上
//输入参数：动态障碍物对象obstacle
//输入参数：参考线路径点数组discretized_ref_points，这个是为了求解动态障碍物边界在参考线上投影的SL坐标
//动态障碍物投影到ST图上的原理见附录
void PathTimeGraph::SetDynamicObstacle(
    const Obstacle* obstacle,
    const std::vector<PathPoint>& discretized_ref_points) {
  //time_range_里储存的ST图的时间起点，和时间终点
  //首先先定义relative_time 为ST图的时间起点time_range_.first
  double relative_time = time_range_.first;
  //当relative_time < ST图的终点时间时
  while (relative_time < time_range_.second) {
  //先获取相对时间relative_time处的障碍物预测轨迹点point 
    TrajectoryPoint point = obstacle->GetPointAtTime(relative_time);
    //获取该轨迹点处该动态障碍物的二维边界盒
    Box2d box = obstacle->GetBoundingBox(point);
    //根据离散的参考线路径点，将障碍物的边界投影到参考线上的SL边界即障碍物的
    //start_s,end_s,start_l,end_l
    SLBoundary sl_boundary =
        ComputeObstacleBoundary(box.GetAllCorners(), discretized_ref_points);

//获取默认的参考线的车道宽度(planning_gflags.cc里定义为4.0m)的一半为参考线车道左宽度，右宽度
    double left_width = FLAGS_default_reference_line_width * 0.5;
    double right_width = FLAGS_default_reference_line_width * 0.5;
    
    //获取障碍物在这个相对时间所在的车道的宽度，宽度左边宽度，右边宽度(正常情况下是车道的一半)放入left_width，right_width
    ptr_reference_line_info_->reference_line().GetLaneWidth(
        sl_boundary.start_s(), &left_width, &right_width);

    //如果障碍物不在参考线的横纵向范围内，但是是动态障碍物也要考虑，那么加这个if的意义是？
    if (sl_boundary.start_s() > path_range_.second ||
        sl_boundary.end_s() < path_range_.first ||
        sl_boundary.start_l() > left_width ||
        sl_boundary.end_l() < -right_width) {
        //若在类成员path_time_obstacle_map_存放的障碍物id和其在ST图的投影的映射关系，若在该类成员里找不到这个id，就直接break不再考虑该障碍物？
      if (path_time_obstacle_map_.find(obstacle->Id()) !=
          path_time_obstacle_map_.end()) {
        break;
      }
      //trajectory_time_resolution在planning_gflags.cc里定义为0.1s
      //然后相对时间再递增0.1s，遍历动态障碍物的整个预测轨迹时间
      relative_time += FLAGS_trajectory_time_resolution;
      continue;
    }

//如果障碍物在参考线的横纵向范围内，去类成员path_time_obstacle_map_根据障碍物id查找对应的ST图投影，若等于最后一个说明没找到，就在path_time_obstacle_map_里增加这个障碍物的id
//path_time_obstacle_map_就是该类用于存放障碍物id和其在ST图上投影的一个映射map
    if (path_time_obstacle_map_.find(obstacle->Id()) ==
        path_time_obstacle_map_.end()) {
      path_time_obstacle_map_[obstacle->Id()].set_id(obstacle->Id());


//简单来说，下面的代码就是在这个relative_time时间上，该动态障碍物在ST图上被投影为一条线段，一条start_s到end_s的线段，然后遍历完所有的relative_time,不就求到了这个动态障碍物在整个障碍物的预测轨迹上的ST图投影
//设置遍历的障碍物预测轨迹的相对时间relative_time处障碍物的边界S_lower为其处在该相对时间下的SL边界的start_s
      path_time_obstacle_map_[obstacle->Id()].set_bottom_left_point(
          SetPathTimePoint(obstacle->Id(), sl_boundary.start_s(),
                           relative_time));
//设置遍历的障碍物预测轨迹的相对时间relative_time处，障碍物的边界S_upper为其在该相对时间下的SL边界的end_s
      path_time_obstacle_map_[obstacle->Id()].set_upper_left_point(
          SetPathTimePoint(obstacle->Id(), sl_boundary.end_s(), relative_time));
    }
    path_time_obstacle_map_[obstacle->Id()].set_bottom_right_point(
        SetPathTimePoint(obstacle->Id(), sl_boundary.start_s(), relative_time));
    path_time_obstacle_map_[obstacle->Id()].set_upper_right_point(
        SetPathTimePoint(obstacle->Id(), sl_boundary.end_s(), relative_time));
    relative_time += FLAGS_trajectory_time_resolution;
  }
}
//设置路径时间点？输入参数障碍物id和s,t
//其实就是设置障碍物边界点在ST图上的投影，对应的id对应的t时障碍物对应的s
STPoint PathTimeGraph::SetPathTimePoint(const std::string& obstacle_id,
                                        const double s, const double t) const {
  STPoint path_time_point(s, t);
  return path_time_point;
}

//获取路径时间障碍物列表，直接返回类成员path_time_obstacles_？
//所有ST图需要考虑的障碍物的列表？
const std::vector<STBoundary>& PathTimeGraph::GetPathTimeObstacles() const {
  return path_time_obstacles_;
}

//获取路径时间障碍物？根据id去类成员path_time_obstacle_map_中查找输入id对应的障碍物ST边界，查询结果放入输入的第二个参数path_time_obstacle指针里
bool PathTimeGraph::GetPathTimeObstacle(const std::string& obstacle_id,
                                        STBoundary* path_time_obstacle) {
  if (path_time_obstacle_map_.find(obstacle_id) ==
      path_time_obstacle_map_.end()) {
    return false;
  }
  *path_time_obstacle = path_time_obstacle_map_[obstacle_id];
  return true;
}

//获取路径阻塞的间隔(我理解就是在t时刻阻塞参考线的障碍物的在ST图上投影的线段[s_lower,s_upper]？可能有多个障碍物阻塞参考线，也就返回多个线段)，输入参数时时间t
std::vector<std::pair<double, double>> PathTimeGraph::GetPathBlockingIntervals(
    const double t) const {
  //检查输入的时间t是否在ST图的时间轴范围内
  ACHECK(time_range_.first <= t && t <= time_range_.second);
  //定义double对的数据对数组？时间间隔列表intervals？
  std::vector<std::pair<double, double>> intervals;
  //遍历类成员path_time_obstacles_也就是ST图上的所有障碍物
  for (const auto& pt_obstacle : path_time_obstacles_) {
    //若输入的时间t，与该次遍历的障碍物的时间无重叠关系，直接跳到下一个障碍物
    if (t > pt_obstacle.max_t() || t < pt_obstacle.min_t()) {
      continue;
    }
    //获取s的上界，根据时间t插值ST图上t时刻障碍物的s上界？
    double s_upper = lerp(pt_obstacle.upper_left_point().s(),
                          pt_obstacle.upper_left_point().t(),
                          pt_obstacle.upper_right_point().s(),
                          pt_obstacle.upper_right_point().t(), t);
//获取s的上界，根据时间t插值ST图上t时刻障碍物的s下界？
    double s_lower = lerp(pt_obstacle.bottom_left_point().s(),
                          pt_obstacle.bottom_left_point().t(),
                          pt_obstacle.bottom_right_point().s(),
                          pt_obstacle.bottom_right_point().t(), t);

    intervals.emplace_back(s_lower, s_upper);
  }
  return intervals;
}

//获取整个ST图时间轴发范围的路径阻塞纵向间隔，返回包含多个[s_lower,s_upper]的数组
//输入参数：t_start ST图时间起点
//输入参数：t_end ST图时间终点
//输入参数：t_resolution ST图时间精度
std::vector<std::vector<std::pair<double, double>>>
PathTimeGraph::GetPathBlockingIntervals(const double t_start,
                                        const double t_end,
                                        const double t_resolution) {
  //定义个空的纵向间隔intervals数组 将存放多个[s_lower,s_upper]
  std::vector<std::vector<std::pair<double, double>>> intervals;
  //遍历ST图的时间轴，每次时间递增t_resolution(通常是0.1s)
  for (double t = t_start; t <= t_end; t += t_resolution) {
    //调用上面的函数找到t时刻阻碍参考线的障碍物边界的[s_lower,s_supper]塞入intervals
    intervals.push_back(GetPathBlockingIntervals(t));
  }
  //返回intervals
  return intervals;
}

//获取ST图路径s的范围？直接返回类成员path_range_
std::pair<double, double> PathTimeGraph::get_path_range() const {
  return path_range_;
}

//获取ST图的时间范围，直接返回类成员time_range_
std::pair<double, double> PathTimeGraph::get_time_range() const {
  return time_range_;
}

//获取障碍物周围的点？返回的是ST点的数组
//输入参数：障碍物id,s的距离s_dist(通常很小调用时代入1e-6)?最小的时间密度t_min_density(通常代入配置文件中定义的1.0s)?
//获取输入障碍物从开始阻塞参考线到结束阻塞时间段里一系列得(s,t)，对其进行二次时间采样变成时间间隔为1.0s(t_min_density)得一系列(s,t)点？
std::vector<STPoint> PathTimeGraph::GetObstacleSurroundingPoints(
    const std::string& obstacle_id, const double s_dist,
    const double t_min_density) const {
  ACHECK(t_min_density > 0.0);
  //定义ST点对数组pt_pairs
  std::vector<STPoint> pt_pairs;
  //去类成员path_time_obstacle_map_根据id查找障碍物的ST边界，如果没有找到对应的id直接返回空的点对数组
  if (path_time_obstacle_map_.find(obstacle_id) ==
      path_time_obstacle_map_.end()) {
    return pt_pairs;
  }

//获取输入id的障碍物的ST边界类对象
  const auto& pt_obstacle = path_time_obstacle_map_.at(obstacle_id);

//定义t0,s0,t1,s1先全部初始化为0
  double s0 = 0.0;
  double s1 = 0.0;

  double t0 = 0.0;
  double t1 = 0.0;
  
  //一般输入的参数s_dist是一个小正数1e-6
  //获取输入障碍物id对应的障碍物在ST图上的投影的
 //s0为障碍物在ST图上投影的平行四边形的左上角点对应的s(t0障碍物开始阻塞参考线的时间)
  //s1为障碍物在ST图上投影的平行四边形的右上角点对应的s(t1障碍物结束阻塞参考线的时间)
  if (s_dist > 0.0) {
    s0 = pt_obstacle.upper_left_point().s();
    s1 = pt_obstacle.upper_right_point().s();

    t0 = pt_obstacle.upper_left_point().t();
    t1 = pt_obstacle.upper_right_point().t();
  } else {   //一般else不会触发，s_dist确保为一个小正数
    s0 = pt_obstacle.bottom_left_point().s();
    s1 = pt_obstacle.bottom_right_point().s();

    t0 = pt_obstacle.bottom_left_point().t();
    t1 = pt_obstacle.bottom_right_point().t();
  }

//求得该障碍物从开始阻塞参考线到结束阻塞所经历的时间间隔time_gap
  double time_gap = t1 - t0;
  ACHECK(time_gap > -FLAGS_numerical_epsilon);
  //对这个时间间隔取绝对值
  time_gap = std::fabs(time_gap);

//求得这个时间间隔有几个1.0s，就有time_gap / t_min_density + 1个采样点
  size_t num_sections = static_cast<size_t>(time_gap / t_min_density + 1);
  double t_interval = time_gap / static_cast<double>(num_sections);

//这是将t0-t1按时间间隔1.0s重新采样得到得新的ST点(s,t)塞入pt_pairs
//最后pt_pairs就储存了这个障碍物从开始阻塞参考线到结束阻塞时间段里一系列得(s,t)?
  for (size_t i = 0; i <= num_sections; ++i) {
    double t = t_interval * static_cast<double>(i) + t0;
    double s = lerp(s0, t0, s1, t1, t) + s_dist;

    STPoint ptt;
    ptt.set_t(t);
    ptt.set_s(s);
    pt_pairs.push_back(std::move(ptt));
  }

  return pt_pairs;
}

//根据输入得障碍物id判断障碍物是否在ST图上
//其实就是去类成员path_time_obstacle_map_里看能不能找到这个障碍物id
//如果找不到就会返回最后一个说明就没找到，障碍物就不在ST图上
bool PathTimeGraph::IsObstacleInGraph(const std::string& obstacle_id) {
  return path_time_obstacle_map_.find(obstacle_id) !=
         path_time_obstacle_map_.end();
}

//获取横向得边界
//输入参数：ST图得s得起点s_start和终点s_end
//输入参数：s得采样精度s_resolution，在调用时该参数是一个从planning_gflags.cc里定义得参数为1.0m
//返回的是s从ST图start~end每间隔1.0m采样的横向上下边界{[d_lower,d_upper]...}
//首先会用车道宽度去设置横向边界，然后遍历所有的ST图的静态障碍物去更新这个边界数组，最后将这个边界转化成车辆后轴中心位置横向偏移的边界(加上或减去车宽一半即可)
std::vector<std::pair<double, double>> PathTimeGraph::GetLateralBounds(
    const double s_start, const double s_end, const double s_resolution) {
  CHECK_LT(s_start, s_end);
  CHECK_GT(s_resolution, FLAGS_numerical_epsilon);
  //定义个边界变量bounds，其里储存得数据格式类似于{(start_l0,endl0),(start_l1,endl1)}
  std::vector<std::pair<double, double>> bounds;
  //定义一条空得离散路径discretized_path
  std::vector<double> discretized_path;
  //获取ST图得纵向S得范围
  double s_range = s_end - s_start;
  //定义当前s为s_start
  double s_curr = s_start;
  
  //定义横向边界采样点得数量 = ST图得s总长 / s得采样长度1.0m
  size_t num_bound = static_cast<size_t>(s_range / s_resolution);

//获取车辆额参数配置，里面得车辆宽度ego_width
  const auto& vehicle_config =
      common::VehicleConfigHelper::Instance()->GetConfig();
  double ego_width = vehicle_config.vehicle_param().width();

  //用参考线得车道宽度初始化横向边界点数组
  //s从s_start开始到s_end每次递增1.0m，求出s对应的横向上下边界，取(车道宽度一半)和(自车初始横向偏移 + 车宽一半 + 缓冲0.1m)两者里的较大值？
  for (size_t i = 0; i < num_bound; ++i) {
  //默认参考线车道宽default_reference_line_width 4.0m
    double left_width = FLAGS_default_reference_line_width / 2.0;
    double right_width = FLAGS_default_reference_line_width / 2.0;
    //获取当前自车s_curr(s_start)所处得车道宽度，车道中心线左边宽度，右边宽度分别放入left_width，right_width
    ptr_reference_line_info_->reference_line().GetLaneWidth(s_curr, &left_width,
    
                                                            &right_width);
    //初始自车得横向边界，初始的自车位置横向偏移init_d_
    //在此基础上减去车辆宽的一半就是横向下边界(右)，加车宽一半就是自车横向上边界(左)
    double ego_d_lower = init_d_[0] - ego_width / 2.0;
    double ego_d_upper = init_d_[0] + ego_width / 2.0;
    //在确定车辆的横向边界时左右各留了0.1m的缓冲bound_buffer
    bounds.emplace_back(
    //车辆的横向左边界取自车左边界的d坐标(初始车辆横向偏移的d坐标再加上车宽一半再加0.1m的缓冲区)和车道宽度一半这两者里的较大值，横向右边界同理
        std::min(-right_width, ego_d_lower - FLAGS_bound_buffer),
        std::max(left_width, ego_d_upper + FLAGS_bound_buffer));
    //离散路径把s_curr塞入？
    discretized_path.push_back(s_curr);
    //s_curr递增1.0m
    s_curr += s_resolution;
  }

//遍历类成员静态障碍物SL边界的列表static_obs_sl_boundaries_
  for (const SLBoundary& static_sl_boundary : static_obs_sl_boundaries_) {
   //用静态障碍物去更新横向边界，更新的结果依然存放再bounds里
    UpdateLateralBoundsByObstacle(static_sl_boundary, discretized_path, s_start,
                                  s_end, &bounds);
  }

//将横向边界采样点里的每个点，右边界加上自车车宽的一半变为自车后轴中心的位置横向的边界
//左边界同理
  for (size_t i = 0; i < bounds.size(); ++i) {
    bounds[i].first += ego_width / 2.0;
    bounds[i].second -= ego_width / 2.0;
    if (bounds[i].first >= bounds[i].second) {
      bounds[i].first = 0.0;
      bounds[i].second = 0.0;
    }
  }
  //返回上述处理过后的横向边界
  return bounds;
}

//根据障碍物更新横向边界
//输入参数:障碍物的SL边界sl_boundary，包括start_s,end_s,start_l,end_l
//输入参数：参考线的离散路径点数组
//输入参数：ST图的s轴的起点，终点
//输入参数：bounds用于存放更新后的横向边界,输入进来时存放着更新前的自车横向边界，即自车后轴中心的横向坐标d坐标的上下限
void PathTimeGraph::UpdateLateralBoundsByObstacle(
    const SLBoundary& sl_boundary, const std::vector<double>& discretized_path,
    const double s_start, const double s_end,
    std::vector<std::pair<double, double>>* const bounds) {
 //首先判断输入的障碍物是否在ST图的S轴范围内，不在的话直接return，不做任何更新
  if (sl_boundary.start_s() > s_end || sl_boundary.end_s() < s_start) {
    return;
  }
  //在参考线路径点上找到距离输入障碍物最近的那个start_iter，刚好不大于输入障碍物的start_s的路径点？(可以理解为障碍物车尾最近的参考路径点，取得s稍小那个)
  auto start_iter = std::lower_bound(
      discretized_path.begin(), discretized_path.end(), sl_boundary.start_s());
      //在参考线路径点上找到距离输入障碍物最近的那个end_iter，刚好不小于输入障碍物的start_s的路径点？(可以理解为障碍物车尾最近的参考路径点，取得s稍大那个)
  auto end_iter = std::upper_bound(
      discretized_path.begin(), discretized_path.end(), sl_boundary.start_s());
      //障碍物起始点最近的参考线路径点(区间左边)的下标
  size_t start_index = start_iter - discretized_path.begin();
  //障碍物起始点最近的参考线路径点(区间右边)的下标
  size_t end_index = end_iter - discretized_path.begin();
 //如果输入的障碍物的车左边界超过了l=0(参考线中心)? 且车右边界没超过l=0(参考线中心)？其实就是障碍物占据了这个s对应的位置，其实就是这个障碍物车占据了参考线路径
  if (sl_boundary.end_l() > -FLAGS_numerical_epsilon &&
      sl_boundary.start_l() < FLAGS_numerical_epsilon) {
    //那么参考线路径在这个障碍物附近的路径点的横向边界全部设置为0 [-1e-6 1e-6]
    //就这个s的地方，自车根本不能过来
    for (size_t i = start_index; i < end_index; ++i) {
      bounds->operator[](i).first = -FLAGS_numerical_epsilon;
      bounds->operator[](i).second = FLAGS_numerical_epsilon;
    }
    return;
  }
   //如果输入的障碍物的车左边界没有向左越过了l=0(参考线中心)?
   //那么说明该障碍物没有阻塞参考线路径？(车辆处于参考线路径中心线右边，且侵占未超过一半？)
  if (sl_boundary.end_l() < FLAGS_numerical_epsilon) {
    //那么参考线路径在这个障碍物附近的路径点的横向边界的设置
    for (size_t i = start_index; i < std::min(end_index + 1, bounds->size());
         ++i) {
         //自车在此处的横线位置的下边界(最多是障碍物的左边界的l坐标 + 碰撞缓冲0.3m)
         //nudge_buffer在planning_gflags.cc里定义为0.3m
      bounds->operator[](i).first =
          std::max(bounds->operator[](i).first,
                   sl_boundary.end_l() + FLAGS_nudge_buffer);
    }
    return;
  }
   //如果输入的障碍物的车右边界没有向右越过了l=0(参考线中心)?(车辆处于参考线路径中心线左边，且侵占未超过一半？)
   //那么说明该障碍物没有阻塞参考线路径？
  if (sl_boundary.start_l() > -FLAGS_numerical_epsilon) {
     //那么参考线路径在这个障碍物附近的路径点的横向边界的设置
    for (size_t i = start_index; i < std::min(end_index + 1, bounds->size());
         ++i) {
         //自车在此处的横线位置的上边界(最多是障碍物的右边界的l坐标 - 碰撞缓冲0.3m)
         //nudge_buffer在planning_gflags.cc里定义为0.3m
      bounds->operator[](i).second =
          std::min(bounds->operator[](i).second,
                   sl_boundary.start_l() - FLAGS_nudge_buffer);
    }
    return;
  }
}

}  // namespace planning
}  // namespace apollo

附录

静态障碍物在ST图上的投影

动态障碍物在ST图上的投影

障碍物平行四边形？