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

概述

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

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

从代码来看ConstraintChecker1d类主要是实现：
感觉和前面的ConstraintChecker类实现的功能有点重复，
检查横纵向轨迹是否有效，纵向横向的速度、加速度、加加速度是否超出设定边界，是的话说明轨迹无效，不是说明有效。

constraint_checker1d.h

#pragma once

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

namespace apollo {
namespace planning {

class ConstraintChecker1d {
 public:
 //禁用默认构造函数
  ConstraintChecker1d() = delete;
  
//检查输入的1维曲线是否是有效的纵向轨迹
//输入参数：纵向轨迹lon_trajectory
  static bool IsValidLongitudinalTrajectory(const Curve1d& lon_trajectory);

//检查输入的1维曲线是否是有效的横向轨迹
//输入参数：横向轨迹lat_trajectory
//输入参数：同时也需要纵向轨迹lon_trajectory
  static bool IsValidLateralTrajectory(const Curve1d& lat_trajectory,
                                       const Curve1d& lon_trajectory);
};

}  // namespace planning
}  // namespace apollo

constraint_checker1d.cc

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

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

namespace apollo {
namespace planning {

namespace {

//内联函数 判断双精度浮点数v是否处于lower和upper之间
inline bool fuzzy_within(const double v, const double lower, const double upper,
                         const double e = 1.0e-4) {
  return v > lower - e && v < upper + e;
}
}  // namespace

//检查输入的1维曲线是否是有效的纵向轨迹
//输入参数：纵向轨迹lon_trajectory
bool ConstraintChecker1d::IsValidLongitudinalTrajectory(
    const Curve1d& lon_trajectory) { 
  double t = 0.0;
  //按照时间间隔0.1s遍历输入纵向轨迹上的点
  while (t < lon_trajectory.ParamLength()) {
    //针对第i个时间t，插值得到其对应的v
    double v = lon_trajectory.Evaluate(1, t);  // evaluate_v
    //v是否在planning_gflags.cc定义的上下限范围内[-0.1,40.0]
    if (!fuzzy_within(v, FLAGS_speed_lower_bound, FLAGS_speed_upper_bound)) {
      return false;
    }

//检查加速度a是否在planning_gflags.cc定义的上下限范围内[-6.0,4.0]
    double a = lon_trajectory.Evaluate(2, t);  // evaluate_a
    if (!fuzzy_within(a, FLAGS_longitudinal_acceleration_lower_bound,
                      FLAGS_longitudinal_acceleration_upper_bound)) {
      return false;
    }

//检查加加速度jerk是否在planning_gflags.cc定义的上下限范围内[-4.0,2.0]
    double j = lon_trajectory.Evaluate(3, t);
    if (!fuzzy_within(j, FLAGS_longitudinal_jerk_lower_bound,
                      FLAGS_longitudinal_jerk_upper_bound)) {
      return false;
    }
    t += FLAGS_trajectory_time_resolution;
  }
  //如果上面都没有返回false，说明输入的纵向轨迹是有效的，没有违背相关约束
  return true;
}

//检查输入的1维曲线是否是有效的横向轨迹
//输入参数：横向轨迹lat_trajectory
//输入参数：同时也需要纵向轨迹lon_trajectory
bool ConstraintChecker1d::IsValidLateralTrajectory(
    const Curve1d& lat_trajectory, const Curve1d& lon_trajectory) {
  double t = 0.0;
  //按照0.1s的时间间隔遍历横向的时间对应的轨迹点
  while (t < lon_trajectory.ParamLength()) {
    //Evaluate代表插值，前面的0代表插值的阶数，0阶代表直接插值，1代表对其导数进行插值
    //获得遍历的第i个时间对应的纵向轨迹点的s, s',s''
    //获得遍历的第i个时间对应的横向轨迹点的d', d''
    double s = lon_trajectory.Evaluate(0, t);
    double dd_ds = lat_trajectory.Evaluate(1, s);
    double ds_dt = lon_trajectory.Evaluate(1, t);

    double d2d_ds2 = lat_trajectory.Evaluate(2, s);
    double d2s_dt2 = lon_trajectory.Evaluate(2, t);

//如果没有超过纵向轨迹的话，计算其横向加速度ay=d''*s'*s'+d'*s''
    double a = 0.0;
    if (s < lat_trajectory.ParamLength()) {
      a = d2d_ds2 * ds_dt * ds_dt + dd_ds * d2s_dt2;
    }

//检查横向轨迹的第i个轨迹点的横向加速度在planning_gflags.cc定义的上下限范围内？
//[-4,4]
    if (!fuzzy_within(a, -FLAGS_lateral_acceleration_bound,
                      FLAGS_lateral_acceleration_bound)) {
      return false;
    }

    // this is not accurate, just an approximation...
    //apollo原注释，这里不精确只是一个估计近似...，
    double j = 0.0;
    //计算横向加加速度jerk = d'''*s'''?
    if (s < lat_trajectory.ParamLength()) {
      j = lat_trajectory.Evaluate(3, s) * lon_trajectory.Evaluate(3, t);
    }

//检查横向轨迹上的轨迹点的横向加加速度是否超限，planning_gflags.cc里定义的范围为
//[-4,4]
    if (!fuzzy_within(j, -FLAGS_lateral_jerk_bound, FLAGS_lateral_jerk_bound)) {
      return false;
    }
    t += FLAGS_trajectory_time_resolution;
  }
  return true;
}

}  // namespace planning
}  // namespace apollo