/******************************************************************************
* Copyright 2017 The Apollo Authors. All Rights Reserved.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*****************************************************************************/
/**
* @file dp_road_graph.cc
**/
#include "modules/planning/tasks/optimizers/road_graph/dp_road_graph.h"
#include "cyber/common/log.h"
#include "cyber/task/task.h"
#include "modules/common/configs/vehicle_config_helper.h"
#include "modules/common/math/cartesian_frenet_conversion.h"
#include "modules/common/proto/error_code.pb.h"
#include "modules/common/util/util.h"
#include "modules/map/hdmap/hdmap_util.h"
#include "modules/planning/common/path/frenet_frame_path.h"
#include "modules/planning/common/planning_context.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/math/curve1d/quintic_polynomial_curve1d.h"
#include "modules/planning/proto/planning_internal.pb.h"
#include "modules/planning/proto/planning_status.pb.h"
namespace apollo {
namespace planning {
DpRoadGraph::DpRoadGraph(const DpPolyPathConfig &config,
const ReferenceLineInfo &reference_line_info,
const SpeedData &speed_data)
: config_(config),
reference_line_info_(reference_line_info),
reference_line_(reference_line_info.reference_line()),
speed_data_(speed_data) {}
//借助GenerateMinCostPath得到代价最小路径 将每段最小代价curve以path_resolution为间隔离散化,结果存入path_data
//FindPathTunnel()的结果是依据若干level之间分段5次多项式采样点 保存在path_data.frenet_path(SL系)和path_data.discretized_path(XY系)中
bool DpRoadGraph::FindPathTunnel(const common::TrajectoryPoint &init_point,
const std::vector<const Obstacle *> &obstacles,
PathData *const path_data) {
CHECK_NOTNULL(path_data);
init_point_ = init_point;
if (!reference_line_.XYToSL(init_point_.path_point(), &init_sl_point_)) {
AERROR << "Fail to create init_sl_point from : "
<< init_point.DebugString();
return false;
}
//起始点所对应的在参考线上的点
init_frenet_frame_point_ =
reference_line_.GetFrenetPoint(init_point_.path_point()); //得到参考线上点的s值有计算笛卡尔坐标和ferenet坐标转换时,计算
waypoint_sampler_->Init(&reference_line_info_, init_sl_point_,
init_frenet_frame_point_);
waypoint_sampler_->SetDebugLogger(planning_debug_);
// Step B. 获取当前参考线下最优的前进路线
std::vector<DpRoadGraphNode> min_cost_path;
if (!GenerateMinCostPath(obstacles, &min_cost_path)) {
AERROR << "Fail to generate graph!";
return false;
}
std::vector<common::FrenetFramePoint> frenet_path;
double accumulated_s = min_cost_path.front().sl_point.s();
const double path_resolution = config_.path_resolution();
//如果是4个点 那么 i<4 i只能到3 cur_node[3]
for (size_t i = 1; i < min_cost_path.size(); ++i) { //min_cost_path是一个DpRoadGraphNode 结构体
const auto &prev_node = min_cost_path[i - 1]; //前点
const auto &cur_node = min_cost_path[i]; //现在的点
const double path_length = cur_node.sl_point.s() - prev_node.sl_point.s();
double current_s = 0.0; //s是每一个prev到cur之间的s
const auto &curve = cur_node.min_cost_curve; //现在该点下一时刻的最小cost曲线
//对每一段curve进行采样 current_s 以path_resolution的进行增加
while (current_s + path_resolution / 2.0 < path_length) {
//表示当前s向前移动一个分辨率步长的中点的长度,这个中点位置被用来判断当前位置是否已经接近路径的末端, 如果跳出循环说明距离不够一个分辨率
const double l = curve.Evaluate(0, current_s); //计算出轨迹在当前s的l至
const double dl = curve.Evaluate(1, current_s);
const double ddl = curve.Evaluate(2, current_s);
common::FrenetFramePoint frenet_frame_point;
frenet_frame_point.set_s(accumulated_s + current_s);
frenet_frame_point.set_l(l);
frenet_frame_point.set_dl(dl);
frenet_frame_point.set_ddl(ddl);
frenet_path.push_back(std::move(frenet_frame_point));
current_s += path_resolution;
}
if (i == min_cost_path.size() - 1) { //之前并没有计算累计距离 ,对于最后一个路径点距离应该是初始的s+现在的s 因为最后一段距离可能不够一定、
accumulated_s += current_s;
} else {
accumulated_s += path_length; //这个看整个for循环
}
}
path_data->SetReferenceLine(&reference_line_);
path_data->SetFrenetPath(FrenetFramePath(std::move(frenet_path)));
return true;
}
bool DpRoadGraph::GenerateMinCostPath(
const std::vector<const Obstacle *> &obstacles,
std::vector<DpRoadGraphNode> *min_cost_path) {
ACHECK(min_cost_path != nullptr);
//基于当前参考线及初始点,生成候选路径采样点数组
//路径航向(path_waypoints)里面的每个vector存储相同s值 即每个s时的横向采样
std::vector<std::vector<common::SLPoint>> path_waypoints;
if (!waypoint_sampler_->SamplePathWaypoints(init_point_, &path_waypoints) ||
path_waypoints.size() < 1) {
AERROR << "Fail to sample path waypoints! reference_line_length = "
<< reference_line_.Length();
return false;
}
const auto &vehicle_config =
common::VehicleConfigHelper::Instance()->GetConfig();
/*-------------------轨迹代价的初始化--------------------*/
TrajectoryCost trajectory_cost(
config_, reference_line_, reference_line_info_.IsChangeLanePath(),
obstacles, vehicle_config.vehicle_param(), speed_data_, init_sl_point_,
reference_line_info_.AdcSlBoundary());
/*--------最小代价值路图节表点链表---------*/
//储存构建的graph,速度DP过程用的是数据结构 vector<vector<>>, 路径这里是list< list<> > 每个node都保存了从前一列到该节点的最小cost
std::list<std::list<DpRoadGraphNode>> graph_nodes; //这是最后的前向遍历图
// find one point from first row 从第一个s下找一个点
const auto &first_row = path_waypoints.front(); //first_row = [l1,l2,l3,l4,l5] 第一列的点
size_t nearest_i = 0;
//在第一列中寻找距离初始点node最近的l点
for (size_t i = 1; i < first_row.size(); ++i) {
if (std::fabs(first_row[i].l() - init_sl_point_.l()) <
std::fabs(first_row[nearest_i].l() - init_sl_point_.l())) {
nearest_i = i;
}
}
graph_nodes.emplace_back();//空的 第0列
graph_nodes.back().emplace_back(first_row[nearest_i], nullptr,
ComparableCost()); //是在第0列中emplace也就是 加入第0列第0行
auto &front = graph_nodes.front().front(); //第0列第0行
size_t total_level = path_waypoints.size(); //总的列
//采用自下而上的动态规划算法,迭代更新最小代价值
//graph_nodes 储存的就是各级(level)路径航点(path_waypoints)所包含的最小代价航点
//graph_nodes.back() (即最后一个航点链表)就是我们所需的最小代价航点链表
//构建 graph_nodes
//两层循环 :
// 外循环 -- 撒点的列数;
// 内循环 -- 列中的每个点
//说白了 在for循环中可以看到的语句就是 遍历点 以及生成node message
for (size_t level = 1; level < path_waypoints.size(); ++level) { //path_waypoints vector<vector<>> 遍历路径点的各层
// graph_nodes list< list<> > 目前数据内部只含有 第一列距离初始位置最近的点
const auto &prev_dp_nodes = graph_nodes.back(); //上一层路径的最优路径点的集合
const auto &level_points = path_waypoints[level]; //获取当前层的路径航点
graph_nodes.emplace_back(); //作用是在list中插入空的列
std::vector<std::future<void>> results; //用future储存并行执行结果
//遍历当前层的所有航迹点
for (size_t i = 0; i < level_points.size(); ++i) { //每一列点的大小
//第level列第i行
const auto &cur_point = level_points[i]; //因为在这已经不太容易判断类型了 每一列的第一个点开始撒点
//因为graph_nodes是双list嵌套的 就是在列empalce_back 而类型是DpRoadGraphNode 详见其构造函数 可以看到需要传入参数的类型
//在外层空列中 插入当前node
graph_nodes.back().emplace_back(cur_point, nullptr);
auto msg = std::make_shared<RoadGraphMessage>( //作用就是记录下来当前node和前一个node下的cost
prev_dp_nodes, level, total_level, &trajectory_cost, &front, //front是上一个点
&(graph_nodes.back().back())); //graph_nodes.back
//采用多线程并行计算最小代价值行点
if (FLAGS_enable_multi_thread_in_dp_poly_path) {
results.emplace_back(cyber::Async(&DpRoadGraph::UpdateNode, this, msg));
} else {
/*-----------------------------------------------------------------------------------*/
/*----------计算前一层prev_dp_nodes和当前层的节点cur_node的开销cost,取prev_dp_nodes中与cur_node开销cost最小的节点,设置最优路径----------------------*/
/*-----------------------------------------------------------------------------------*/
UpdateNode(msg); //msg中的东西由上面赋值
}
}
if (FLAGS_enable_multi_thread_in_dp_poly_path) {
for (auto &result : results) {
result.get();
}
}
}
//上面计算出来了所有level中每一个node: 当前node与前一level中的最小cost
//以上只是把2node间的代价计算出来了,并没有寻找代价最小的路径
//选择最后一个level中,具有最小cost的node最为规划终点
// find best path
DpRoadGraphNode fake_head;
//这是有两层遍历
for (const auto &cur_dp_node : graph_nodes.back()) { //只遍历最后一列 cur_dp_node 是遍历列中的点
fake_head.UpdateCost(&cur_dp_node, cur_dp_node.min_cost_curve,
cur_dp_node.min_cost);
//选择最后一个level中,具有最小cost的node作为规划终点 记录下来最小cost的点
}
//在最后的level采样点中,寻找代价最小的节点,作为一次规划的终点,即fake_head的prev
//其实其他level的采样点很可能代价更小,这里为什么从最后的level选呢?
//因为规划的长度和时间越长越好,规划越长,相当于自车看到的越远
//根据父指针向前遍历,得到至后向前的一条路径
const auto *min_cost_node = &fake_head; //倒数第二列到最后一列的最小cost
//Todo:min_cost_path没有把fake_head保存进去,fake_head->min_cost_prev_node才是真正的终点
while (min_cost_node->min_cost_prev_node) {
min_cost_node = min_cost_node->min_cost_prev_node;
min_cost_path->push_back(*min_cost_node);
}
if (min_cost_node != &graph_nodes.front().front()) {
return false;
}
std::reverse(min_cost_path->begin(), min_cost_path->end());
for (const auto &node : *min_cost_path) {
ADEBUG << "min_cost_path: " << node.sl_point.ShortDebugString();
planning_debug_->mutable_planning_data()
->mutable_dp_poly_graph()
->add_min_cost_point()
->CopyFrom(node.sl_point);
}
return true;
}
//遍历前一蹭的node 计算出 从初始点到当前node的最小cost
void DpRoadGraph::UpdateNode(const std::shared_ptr<RoadGraphMessage> &msg) {
CHECK_NOTNULL(msg);
CHECK_NOTNULL(msg->trajectory_cost);
CHECK_NOTNULL(msg->front);
CHECK_NOTNULL(msg->cur_node);
for (const auto &prev_dp_node : msg->prev_nodes) { //对上一列的node进行遍历
const auto &prev_sl_point = prev_dp_node.sl_point; //上一列遍历点的sl
const auto &cur_point = msg->cur_node->sl_point; //本个遍历点的
double init_dl = 0.0;
double init_ddl = 0.0;
if (msg->level == 1) { //
init_dl = init_frenet_frame_point_.dl(); //因为第一列没有dl和ddl
init_ddl = init_frenet_frame_point_.ddl();
}
QuinticPolynomialCurve1d curve(prev_sl_point.l(), init_dl, init_ddl,
cur_point.l(), 0.0, 0.0,
cur_point.s() - prev_sl_point.s()); //由l,s值拟合五次多项式
if (!IsValidCurve(curve)) {
continue;
}
const auto cost =
msg->trajectory_cost->Calculate(curve, prev_sl_point.s(), cur_point.s(), //通过调用函数来计算cost
msg->level, msg->total_level) + //为什么要传入level和total_level 因为最后一列时要保证尽量的接近参考线
prev_dp_node.min_cost;
//cost = 现在两列node之间的cost + 之前 因为每个node中,都保存了,从规划起始点到该节点的最小cost 所以 要加上之前的min_cost
//更新前一个点与现在点的最小cost,并记录下来前一个点的位置,和上边拟合的曲线数据
msg->cur_node->UpdateCost(&prev_dp_node, curve, cost); //曲线,该曲线的cost
}
// try to connect the current point with the first point directly 尝试用5次多项式拟合第一个点与当前node
if (reference_line_info_.IsChangeLanePath() && msg->level >= 2) { //如果 大于第二列并且要变道
const double init_dl = init_frenet_frame_point_.dl();
const double init_ddl = init_frenet_frame_point_.ddl();
QuinticPolynomialCurve1d curve(
init_sl_point_.l(), init_dl, init_ddl, msg->cur_node->sl_point.l(), 0.0,
0.0, msg->cur_node->sl_point.s() - init_sl_point_.s());
if (!IsValidCurve(curve)) {
return;
}
const auto cost = msg->trajectory_cost->Calculate(
curve, init_sl_point_.s(), msg->cur_node->sl_point.s(), msg->level,
msg->total_level);
msg->cur_node->UpdateCost(msg->front, curve, cost);
}
}
bool DpRoadGraph::IsValidCurve(const QuinticPolynomialCurve1d &curve) const {
static constexpr double kMaxLateralDistance = 20.0;
for (double s = 0.0; s < curve.ParamLength(); s += 2.0) {
const double l = curve.Evaluate(0, s);
if (std::fabs(l) > kMaxLateralDistance) {
return false;
}
}
return true;
}
void DpRoadGraph::GetCurveCost(TrajectoryCost trajectory_cost,
const QuinticPolynomialCurve1d &curve,
const double start_s, const double end_s,
const uint32_t curr_level,
const uint32_t total_level,
ComparableCost *cost) {
*cost =
trajectory_cost.Calculate(curve, start_s, end_s, curr_level, total_level);
}
} // namespace planning
} // namespace apollo
apollo DP dp_road_graph.cc代码注释
于 2023-03-22 16:14:24 首次发布
DpRoadGraph类用于在Apollo自动驾驶系统中进行路径规划。它基于动态规划算法,找到从起点到终点的最小代价路径。首先,根据参考线和初始点采样路径点,然后通过计算每个点与前一个点之间的最小成本曲线,构建一个代价最小的路径。最后,使用多线程并行计算更新节点的成本,并找出整个路径中的最小成本路径。该过程涉及曲线拟合、障碍物避障和路径约束的考虑。
摘要由CSDN通过智能技术生成