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

概述

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

从类名来看，应该是约束检查器类？

从代码来看ConstraintChecker类主要是实现：
检查一条离散轨迹对象是否有效？检查轨迹是否有效/纵向横向的速度加速度加加速度超出边界/曲率超出边界，推测该类主要是用于筛选规划轨迹，缩小优化搜索的空间。

constraint_checker.h

#pragma once

#include "modules/planning/common/trajectory/discretized_trajectory.h"

namespace apollo {
namespace planning {

class ConstraintChecker {
 public:
 //一个枚举类型
//Result类其包含值VALID/LON_VELOCITY_OUT_OF_BOUND/LON_ACCELERATION_OUT_OF_BOUND/LON_JERK_OUT_OF_BOUND/...等等，主要就是检查轨迹是否有效/纵向横向的速度加速度加加速度超出边界/曲率超出边界
  enum class Result {
    VALID,
    LON_VELOCITY_OUT_OF_BOUND,
    LON_ACCELERATION_OUT_OF_BOUND,
    LON_JERK_OUT_OF_BOUND,
    LAT_VELOCITY_OUT_OF_BOUND,
    LAT_ACCELERATION_OUT_OF_BOUND,
    LAT_JERK_OUT_OF_BOUND,
    CURVATURE_OUT_OF_BOUND,
  };
  //禁用默认构造函数
  ConstraintChecker() = delete;
  
  //检查轨迹是否有效？
//返回值是ConstraintChecker::Result，是constraint_checker.h里声明的一个枚举类型
//Result类其包含值VALID/LON_VELOCITY_OUT_OF_BOUND/LON_ACCELERATION_OUT_OF_BOUND/LON_JERK_OUT_OF_BOUND/...等等，主要就是检查轨迹是否有效/纵向横向的速度加速度加加速度超出边界/曲率超出边界
//输入的是一条离散轨迹类对象
//推测该函数主要是用于筛选规划轨迹，缩小优化搜索的空间
//返回的就是输入轨迹是否有效及无效的原因？
  static Result ValidTrajectory(const DiscretizedTrajectory& trajectory);
};

}  // namespace planning
}  // namespace apollo

constraint_checker.cc

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

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

namespace apollo {
namespace planning {

namespace {
//定义了个模板函数，判断输入参数v的大小是否在同类型的lower,upper范围之间
template <typename T>
bool WithinRange(const T v, const T lower, const T upper) {
  return lower <= v && v <= upper;
}
}  // namespace

//检查轨迹是否有效？
//返回值是ConstraintChecker::Result，是constraint_checker.h里声明的一个枚举类型
//Result类其包含值VALID/LON_VELOCITY_OUT_OF_BOUND/LON_ACCELERATION_OUT_OF_BOUND/LON_JERK_OUT_OF_BOUND/...等等，主要就是检查轨迹是否有效/纵向横向的速度加速度加加速度超出边界/曲率超出边界
//输入的是一条离散轨迹类对象
//推测该函数主要是用于筛选规划轨迹，缩小优化搜索的空间
ConstraintChecker::Result ConstraintChecker::ValidTrajectory(
    const DiscretizedTrajectory& trajectory) {
  //定义一个常数最大检查的的相对时间为 planning_gflags.cc里定义的trajectory_time_length也就是轨迹时间长度8.0s
  const double kMaxCheckRelativeTime = FLAGS_trajectory_time_length;
  //遍历输入的离散轨迹
  for (const auto& p : trajectory) {
    //获取遍历的第i个轨迹点的相对时间 t
    double t = p.relative_time();
    //如果t已经大于8s则直接跳出for循环
    if (t > kMaxCheckRelativeTime) {
      break;
    }
    //获取遍历的第i个轨迹点的速度
    double lon_v = p.v();
    //判断遍历的第i个轨迹点的速度是否在planning_gflags里定义的速度上下限范围内
    //速度下限-0.1,上限40.0
    //轨迹点速度不在该范围内，报debug信息，返回LON_VELOCITY_OUT_OF_BOUND纵向速度超出边界
    if (!WithinRange(lon_v, FLAGS_speed_lower_bound, FLAGS_speed_upper_bound)) {
      ADEBUG << "Velocity at relative time " << t
             << " exceeds bound, value: " << lon_v << ", bound ["
             << FLAGS_speed_lower_bound << ", " << FLAGS_speed_upper_bound
             << "].";
      return Result::LON_VELOCITY_OUT_OF_BOUND;
    }

//遍历的第i个轨迹点的检查加速度是否超限，同理
//planning_gflags里定义的加速度上下限范围[-6.0,4.0]
    double lon_a = p.a();
    if (!WithinRange(lon_a, FLAGS_longitudinal_acceleration_lower_bound,
                     FLAGS_longitudinal_acceleration_upper_bound)) {
      ADEBUG << "Longitudinal acceleration at relative time " << t
             << " exceeds bound, value: " << lon_a << ", bound ["
             << FLAGS_longitudinal_acceleration_lower_bound << ", "
             << FLAGS_longitudinal_acceleration_upper_bound << "].";
      return Result::LON_ACCELERATION_OUT_OF_BOUND;
    }

//遍历的第i个轨迹点的检查曲率是否超限，同理
//planning_gflags里定义的曲率上下限范围[-0.1979,0.1979]
    double kappa = p.path_point().kappa();
    if (!WithinRange(kappa, -FLAGS_kappa_bound, FLAGS_kappa_bound)) {
      ADEBUG << "Kappa at relative time " << t
             << " exceeds bound, value: " << kappa << ", bound ["
             << -FLAGS_kappa_bound << ", " << FLAGS_kappa_bound << "].";
      return Result::CURVATURE_OUT_OF_BOUND;
    }
  }

//又开始遍历输入的轨迹的轨迹点？从第2个点开始，因为要用到前一个点
  for (size_t i = 1; i < trajectory.NumOfPoints(); ++i) {
    //获取第i-1个轨迹点p0
    const auto& p0 = trajectory.TrajectoryPointAt(static_cast<uint32_t>(i - 1));
    //获取第i个轨迹点p1
    const auto& p1 = trajectory.TrajectoryPointAt(static_cast<uint32_t>(i));

//如果第i个轨迹点p1的相对时间大于最大检查时间8s直接break跳出循环
    if (p1.relative_time() > kMaxCheckRelativeTime) {
      break;
    }

//获取第i-1个轨迹点p0的相对时间
    double t = p0.relative_time();

//求出p0p1轨迹点的时间间隔
    double dt = p1.relative_time() - p0.relative_time();
    //计算p0p1轨迹点的加速度的差值
    double d_lon_a = p1.a() - p0.a();
    //差分法计算第i个轨迹i点加加速度，是因为规划轨迹点上没有jerk，所以只能再高一个for循环做差分判断
    double lon_jerk = d_lon_a / dt;
    //判断第i个轨迹点处的加加速度是否超限[-4.0,2.0]，planning_gflags里定义的
    if (!WithinRange(lon_jerk, FLAGS_longitudinal_jerk_lower_bound,
                     FLAGS_longitudinal_jerk_upper_bound)) {
      ADEBUG << "Longitudinal jerk at relative time " << t
             << " exceeds bound, value: " << lon_jerk << ", bound ["
             << FLAGS_longitudinal_jerk_lower_bound << ", "
             << FLAGS_longitudinal_jerk_upper_bound << "].";
      return Result::LON_JERK_OUT_OF_BOUND;
    }

//检查横向加速度是否超限，planning_gflags里定义的范围[-4.0,4.0]
//横向加速度轨迹点上也没有，用公式v^2/R=V^2*kappa
    double lat_a = p1.v() * p1.v() * p1.path_point().kappa();
    if (!WithinRange(lat_a, -FLAGS_lateral_acceleration_bound,
                     FLAGS_lateral_acceleration_bound)) {
      ADEBUG << "Lateral acceleration at relative time " << t
             << " exceeds bound, value: " << lat_a << ", bound ["
             << -FLAGS_lateral_acceleration_bound << ", "
             << FLAGS_lateral_acceleration_bound << "].";
      return Result::LAT_ACCELERATION_OUT_OF_BOUND;
    }

//横向的jerk检查注释掉了？其实横向jerk也蛮影响舒适性的
    // TODO(zhangyajia): this is temporarily disabled
    // due to low quality reference line.
    /**
    double d_lat_a = p1.v() * p1.v() * p1.path_point().kappa() -
                     p0.v() * p0.v() * p0.path_point().kappa();
    double lat_jerk = d_lat_a / dt;
    if (!WithinRange(lat_jerk, -FLAGS_lateral_jerk_bound,
                     FLAGS_lateral_jerk_bound)) {
      ADEBUG << "Lateral jerk at relative time " << t
             << " exceeds bound, value: " << lat_jerk << ", bound ["
             << -FLAGS_lateral_jerk_bound << ", " << FLAGS_lateral_jerk_bound
             << "].";
      return Result::LAT_JERK_OUT_OF_BOUND;
    }
    **/
  }

//上面都没返回，说明轨迹是有效的
  return Result::VALID;
}

}  // namespace planning
}  // namespace apollo