百度apollo planning-Apollo\modules\planning\lattice\trajectory_generation\BackupTrajectoryGenerator类详解

概述

BackupTrajectoryGenerator类是apollo planning模块下modules\planning\lattice\trajectory_generation\backup_trajectory_generator.cc/.h实现

从类名来看，应该是备选轨迹生成器类？主要是生成减速时的轨迹？

从代码来看BackupTrajectoryGenerator类主要是实现：
1.储存车辆初始的横纵向位置，相对时间，碰撞检查对象等？；
2.产生一系列的减速的横纵向一维轨迹，组成(纵向，横向）轨迹对存入类成员中作为候选轨迹集；
3.找到2中候选轨迹集里的终端速度最大且无碰撞的轨迹，生成DiscretizedTrajectory 离散轨迹类对象；

*State类数据类型是一个三个元素的数组
State init_s 就是(s,ds,dds)
State init_d就是(L,dL,ddL)

backup_trajectory_generator.h

#pragma once

#include <functional>
#include <memory>
#include <queue>
#include <utility>
#include <vector>

#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/common/trajectory/discretized_trajectory.h"
#include "modules/planning/common/trajectory1d/constant_deceleration_trajectory1d.h"
#include "modules/planning/constraint_checker/collision_checker.h"
#include "modules/planning/lattice/trajectory_generation/trajectory1d_generator.h"
#include "modules/planning/math/curve1d/curve1d.h"

namespace apollo {
namespace planning {

class BackupTrajectoryGenerator {
 public:
 //这个pair类型，由两个一维轨迹类指针(纵向，横向)成对构成，换个别名Trajectory1dPair;
  typedef std::pair<std::shared_ptr<Curve1d>, std::shared_ptr<Curve1d>>
      Trajectory1dPair;
  //每个Trajectory1dPair对象及其cost成对构成PairCost类型
  typedef std::pair<Trajectory1dPair, double> PairCost;
  
//类带参构造函数
//输入参数：init_s初始相对于参考线的s纵坐标? 都是3维数组？
//输入参数：init_d初始相对于参考线的d横向坐标？都是3维数组？
//输入参数：init_relative_time初始的相对时间
//输入参数：ptr_collision_checker碰撞检查器指针
//输入参数：trajectory1d_generator1维轨迹生成器
//这个函数就是把输入的参数全部赋值给类成员进行初始化
  BackupTrajectoryGenerator(
      const std::array<double, 3>& init_s, const std::array<double, 3>& init_d,
      const double init_relative_time,
      const std::shared_ptr<CollisionChecker>& ptr_collision_checker,
      const Trajectory1dGenerator* trajectory1d_generator);

//生成轨迹函数？
//输入参数：离散的参考点序列discretized_ref_points
//std::priority_queue优先队列
//std::priority_queue:在优先队列中，优先级高的元素先出队列，并非按照先进先出的要求，类似一个堆(heap)。其模板声明带有三个参数，priority_queue<Type, Container, Functional>, 其中Type为数据类型，Container为保存数据的容器，Functional为元素比较方式
/*std::priority_queue<Trajectory1dPair, std::vector<Trajectory1dPair>,
                      CostComparator>
      trajectory_pair_pqueue_;类成员*/
//这个函数是找到类成员trajectory_pair_pqueue_备选轨迹集中，无碰撞且终端速度最快的轨迹？
  DiscretizedTrajectory GenerateTrajectory(
      const std::vector<common::PathPoint>& discretized_ref_points);

 private:
 //就是根据初始的横纵向坐标依据不同的预期减速度，以及不同的横向偏移类似于撒点生成一些列备选的轨迹集，不同的横向偏移生成1维横向轨迹不在该类中实现，所以具体原理先搁置，涉及到再解析。产生的横纵向轨迹组轨迹对储存到类成员trajectory_pair_pqueue_中
  void GenerateTrajectory1dPairs(const std::array<double, 3>& init_s,
                                 const std::array<double, 3>& init_d);

  double init_relative_time_;

  std::shared_ptr<CollisionChecker> ptr_collision_checker_;

  const Trajectory1dGenerator* ptr_trajectory1d_generator_;

//这里头文件里定义了一个cost代价比较函数？其实就是用来对成员优先队列trajectory_pair_pqueue_(备选轨迹集)进行排序，终端速度大的排在前面先出
  struct CostComparator
      : public std::binary_function<const Trajectory1dPair&,
                                    const Trajectory1dPair&, bool> {
    bool operator()(const Trajectory1dPair& left,
                    const Trajectory1dPair& right) const {
      auto lon_left = left.first;
      auto lon_right = right.first;
      auto s_dot_left = lon_left->Evaluate(1, FLAGS_trajectory_time_length);
      auto s_dot_right = lon_right->Evaluate(1, FLAGS_trajectory_time_length);
      if (s_dot_left < s_dot_right) {
        return true;
      }
      return false;
    }
  };

  std::priority_queue<Trajectory1dPair, std::vector<Trajectory1dPair>,
                      CostComparator>
      trajectory_pair_pqueue_;
};

}  // namespace planning
}  // namespace apollo

backup_trajectory_generator.cc

#include "modules/planning/lattice/trajectory_generation/backup_trajectory_generator.h"

#include "modules/planning/lattice/trajectory_generation/trajectory_combiner.h"

namespace apollo {
namespace planning {

using apollo::common::PathPoint;
using State = std::array<double, 3>;

//类带参构造函数
//输入参数：init_s初始相对于参考线的s纵坐标? 都是3维数组？
//输入参数：init_d初始相对于参考线的d横向坐标？都是3维数组？
//输入参数：init_relative_time初始的相对时间
//输入参数：ptr_collision_checker碰撞检查器指针
//输入参数：trajectory1d_generator1维轨迹生成器
//这个函数就是把输入的参数全部赋值给类成员进行初始化
BackupTrajectoryGenerator::BackupTrajectoryGenerator(
    const State& init_s, const State& init_d, const double init_relative_time,
    const std::shared_ptr<CollisionChecker>& ptr_collision_checker,
    const Trajectory1dGenerator* trajectory1d_generator)
    : init_relative_time_(init_relative_time),
      ptr_collision_checker_(ptr_collision_checker),
      ptr_trajectory1d_generator_(trajectory1d_generator) {
  GenerateTrajectory1dPairs(init_s, init_d);
}

//产生1维轨迹的SD坐标点对
//就是根据初始的横纵向坐标依据不同的预期减速度，以及不同的横向偏移类似于撒点生成一些列备选的轨迹集，不同的横向偏移生成1维横向轨迹不在该类中实现，所以具体原理先搁置，涉及到再解析。产生的横纵向轨迹组轨迹对储存到类成员trajectory_pair_pqueue_中
void BackupTrajectoryGenerator::GenerateTrajectory1dPairs(const State& init_s,
                                                          const State& init_d) {
  //纵向轨迹点？储存s坐标的序列？
  std::vector<std::shared_ptr<Curve1d>> lon_trajectories;
  //s的二阶候选序列(加速度？){-0.1, -1.0, -2.0, -3.0, -4.0}
  std::array<double, 5> dds_condidates = {-0.1, -1.0, -2.0, -3.0, -4.0};
  //遍历这个s的二阶候选序列，按照上面的5个固定减速度分别取产生5条匀减速纵向轨迹
  for (const auto dds : dds_condidates) {
    lon_trajectories.emplace_back(
        new ConstantDecelerationTrajectory1d(init_s[0], init_s[1], dds));
  }

//横向轨迹点，储存横向的坐标序列
  std::vector<std::shared_ptr<Curve1d>> lat_trajectories;
  //产生横向的轨迹包？放入lat_trajectories,至于如何产生横向轨迹集参见modules\planning\lattice\trajectory_generation\trajectory1d_generator.cc，解析到那个源文件再进行原理学习。总之就是沿着初始的s坐标向前60m确定横向的偏移，按照纵向每隔1m进行采样
  ptr_trajectory1d_generator_->GenerateLateralTrajectoryBundle(
      &lat_trajectories);

//将这些横纵向轨迹进行一一组合，组合成{{{s0,L0},...}}多条备选的横纵向轨迹集？
  for (auto& lon : lon_trajectories) {
    for (auto& lat : lat_trajectories) {
      trajectory_pair_pqueue_.emplace(lon, lat);
    }
  }
}

//生成轨迹函数？
//输入参数：离散的参考点序列discretized_ref_points
//std::priority_queue优先队列
//std::priority_queue:在优先队列中，优先级高的元素先出队列，并非按照先进先出的要求，类似一个堆(heap)。其模板声明带有三个参数，priority_queue<Type, Container, Functional>, 其中Type为数据类型，Container为保存数据的容器，Functional为元素比较方式
/*std::priority_queue<Trajectory1dPair, std::vector<Trajectory1dPair>,
                      CostComparator>
      trajectory_pair_pqueue_;类成员*/
//这个函数是找到类成员trajectory_pair_pqueue_备选轨迹集中，无碰撞且终端速度最快的轨迹？
DiscretizedTrajectory BackupTrajectoryGenerator::GenerateTrajectory(
    const std::vector<PathPoint>& discretized_ref_points) {
  //trajectory_pair_pqueue_是横纵向轨迹的组合对队列
  //该优先队列优先级高的先出，优先级比较函数在modules\planning\lattice\trajectory_generation\backup_trajectory_generator.h中被定义CostComparator(),大致就是按照轨迹对里纵向轨迹的末端速度越大优先级越高
  while (trajectory_pair_pqueue_.size() > 1) {
    //取出候选轨迹集trajectory_pair_pqueue_中纵向末端速度最大的那一个
    auto top_pair = trajectory_pair_pqueue_.top();
    trajectory_pair_pqueue_.pop();
    
    //结合输入离散的路径点，纵向轨迹，横向轨迹，初始的相对时间合并成离散轨迹对象DiscretizedTrajectory类对象
    DiscretizedTrajectory trajectory =
        TrajectoryCombiner::Combine(discretized_ref_points, *top_pair.first,
                                    *top_pair.second, init_relative_time_);
    //对轨迹进行碰撞检查，若无碰撞则直接返回
    //意思就是找到轨迹候选集中最快的无碰轨迹
    if (!ptr_collision_checker_->InCollision(trajectory)) {
      return trajectory;
    }
  }
  //如果遍历完都没有找到无碰的轨迹，就返回优先级最高也就是速度最快的那个？
  auto top_pair = trajectory_pair_pqueue_.top();
  return TrajectoryCombiner::Combine(discretized_ref_points, *top_pair.first,
                                     *top_pair.second, init_relative_time_);
}

}  // namespace planning
}  // namespace apollo