百度apollo自动驾驶planning代码学习-Apollo\modules\planning\lattice\behavior\PredictionQuerier类代码详解

概述

PredictionQuerier类是apollo planning模块下modules\planning\lattice\behavior\prediction_querier.cc/.h实现

从类名来看，应该是预测查询器类？

从代码来看PredictionQuerier类主要是实现：
1.储存障碍物id和障碍物对象的映射map;
2.获取类成员障碍物对象的接口；
3.将t时刻障碍物速度投影到类成员参考线上

主要就是将障碍物的预测速度投影到参考线上？就是在指定的纵向位置s处该障碍物速度在参考线的对应点方向上的投影。

prediction_querier.h

#pragma once

#include 
#include 
#include 
#include 

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

namespace apollo {
namespace planning {

class PredictionQuerier {
 public:
 //带参构造函数
//输入参数：障碍物对象列表obstacles
//输入参数：路径点数组类型参考线指针ptr_reference_line
//其实就是将输入参数用来初始化类成员？
  PredictionQuerier(const std::vector<const Obstacle*>& obstacles,
                    const std::shared_ptr<std::vector<common::PathPoint>>&
                        ptr_reference_line);
//默认析构函数
  virtual ~PredictionQuerier() = default;

//获取类成员障碍物列表obstacles_
  std::vector<const Obstacle*> GetObstacles() const;

//将t时刻障碍物速度投影到参考线上？
//输入参数:障碍物id obstacle_id
//输入参数：障碍物的s,投影时刻t
  double ProjectVelocityAlongReferenceLine(const std::string& obstacle_id,
                                           const double s,
                                           const double t) const;

 private:
  std::unordered_map<std::string, const Obstacle*> id_obstacle_map_;

  std::vector<const Obstacle*> obstacles_;

  std::shared_ptr<std::vector<common::PathPoint>> ptr_reference_line_;
};

}  // namespace planning
}  // namespace apollo

prediction_querier.cc

#include "modules/planning/lattice/behavior/prediction_querier.h"

#include "modules/common/math/linear_interpolation.h"
#include "modules/common/math/path_matcher.h"

namespace apollo {
namespace planning {

//带参构造函数
//输入参数：障碍物对象列表obstacles
//输入参数：路径点数组类型参考线指针ptr_reference_line
//其实就是将输入参数用来初始化类成员？
PredictionQuerier::PredictionQuerier(
    const std::vector<const Obstacle*>& obstacles,
    const std::shared_ptr<std::vector<common::PathPoint>>& ptr_reference_line)
    : ptr_reference_line_(ptr_reference_line) {
  //遍历输入的障碍物列表
  for (const auto ptr_obstacle : obstacles) {
    //如果类成员障碍物id和障碍物对象的映射map id_obstacle_map_里若没有这个障碍物就把它塞进去？
    if (common::util::InsertIfNotPresent(&id_obstacle_map_, ptr_obstacle->Id(),
                                         ptr_obstacle)) {
      obstacles_.push_back(ptr_obstacle);
    } else {
      AWARN << "Duplicated obstacle found [" << ptr_obstacle->Id() << "]";
    }
  }
}

//获取类成员障碍物列表obstacles_
std::vector<const Obstacle*> PredictionQuerier::GetObstacles() const {
  return obstacles_;
}

//t时刻障碍物速度投影到参考线上？
//输入参数:障碍物id obstacle_id
//输入参数：障碍物的s,投影时刻t
double PredictionQuerier::ProjectVelocityAlongReferenceLine(
    const std::string& obstacle_id, const double s, const double t) const {
  ACHECK(id_obstacle_map_.find(obstacle_id) != id_obstacle_map_.end());
//类成员id_obstacle_map_ id和障碍物对象的映射map
//获取输入的障碍物id对应的预测轨迹trajectory
  const auto& trajectory = id_obstacle_map_.at(obstacle_id)->Trajectory();
  //获取障碍物预测轨迹点数量num_traj_point 
  int num_traj_point = trajectory.trajectory_point_size();
  //如果障碍物的预测轨迹点的数量小于2，那么障碍物t时刻投影到参考线上的速度直接设置为0
  if (num_traj_point < 2) {
    return 0.0;
  }

//如果输入的时刻t，障碍物的预测轨迹并不包含输入的时刻t的话，那么障碍物t时刻投影到参考线上的速度直接设置为0
  if (t < trajectory.trajectory_point(0).relative_time() ||
      t > trajectory.trajectory_point(num_traj_point - 1).relative_time()) {
    return 0.0;
  }

//找到障碍物预测轨迹上的最接近输入时刻t的轨迹点
  auto matched_it =
      std::lower_bound(trajectory.trajectory_point().begin(),
                       trajectory.trajectory_point().end(), t,
                       [](const common::TrajectoryPoint& p, const double t) {
                         return p.relative_time() < t;
                       });
//获取障碍物预测轨迹上的最接近输入时刻t的轨迹点速度
  double v = matched_it->v();
 //获取障碍物预测轨迹上的最接近输入时刻t的轨迹点上的路径点的航向theta 
  double theta = matched_it->path_point().theta();
 //将障碍物的纵向速度投影到大地坐标的x,y轴上的分速度v_x ，v_y 
  double v_x = v * std::cos(theta);
  double v_y = v * std::sin(theta);

//找到障碍物对象在参考线s坐标处对应的路径点obstacle_point_on_ref_line 
  common::PathPoint obstacle_point_on_ref_line =
      common::math::PathMatcher::MatchToPath(*ptr_reference_line_, s);
  //获取障碍物对象在参考线s坐标处对应的路径点的航向
  auto ref_theta = obstacle_point_on_ref_line.theta();
//然后又将障碍物t时刻在大地XY轴对应的分速度再投影到参考线上(乘上参考线同一s坐标对应的路径点的theta的单位向量)
  return std::cos(ref_theta) * v_x + std::sin(ref_theta) * v_y;
}

}  // namespace planning
}  // namespace apollo