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

概述

跳过了Apollo\modules\planning\conf没有介绍，里面都是一些配置文件，单独介绍意义不大，结合具体涉及的代码，也方便理解相关参数的意义。

CollisionChecker类是apollo planning模块下modules\planning\constraint_checker\collision_checker.cc/.h实现

从类名来看，应该是碰撞检查器类？

从代码来看CollisionChecker类主要是实现：
1.储存自车相对于参考线的横纵向偏移sd坐标，参考线信息对象，以及路径-时间图对象；
2.根据的储存的信息构建自车周围的环境，筛选出需要考虑的障碍物，建立所有需要考虑的障碍物在其预测轨迹上的二维矩形边界盒，储存到类成员预测边界盒列表里；
3.根据上一步建立的周围环境，也就是预测边界盒列表，遍历自车规划轨迹上的每一个自车边界盒，是否与预测边界盒列表里的对应时刻所有待考虑障碍物边界盒重叠，重叠则说明产生碰撞；虚拟障碍物和位于自车后面同一车道的障碍物不考虑；
4.判断自车是否处于参考线车道内？
5.判断障碍物是否在自车同一车道的后方？

言而总之，就是对自车的规划轨迹进行碰撞检测，判断每一个时刻自车规划轨迹点的自车矩形边界盒去判断是否与该时刻所有需要考虑的障碍物矩形边界盒是否有几何上的重叠关系？其他成员函数都是实现这一功能的辅助函数。

*函数里中间变量predicted_env储存了是其中一个时刻的所有待考虑障碍物二维矩形边界盒
*类成员predicted_bounding_rectangles_是储存了0-8s时刻的predicted_env

collision_checker.h

#pragma once

#include <memory>
#include <vector>

#include "modules/common/math/box2d.h"
#include "modules/planning/common/obstacle.h"
#include "modules/planning/common/reference_line_info.h"
#include "modules/planning/common/trajectory/discretized_trajectory.h"
#include "modules/planning/lattice/behavior/path_time_graph.h"

namespace apollo {
namespace planning {

class CollisionChecker {
 public:
 //带参构造函数
//输入参数：障碍物列表obstacles
//输入参数：自车的纵向坐标ego_vehicle_s
//输入参数：自车的横向偏移ego_vehicle_d
//输入参数：离散参考线ptr_reference_line_info
//输入参数：路径-时间图ptr_path_time_graph
//其实也就是把输入参数赋值给类成员
  CollisionChecker(
      const std::vector<const Obstacle*>& obstacles, const double ego_vehicle_s,
      const double ego_vehicle_d,
      const std::vector<common::PathPoint>& discretized_reference_line,
      const ReferenceLineInfo* ptr_reference_line_info,
      const std::shared_ptr<PathTimeGraph>& ptr_path_time_graph);

//判断是否碰撞，输入参数只有一个自车的离散轨迹？
  bool InCollision(const DiscretizedTrajectory& discretized_trajectory);

//InCollision判断是否会产生碰撞？
//输入参数：obstacles障碍物列表
//输入参数：ego_trajectory自车的离散轨迹？是规划的0-8s的自车轨迹吗？
//输入参数：ego_length，ego_width自车的长宽
//输入参数：ego_back_edge_to_center自车的后边缘到质心的距离？
  static bool InCollision(const std::vector<const Obstacle*>& obstacles,
                          const DiscretizedTrajectory& ego_trajectory,
                          const double ego_length, const double ego_width,
                          const double ego_edge_to_center);

 private:
 //建立预测环境函数？其实就是把待考虑障碍物的0-8s预测轨迹上的二维边界盒都塞入类成员predicted_bounding_rectangles_了？
//输入参数：障碍物列表，自车横纵向坐标，离散参考线
  void BuildPredictedEnvironment(
      const std::vector<const Obstacle*>& obstacles, const double ego_vehicle_s,
      const double ego_vehicle_d,
      const std::vector<common::PathPoint>& discretized_reference_line);

//判断自车是否在参考线的车道上
//输入参数：ego_vehicle_s自车的纵向位置s
//输入参数：ego_vehicle_d自车的横向偏移
  bool IsEgoVehicleInLane(const double ego_vehicle_s,
                          const double ego_vehicle_d);

//判断障碍物是否在自车后面的同一车道上
//输入参数：障碍物对象obstacle
//输入参数：自车的纵向位置ego_vehicle_s
//输入参数：discretized_reference_line离散的参考线，也就是路径点的vector，是自车的参考线？
  bool IsObstacleBehindEgoVehicle(
      const Obstacle* obstacle, const double ego_vehicle_s,
      const std::vector<apollo::common::PathPoint>& discretized_reference_line);

 private:
  const ReferenceLineInfo* ptr_reference_line_info_;
  std::shared_ptr<PathTimeGraph> ptr_path_time_graph_;
  std::vector<std::vector<common::math::Box2d>> predicted_bounding_rectangles_;
};

}  // namespace planning
}  // namespace apollo

collision_checker.cc

#include "modules/planning/constraint_checker/collision_checker.h"

#include <utility>

#include "cyber/common/log.h"
#include "modules/common/configs/vehicle_config_helper.h"
#include "modules/common/math/path_matcher.h"
#include "modules/common/math/vec2d.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/prediction/proto/prediction_obstacle.pb.h"

namespace apollo {
namespace planning {

using apollo::common::PathPoint;
using apollo::common::TrajectoryPoint;
using apollo::common::math::Box2d;
using apollo::common::math::PathMatcher;
using apollo::common::math::Vec2d;

//带参构造函数
//输入参数：障碍物列表obstacles
//输入参数：自车的纵向坐标ego_vehicle_s
//输入参数：自车的横向偏移ego_vehicle_d
//输入参数：离散参考线ptr_reference_line_info
//输入参数：路径-时间图ptr_path_time_graph
//其实也就是把输入参数赋值给类成员
CollisionChecker::CollisionChecker(
    const std::vector<const Obstacle*>& obstacles, const double ego_vehicle_s,
    const double ego_vehicle_d,
    const std::vector<PathPoint>& discretized_reference_line,
    const ReferenceLineInfo* ptr_reference_line_info,
    const std::shared_ptr<PathTimeGraph>& ptr_path_time_graph) {
  ptr_reference_line_info_ = ptr_reference_line_info;
  ptr_path_time_graph_ = ptr_path_time_graph;
  //将自车附近需要考虑的障碍物0-8s预测轨迹上的二维边界盒都塞入类成员predicted_bounding_rectangles_来构建二维边界盒列表，代表0-8s自车周围的环境？
  BuildPredictedEnvironment(obstacles, ego_vehicle_s, ego_vehicle_d,
                            discretized_reference_line);
}

//InCollision判断是否会产生碰撞？
//输入参数：obstacles障碍物列表
//输入参数：ego_trajectory自车的离散轨迹？是规划的0-8s的自车轨迹吗？
//输入参数：ego_length，ego_width自车的长宽
//输入参数：ego_back_edge_to_center自车的后边缘到质心的距离？
bool CollisionChecker::InCollision(
    const std::vector<const Obstacle*>& obstacles,
    const DiscretizedTrajectory& ego_trajectory, const double ego_length,
    const double ego_width, const double ego_back_edge_to_center) {
  //遍历自车轨迹上每一个点
  for (size_t i = 0; i < ego_trajectory.NumOfPoints(); ++i) {
    //获取第i个自车轨迹点ego_point
    const auto& ego_point =
        ego_trajectory.TrajectoryPointAt(static_cast<std::uint32_t>(i));
    //第i个自车轨迹点的相对时间relative_time
    const auto relative_time = ego_point.relative_time();
    //第i个自车轨迹点的航向ego_theta
    const auto ego_theta = ego_point.path_point().theta();
    //建立第i个自车轨迹点的二位边界盒
    Box2d ego_box({ego_point.path_point().x(), ego_point.path_point().y()},
                  ego_theta, ego_length, ego_width);

    // correct the inconsistency of reference point and center point
    // TODO(all): move the logic before constructing the ego_box
    //这是计算自车质心到几何中心的距离
    double shift_distance = ego_length / 2.0 - ego_back_edge_to_center;
    
    //下面3行就是把自车的第i个轨迹点的二维边界盒中心从质心转化到几何中心了？
    Vec2d shift_vec(shift_distance * std::cos(ego_theta),
                    shift_distance * std::sin(ego_theta));
    ego_box.Shift(shift_vec);

//定义一个障碍物的二维边界盒空列表obstacle_boxes
    std::vector<Box2d> obstacle_boxes;
    //遍历输入的障碍物列表，建立障碍物同样的相对时间下的边界盒，判断自车边界盒与障碍物边界盒有overlap也就是重叠？有的话就返回true，那么检测两个二维边界盒的实现代码位于modules\common\math\box2d.cc里的Box2d::HasOverlap(const Box2d &box)函数，判断两个边界盒是否有重叠
    for (const auto obstacle : obstacles) {
      auto obtacle_point = obstacle->GetPointAtTime(relative_time);
      Box2d obstacle_box = obstacle->GetBoundingBox(obtacle_point);
      if (ego_box.HasOverlap(obstacle_box)) {
        return true;
      }
    }
  }
  //如果执行到这里还没返回，说明没有碰撞
  return false;
}

//判断是否碰撞，输入参数只有一个自车的离散轨迹？
bool CollisionChecker::InCollision(
    const DiscretizedTrajectory& discretized_trajectory) {
  //CHECK_LE检查离散轨迹点的数量 <= 预测边界盒的数量(所有待考虑障碍物在0-8s预测轨迹上的所有边界盒)？
  CHECK_LE(discretized_trajectory.NumOfPoints(),
           predicted_bounding_rectangles_.size());
  //获取车辆的配置，然后获取车辆的物理参数，也就是长宽
  const auto& vehicle_config =
      common::VehicleConfigHelper::Instance()->GetConfig();
  double ego_length = vehicle_config.vehicle_param().length();
  double ego_width = vehicle_config.vehicle_param().width();

//遍历自车的离散轨迹？
  for (size_t i = 0; i < discretized_trajectory.NumOfPoints(); ++i) {
    //获取自车轨迹上自车的第i个轨迹点trajectory_point
    const auto& trajectory_point =
        discretized_trajectory.TrajectoryPointAt(static_cast<std::uint32_t>(i));
    //获取第i个轨迹点上自车的航向ego_theta
    double ego_theta = trajectory_point.path_point().theta();
    //轨迹点以及车辆长宽，航向，建立车辆的二维边界盒对象ego_box
    Box2d ego_box(
        {trajectory_point.path_point().x(), trajectory_point.path_point().y()},
        ego_theta, ego_length, ego_width);
    //下面6行又是将自车以质心建立的二维边界盒转化到几何中心的边界盒
    double shift_distance =
        ego_length / 2.0 - vehicle_config.vehicle_param().back_edge_to_center();
    Vec2d shift_vec{shift_distance * std::cos(ego_theta),
                    shift_distance * std::sin(ego_theta)};
    ego_box.Shift(shift_vec);

//遍历类成员predicted_bounding_rectangles_中储存的0-8s所有待考虑的障碍物的预测轨迹上的所有点处的二维边界盒，调用modules\common\math\box2d.cc里的Box2d::HasOverlap(const Box2d &box)函数，判断两个边界盒是否有重叠
//有重叠的话说明会产生碰撞
    for (const auto& obstacle_box : predicted_bounding_rectangles_[i]) {
      if (ego_box.HasOverlap(obstacle_box)) {
        return true;
      }
    }
  }
  //执行到这里都没有返回，说明没有产生碰撞
  return false;
}

//建立预测环境函数？其实就是把待考虑障碍物的0-8s预测轨迹上的二维边界盒都塞入类成员predicted_bounding_rectangles_了？
//输入参数：障碍物列表，自车横纵向坐标，离散参考线
void CollisionChecker::BuildPredictedEnvironment(
    const std::vector<const Obstacle*>& obstacles, const double ego_vehicle_s,
    const double ego_vehicle_d,
    const std::vector<PathPoint>& discretized_reference_line) {
  ACHECK(predicted_bounding_rectangles_.empty());

  // 如果自车在车道上，然后忽略所有来自相同车道的障碍物？
  //调用车辆的横纵向坐标判断自车是否在车道上？
  bool ego_vehicle_in_lane = IsEgoVehicleInLane(ego_vehicle_s, ego_vehicle_d);
  //待考虑的障碍物列表obstacles_considered
  std::vector<const Obstacle*> obstacles_considered;
  //遍历输入的障碍物列表
  for (const Obstacle* obstacle : obstacles) {
    //如果障碍物是虚拟的则直接跳到下一个障碍物
    if (obstacle->IsVirtual()) {
      continue;
    }
    //如果自车在车道上 且 (障碍物也在自车的后面二千也在同意车道 或 障碍物不在路径时间图上)，那么就直接跳到下一个障碍物
    if (ego_vehicle_in_lane &&
        (IsObstacleBehindEgoVehicle(obstacle, ego_vehicle_s,
                                    discretized_reference_line) ||
         !ptr_path_time_graph_->IsObstacleInGraph(obstacle->Id()))) {
      continue;
    }

//若上面都没有continue，待考虑的障碍物增加一个
    obstacles_considered.push_back(obstacle);
  }

//定义相对时间relative_time=0.0
  double relative_time = 0.0;
  //当相对时间 < 轨迹时间长度8s就一直在while循环内
  while (relative_time < FLAGS_trajectory_time_length) {
   //定义一个二维预测边界盒vector数组？predicted_env
    std::vector<Box2d> predicted_env;
    //遍历所有需要考虑的障碍物
    for (const Obstacle* obstacle : obstacles_considered) {
      // If an obstacle has no trajectory, it is considered as static.
      // Obstacle::GetPointAtTime has handled this case.
      // 如果障碍物没有轨迹，则被认为是静态障碍物
      // 障碍物：GetPointAtTime已经解决了这个case这种情况？
      // 障碍物在0,0.1,0.2,0.3,0.4,0.5,...7.9,8.0s处的轨迹点，看你处在while的第i次循环，就获取障碍物预测轨迹上相对时间等于i*0.1s的轨迹点放在point
      TrajectoryPoint point = obstacle->GetPointAtTime(relative_time);
      //获取障碍物在这个相对时间的二维边界盒box
      Box2d box = obstacle->GetBoundingBox(point);
      //lon_collision_buffer在planning_gflags.cc里被定义为2.0m，也就是障碍物的边界盒在纵向上往外前后各扩张1m，留一个安全余量作为碰撞的缓冲
      box.LongitudinalExtend(2.0 * FLAGS_lon_collision_buffer);
      //lat_collision_buffer在planning_gflags.cc里被定义为0.1m，就是障碍物边界盒左右各扩张0.05m作为碰撞的缓冲。
      box.LateralExtend(2.0 * FLAGS_lat_collision_buffer);
       //然后预测的环境predicted_env往里塞入这个障碍物的边界盒
       //predicted_env储存了是其中一个时刻的所有待考虑障碍物边界盒
       //predicted_bounding_rectangles_是储存了0-8s时刻的predicted_env
      predicted_env.push_back(std::move(box));
    }
    //遍历完所有障碍物后感知边界矩形列表往里塞入predicted_env
    predicted_bounding_rectangles_.push_back(std::move(predicted_env));
    //在相对时间i*0.1s（0，0.1，...，8.0s）处，所有的待考虑的障碍物的边界矩形都塞入predicted_bounding_rectangles_，那是不是最后0-8s所有的障碍物及其预测轨迹的边界盒也都塞进predicted_bounding_rectangles_了？每个障碍物0-8s的边界盒都塞入了
    relative_time += FLAGS_trajectory_time_resolution;
  }
}

//判断自车是否在参考线的车道上
//输入参数：ego_vehicle_s自车的纵向位置s
//输入参数：ego_vehicle_d自车的横向偏移
bool CollisionChecker::IsEgoVehicleInLane(const double ego_vehicle_s,
                                          const double ego_vehicle_d) {
  //首先取出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;
  //根据类成员参考线信息的指针去查询当前车辆所在的纵向位置ego_vehicle_s所处的车道的左宽度右宽度(中心线左宽度+中心线右宽度=车道宽度)，结果放入left_width，right_width
  ptr_reference_line_info_->reference_line().GetLaneWidth(
      ego_vehicle_s, &left_width, &right_width);
  //主要判断车辆的横向偏移是否超出了车道中心线左右宽度的范围
  return ego_vehicle_d < left_width && ego_vehicle_d > -right_width;
}

//判断障碍物是否在自车后面的同一车道上
//输入参数：障碍物对象obstacle
//输入参数：自车的纵向位置ego_vehicle_s
//输入参数：discretized_reference_line离散的参考线，也就是路径点的vector，是自车的参考线？
bool CollisionChecker::IsObstacleBehindEgoVehicle(
    const Obstacle* obstacle, const double ego_vehicle_s,
    const std::vector<PathPoint>& discretized_reference_line) {
  //又取出默认车道宽度4.0m的一半也就是2.0m
  double half_lane_width = FLAGS_default_reference_line_width * 0.5;
  //获取障碍物在当前时间戳处的轨迹点
  TrajectoryPoint point = obstacle->GetPointAtTime(0.0);
 //将障碍物当前的xy坐标投影到相对于自车参考线的frenet系sl坐标
  auto obstacle_reference_line_position = PathMatcher::GetPathFrenetCoordinate(
      discretized_reference_line, point.path_point().x(),
      point.path_point().y());

//如果障碍物的sl坐标里的s<自车的s 且障碍物的横向l坐标绝对值小于车道宽度的一半，意思就是既在自车后面，又在同一条车道内，就返回true
  if (obstacle_reference_line_position.first < ego_vehicle_s &&
      std::fabs(obstacle_reference_line_position.second) < half_lane_width) {
    ADEBUG << "Ignore obstacle [" << obstacle->Id() << "]";
    return true;
  }
  //到这里都还没返回，说明不在，返回false
  return false;
}

}  // namespace planning
}  // namespace apollo