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

概述

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

从类名来看，应该是速度限速类。

从代码来看SpeedLimit类主要是实现：
用一个vector储存一系列的限速点(s,v)；
在这个vector中增加一对限速点；
清空这个vector中的限速点；
根据指定的s去vector里查找对应的v，根据纵向位置获取对应限速；

*s代表相对于轨迹起点的纵向位置

speed_limit.h

#pragma once

#include 
#include 

namespace apollo {
namespace planning {

class SpeedLimit {
 public:
 //默认构造函数
  SpeedLimit() = default;

//增加一个速度限制
//其实这个类里就是储存一系列的(沿参考线纵坐标s，限速v)的二维向量vector罢了
//这里就是增加一对指定的(s,v)
  void AppendSpeedLimit(const double s, const double v);

//返回类里储存一系列的(沿参考线纵坐标s，限速v)的二维向量vector speed_limit_points_
//类成员，速度限制点，就是一系列的(s,v)坐标
//返回值不可修改
  const std::vector<std::pair<double, double>>& speed_limit_points() const;

//根据输入参数s去类储存的一系列的(s,v)坐标vector里找到纵向坐标s刚好小于给定值的那个点的v
//刚好小于就是这一系列(s,v)点按照s升序排列，就是vector里纵向坐标小于给定s的最后一个点
//说白了，就是根据指定的s去vector里查找对应的v，根据纵向位置获取对应限速
  double GetSpeedLimitByS(const double s) const;

//清空储存的(s,v) vector信息
  void Clear();

 private:
  //用一个vector来代表限速
  //第一个数是s，第二个数是v
  //s意思是从轨迹起点(s=0)处的纵向距离，v是车辆限速
  std::vector<std::pair<double, double>> speed_limit_points_;
};

}  // namespace planning
}  // namespace apollo

speed_limit.cc

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

#include 

#include "cyber/common/log.h"

namespace apollo {
namespace planning {

//增加一个速度限制
//其实这个类里就是储存一系列的(沿参考线纵坐标s，限速v)的二维向量vector罢了
//这里就是增加一对指定的(s,v)
void SpeedLimit::AppendSpeedLimit(const double s, const double v) {
  if (!speed_limit_points_.empty()) {
    DCHECK_GE(s, speed_limit_points_.back().first);
  }
  speed_limit_points_.emplace_back(s, v);
}

//返回类里储存一系列的(沿参考线纵坐标s，限速v)的二维向量vector speed_limit_points_
//类成员，速度限制点，就是一系列的(s,v)坐标
//返回值不可修改
const std::vector<std::pair<double, double>>& SpeedLimit::speed_limit_points()
    const {
  return speed_limit_points_;
}

//根据输入参数s去类储存的一系列的(s,v)坐标vector里找到纵向坐标s刚好小于给定值的那个点的v
//刚好小于就是这一系列(s,v)点按照s升序排列，就是vector里纵向坐标小于给定s的最后一个点
//说白了，就是根据指定的s去vector里查找对应的v，根据纵向位置获取对应限速
double SpeedLimit::GetSpeedLimitByS(const double s) const {
  CHECK_GE(speed_limit_points_.size(), 2U);
  DCHECK_GE(s, speed_limit_points_.front().first);

  auto compare_s = [](const std::pair<double, double>& point, const double s) {
    return point.first < s;
  };

  auto it_lower = std::lower_bound(speed_limit_points_.begin(),
                                   speed_limit_points_.end(), s, compare_s);

  if (it_lower == speed_limit_points_.end()) {
    return (it_lower - 1)->second;
  }
  return it_lower->second;
}

//清空储存的(s,v) vector信息
void SpeedLimit::Clear() { speed_limit_points_.clear(); }

}  // namespace planning
}  // namespace apollo