概述

PredictionQuerier类是apollo planning模块下modulesplanninglatticebehaviorprediction_querier.cc/.h实现

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

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

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

prediction_querier.h

#pragma once
#include <memory>
#include <string>
#include <unordered_map>
#include <vector>
#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

最后

以上就是秀丽钥匙为你收集整理的百度apollo自动驾驶planning代码学习-Apollo\modules\planning\lattice\behavior\PredictionQuerier类代码详解概述prediction_querier.hprediction_querier.cc的全部内容，希望文章能够帮你解决百度apollo自动驾驶planning代码学习-Apollo\modules\planning\lattice\behavior\PredictionQuerier类代码详解概述prediction_querier.hprediction_querier.cc所遇到的程序开发问题。

如果觉得靠谱客网站的内容还不错，欢迎将靠谱客网站推荐给程序员好友。