看一下 dp path 部分的实现
一. dp_poly_path_optimizer
主要在 dp_poly_path_optimizer 这个文件中,在这个路径下
apollo/modules/planning/tasks/optimizers/dp_poly_path/dp_poly_path_optimizer.h
- dp_poly_path_optimizer.h
class DpPolyPathOptimizer : public PathOptimizer {
public:
explicit DpPolyPathOptimizer(const TaskConfig &config);
private:
apollo::common::Status Process(const SpeedData &speed_data,
const ReferenceLine &reference_line,
const common::TrajectoryPoint &init_point,
PathData *const path_data) override;
};
分析:
- 首先
DpPolyPathOptimizer
这个类,继承自PathOptimizer
- 构造函数 传入了 一个
TaskConfig
类型的变量,初始化 - 主要的一个函数
Process()
下面依次来看一下
这里有两个关键字 explicit 和 override,顺便看一下用法:
- explicit
构造的时候必须显示构造,举个例子
DpPolyPathOptimizer dp_path_opt(config); //编译通过
DpPolyPathOptimizer dp_path_opt = config; //编译不通过,不允许隐式的转换
- override
如果派生类在虚函数声明时使用了override描述符,那么该函数必须重载其基类中的同名函数,否则代码将无法通过编译
1. PathOptimizer 类
class PathOptimizer : public Task {
public:
explicit PathOptimizer(const TaskConfig &config);
virtual ~PathOptimizer() = default;
apollo::common::Status Execute(
Frame *frame, ReferenceLineInfo *reference_line_info) override;
protected:
virtual apollo::common::Status Process(
const SpeedData &speed_data, const ReferenceLine &reference_line,
const common::TrajectoryPoint &init_point, PathData *const path_data) = 0;
void RecordDebugInfo(const PathData &path_data);
};
比较重要的:
- 继承自
Task
这个类 - 构造函数也是 传入
TaskConfig
类型的变量,初始化 Execute
函数,传入 Frame*, ReferenceLineInfo* 在这里重写Process()
纯虚函数, 在子类中重写
先放结论:
- Task 类提供两个虚函数 Init、Execute,其中Init 函数用于任务对象的初始化,Task 类目前未做任何实际工作
Execute用于执行实际的优化,Task类也未实现任何功能 - PathOptimizer 类重载了 Task 类的虚函数Execute,同时提供一个保护的纯虚函数 Process(由派生类具体实现),在 Execute 内部调用 Process函数完成实际的优化
- 注意:PathOptimizer是虚基类,不可直接使用它创建类对象,必须从该类派生出具体类后方可实例化
那还是,一个一个分析吧
(1). Task 类
class Task {
public:
explicit Task(const TaskConfig& config);
virtual ~Task() = default;
void SetName(const std::string& name) { name_ = name; }
const std::string& Name() const;
const TaskConfig& Config() const { return config_; }
virtual apollo::common::Status Execute(
Frame* frame, ReferenceLineInfo* reference_line_info);
protected:
Frame* frame_ = nullptr;
ReferenceLineInfo* reference_line_info_ = nullptr;
TaskConfig config_;
std::string name_;
};
Task::Task(const TaskConfig& config) : config_(config) {
name_ = TaskConfig::TaskType_Name(config_.task_type());
}
const std::string& Task::Name() const { return name_; }
Status Task::Execute(Frame* frame, ReferenceLineInfo* reference_line_info) {
frame_ = frame;
reference_line_info_ = reference_line_info;
return Status::OK();
}
分析:
这个就是最上层的基类了
- 构造函数 传入 config, 设置这个 config 的 name_
Execute()
函数 ,输入Frame
类型 和ReferenceLineInfo
类型的指针 初始化
到这里 这个 Task 类,基本整明白了,还有几个小问题:
- TaskConfig 类型是什么
- Frame 类型是什么
- ReferenceLineInfo 类型是什么
那就接着往下看
a. TaskConfig
首先要知道 在哪里定义的
看 task.h
文件,应该是在
#include "modules/planning/proto/planning_config.pb.h"
这里定义的,但是仔细找是没有这个文件的
因为这个 planning_config.pb.h
文件是通过
apollo/modules/planning/proto/planning_config.proto
这个 proto 文件生成的,具体怎么生成的,可以看这里: apollo .proto 文件 使用
ok,那我们看一下 planning_config.proto,里面关于 TaskConfig 的定义
message TaskConfig {
enum TaskType {
DP_POLY_PATH_OPTIMIZER = 0;
DP_ST_SPEED_OPTIMIZER = 1;
QP_SPLINE_PATH_OPTIMIZER = 2;
......
};
optional TaskType task_type = 1;
oneof task_config {
DpPolyPathConfig dp_poly_path_config = 2;
DpStSpeedConfig dp_st_speed_config = 3;
......
}
}
可以看到实际上
TaskConfig 是由 TaskType + task_config 组成的,
即 具体 task 的名称 + 该 task 的相关配置
举个栗子:
TaskConfig mission;
mission->set_TaskType(DP_POLY_PATH_OPTIMIZER);
mission->set_dp_poly_path_config(...)
b. Frame
TODO
先马着吧,我觉得我能看一天,有点多…
-
Frame类
包含了一次Planning计算循环中的所有信息。
c. ReferenceLineInfo
TODO
也先马着吧 …
在Planning模块的每个计算循环中,会先生成ReferencePath,然后在这个基础上进行后面的处理。
例如:把障碍物投影到参考线上。
在ReferenceLine之外,在common目录下还有一个结构:ReferenceLineInfo,这个结构才是各个模块实际用到数据结构,它其中包含了ReferenceLine,但还有其他更详细的数据。
ReferenceLineInfo不仅仅包含了参考线信息,还包含了车辆状态,路径信息,速度信息,决策信息以及轨迹信息等。Planning模块的算法很多都是基于ReferenceLineInfo结构完成的
- Smoother
为了保证车辆轨迹的平顺,参考线必须是经过平滑的,目前Apollo中包含了这么几个Smoother用来做参考线的平滑:
(2). PathOptimizer::Execute(…)
apollo::common::Status PathOptimizer::Execute(
Frame* frame, ReferenceLineInfo* const reference_line_info) {
Task::Execute(frame, reference_line_info);
auto ret = Process(
reference_line_info->speed_data(), reference_line_info->reference_line(),
frame->PlanningStartPoint(), reference_line_info->mutable_path_data());
RecordDebugInfo(reference_line_info->path_data());
if (ret != Status::OK()) {
reference_line_info->SetDrivable(false);
AERROR << "Reference Line " << reference_line_info->Lanes().Id()
<< " is not drivable after " << Name();
}
return ret;
}
可以看到,实际上还是调用 Process 函数完成实际的优化
2. DpPolyPathOptimizer::Process(…)
Status DpPolyPathOptimizer::Process(const SpeedData &speed_data,
const ReferenceLine &,
const common::TrajectoryPoint &init_point,
PathData *const path_data) {
CHECK_NOTNULL(path_data);
const auto &dp_poly_path_config = config_.dp_poly_path_config();
DpRoadGraph dp_road_graph(dp_poly_path_config, *reference_line_info_,
speed_data);
dp_road_graph.SetDebugLogger(reference_line_info_->mutable_debug());
dp_road_graph.SetWaypointSampler(
new WaypointSampler(dp_poly_path_config.waypoint_sampler_config()));
if (!dp_road_graph.FindPathTunnel(
init_point,
reference_line_info_->path_decision()->obstacles().Items(),
path_data)) {
AERROR << "Failed to find tunnel in road graph";
return Status(ErrorCode::PLANNING_ERROR, "dp_road_graph path generation");
}
return Status::OK();
}
写的很清楚了,输入:
- SpeedData
- ReferenceLine
- TrajectoryPoint
- PathData
读取 config 的配置,设置 dp path 的参数,然后具体的实现交给 DpRoadGraph 这个类
需要值得注意的点:
- 输入中的 SpeedData ,ReferenceLine, ReferenceLine,PathData 具体类型
DpRoadGraph
这个类
还是仔细分析一下:
(1). 输入数据类型
a. SpeedData
看一下定义
class SpeedData : public std::vector<common::SpeedPoint> {
public:
SpeedData() = default;
explicit SpeedData(std::vector<common::SpeedPoint> speed_points);
void AppendSpeedPoint(const double s, const double time, const double v,
const double a, const double da);
bool EvaluateByTime(const double time,
common::SpeedPoint* const speed_point) const;
double TotalTime() const;
virtual std::string DebugString() const;
};
- 继承自
std::vector<common::SpeedPoint>
看一下 SpeedPoint
是包括在:
#include "modules/common/proto/pnc_point.pb.h"
这个头文件中的,定义在:
apollo/modules/common/proto/pnc_point.proto
message SpeedPoint {
optional double s = 1;
optional double t = 2;
// speed (m/s)
optional double v = 3;
// acceleration (m/s^2)
optional double a = 4;
// jerk (m/s^3)
optional double da = 5;
}
- 构造:传入 vector speed_points 初始化(会按照时间排序)
SpeedData::SpeedData(std::vector<SpeedPoint> speed_points)
: std::vector<SpeedPoint>(std::move(speed_points)) {
std::sort(begin(), end(), [](const SpeedPoint& p1, const SpeedPoint& p2) {
return p1.t() < p2.t();
});
}
- EvaluateByTime(), 通过 对应的时间 t, 查找速度信息(线性插值)
b. ReferenceLine
见上
c. TrajectoryPoint
也是定义在
apollo/modules/common/proto/pnc_point.proto
message TrajectoryPoint {
// path point
optional PathPoint path_point = 1;
// linear velocity
optional double v = 2; // in [m/s]
// linear acceleration
optional double a = 3;
// relative time from beginning of the trajectory
optional double relative_time = 4;
// longitudinal jerk
optional double da = 5;
// The angle between vehicle front wheel and vehicle longitudinal axis
optional double steer = 6;
}
message PathPoint {
// coordinates
optional double x = 1;
optional double y = 2;
optional double z = 3;
// direction on the x-y plane
optional double theta = 4;
// curvature on the x-y planning
optional double kappa = 5;
// accumulated distance from beginning of the path
optional double s = 6;
// derivative of kappa w.r.t s.
optional double dkappa = 7;
// derivative of derivative of kappa w.r.t s.
optional double ddkappa = 8;
// The lane ID where the path point is on
optional string lane_id = 9;
// derivative of x and y w.r.t parametric parameter t in CosThetareferenceline
optional double x_derivative = 10;
optional double y_derivative = 11;
}
d. PathData
在这里:
apollo/modules/planning/common/path/path_data.h
TODO
把这部分抽出来,详细说
(2). DpRoadGraph
在这里:
apollo/modules/planning/tasks/optimizers/road_graph/dp_road_graph.h
看一下:
class DpRoadGraph {
public:
explicit DpRoadGraph(const DpPolyPathConfig &config,
const ReferenceLineInfo &reference_line_info,
const SpeedData &speed_data);
~DpRoadGraph() = default;
bool FindPathTunnel(const common::TrajectoryPoint &init_point,
const std::vector<const Obstacle *> &obstacles,
PathData *const path_data);
......
void SetWaypointSampler(WaypointSampler *waypoint_sampler) {
waypoint_sampler_.reset(waypoint_sampler);
}
private:
......
private:
DpPolyPathConfig config_;
common::TrajectoryPoint init_point_;
const ReferenceLineInfo &reference_line_info_;
const ReferenceLine &reference_line_;
SpeedData speed_data_;
common::SLPoint init_sl_point_;
common::FrenetFramePoint init_frenet_frame_point_;
apollo::planning_internal::Debug *planning_debug_ = nullptr;
ObjectSidePass sidepass_;
std::unique_ptr<WaypointSampler> waypoint_sampler_;
};
构造函数很简单,两个主要的函数 SetWaypointSampler
和 FindPathTunnel
,分别实现采样和dp
- 采样主要由 WaypointSampler 这个类实现
a. WaypointSampler::SamplePathWaypoints(…)
class WaypointSampler {
public:
explicit WaypointSampler(const WaypointSamplerConfig &config)
: config_(config) {}
~WaypointSampler() = default;
virtual void Init(const ReferenceLineInfo *reference_line_info,
const common::SLPoint &init_sl_point_,
const common::FrenetFramePoint &init_frenet_frame_point);
......
virtual bool SamplePathWaypoints(
const common::TrajectoryPoint &init_point,
std::vector<std::vector<common::SLPoint>> *const points);
protected:
const WaypointSamplerConfig &config_;
const ReferenceLineInfo *reference_line_info_ = nullptr;
common::SLPoint init_sl_point_;
common::FrenetFramePoint init_frenet_frame_point_;
apollo::planning_internal::Debug *planning_debug_ = nullptr;
ObjectSidePass sidepass_;
};
- 仔细看一下 SamplePathWaypoints(…) 这个函数
函数的输入:
bool WaypointSampler::SamplePathWaypoints(
const common::TrajectoryPoint &init_point,
std::vector<std::vector<common::SLPoint>> *const points) {
}
- TrajectoryPoint 前面已经说过了
- SLPoint
①. SLPoint
定义的路径:
apollo/modules/common/proto/pnc_point.proto
message SLPoint {
optional double s = 1;
optional double l = 2;
}
然后仔细看一下采样的函数
bool WaypointSampler::SamplePathWaypoints(
const common::TrajectoryPoint &init_point,
std::vector<std::vector<common::SLPoint>> *const points) {
CHECK_NOTNULL(points);
points->clear();
points->insert(points->begin(), std::vector<common::SLPoint>{init_sl_point_});
const double kMinSampleDistance = 40.0;
const double total_length = std::fmin(
init_sl_point_.s() + std::fmax(init_point.v() * 8.0, kMinSampleDistance),
reference_line_info_->reference_line().Length());
const auto &vehicle_config =
common::VehicleConfigHelper::Instance()->GetConfig();
const double half_adc_width = vehicle_config.vehicle_param().width() / 2.0;
// 每步采样点数
const double num_sample_per_level =
FLAGS_use_navigation_mode ? config_.navigator_sample_num_each_level()
: config_.sample_points_num_each_level();
constexpr double kSamplePointLookForwardTime = 4.0;
const double level_distance =
common::math::Clamp(init_point.v() * kSamplePointLookForwardTime,
config_.step_length_min(), config_.step_length_max());
double accumulated_s = init_sl_point_.s();
double prev_s = accumulated_s;
auto *status = PlanningContext::MutablePlanningStatus();
if (!status->has_pull_over() && status->pull_over().in_pull_over()) {
status->mutable_pull_over()->set_status(PullOverStatus::IN_OPERATION);
const auto &start_point = status->pull_over().start_point();
SLPoint start_point_sl;
if (!reference_line_info_->reference_line().XYToSL(start_point,
&start_point_sl)) {
AERROR << "Fail to change xy to sl.";
return false;
}
if (init_sl_point_.s() > start_point_sl.s()) {
const auto &stop_point = status->pull_over().stop_point();
SLPoint stop_point_sl;
if (!reference_line_info_->reference_line().XYToSL(stop_point,
&stop_point_sl)) {
AERROR << "Fail to change xy to sl.";
return false;
}
std::vector<common::SLPoint> level_points(1, stop_point_sl);
points->emplace_back(level_points);
return true;
}
}
constexpr size_t kNumLevel = 3;
for (size_t i = 0; i < kNumLevel && accumulated_s < total_length; ++i) {
accumulated_s += level_distance;
if (accumulated_s + level_distance / 2.0 > total_length) {
accumulated_s = total_length;
}
const double s = std::fmin(accumulated_s, total_length);
// 最小允许采样步长
constexpr double kMinAllowedSampleStep = 1.0;
// 若本次轨迹弧长与上次轨迹弧长间的差值小于最小允许采样步长,跳过本次采样
if (std::fabs(s - prev_s) < kMinAllowedSampleStep) {
continue;
}
prev_s = s;
double left_width = 0.0;
double right_width = 0.0;
reference_line_info_->reference_line().GetLaneWidth(s, &left_width,
&right_width);
// 边界缓冲
constexpr double kBoundaryBuff = 0.20;
// 右侧允许宽度 = 右车道宽 - 半车宽 - 边界缓冲
const double eff_right_width = right_width - half_adc_width - kBoundaryBuff;
// 左侧允许宽度 = 左车道宽 - 半车宽 - 边界缓冲
const double eff_left_width = left_width - half_adc_width - kBoundaryBuff;
// the heuristic shift of L for lane change scenarios
const double delta_dl = 1.2 / 20.0;
const double kChangeLaneDeltaL = common::math::Clamp(
level_distance * (std::fabs(init_frenet_frame_point_.dl()) + delta_dl),
1.2, 3.5);
// 每个位置上横向采样时,采样点之间的距离,差不多0.2m //
double kDefaultUnitL = kChangeLaneDeltaL / (num_sample_per_level - 1);
if (reference_line_info_->IsChangeLanePath() &&
!reference_line_info_->IsSafeToChangeLane()) {
kDefaultUnitL = 1.0;
}
// 横向采样的宽度
const double sample_l_range = kDefaultUnitL * (num_sample_per_level - 1);
// 右采样边界(车辆右侧为负值)
double sample_right_boundary = -eff_right_width;
// 左采样边界(车辆左侧为正值)
double sample_left_boundary = eff_left_width;
constexpr double kLargeDeviationL = 1.75;
constexpr double kTwentyMilesPerHour = 8.94;
// 参考线正在改变车道时
if (reference_line_info_->IsChangeLanePath() ||
std::fabs(init_sl_point_.l()) > kLargeDeviationL) {
if (EgoInfo::Instance()->start_point().v() > kTwentyMilesPerHour) {
sample_right_boundary = std::fmin(-eff_right_width, init_sl_point_.l());
sample_left_boundary = std::fmax(eff_left_width, init_sl_point_.l());
if (init_sl_point_.l() > eff_left_width) {
sample_right_boundary = std::fmax(
sample_right_boundary, init_sl_point_.l() - sample_l_range);
}
if (init_sl_point_.l() < eff_right_width) {
sample_left_boundary = std::fmin(sample_left_boundary,
init_sl_point_.l() + sample_l_range);
}
}
}
// 横向采样距离数组
std::vector<double> sample_l;
// 参考线正在改变车道且改变车道不安全时,将当前参考线到其他参考线的偏移值存储到横向采样距离数组
if (reference_line_info_->IsChangeLanePath() &&
!reference_line_info_->IsSafeToChangeLane()) {
sample_l.push_back(reference_line_info_->OffsetToOtherReferenceLine());
} else {
// 其他情形,从右采样边界到左采样边界,按照每步采样点数进行均匀采样,并将结果存储到横向采样距离数组
common::util::uniform_slice(
sample_right_boundary, sample_left_boundary,
static_cast<uint32_t>(num_sample_per_level - 1), &sample_l);
}
// 本次采样点数组
std::vector<common::SLPoint> level_points;
planning_internal::SampleLayerDebug sample_layer_debug;
for (size_t j = 0; j < sample_l.size(); ++j) {
common::SLPoint sl = common::util::MakeSLPoint(s, sample_l[j]);
sample_layer_debug.add_sl_point()->CopyFrom(sl);
level_points.push_back(std::move(sl));
}
if (!level_points.empty()) {
planning_debug_->mutable_planning_data()
->mutable_dp_poly_graph()
->add_sample_layer()
->CopyFrom(sample_layer_debug);
points->emplace_back(level_points);
}
}
return true;
}
不得不说很冗长…
TODO 仔细分析
b. DpRoadGraph::FindPathTunnel(…) [detail]
①. TODO 仔细分析
FindPathTunnel() 主要分为3部分:
先设置相关前提条件,
然后查找代价最小路径,
最后对每段代价最小路径采样以构造FrenetFramePath类的实例,并存入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().x(), init_point_.path_point().y()},
&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());
waypoint_sampler_->Init(&reference_line_info_, init_sl_point_,
init_frenet_frame_point_);
waypoint_sampler_->SetDebugLogger(planning_debug_);
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();
for (size_t i = 1; i < min_cost_path.size(); ++i) {
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;
const auto &curve = cur_node.min_cost_curve;
while (current_s + path_resolution / 2.0 < path_length) {
const double l = curve.Evaluate(0, current_s);
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) {
accumulated_s += current_s;
} else {
accumulated_s += path_length;
}
}
FrenetFramePath tunnel(frenet_path);
path_data->SetReferenceLine(&reference_line_);
path_data->SetFrenetPath(tunnel);
return true;
}
查找代价最小路径的核心在于 GenerateMinCostPath()
,分为3部分:
先采样,
然后构造graph,
最后查找从起点(自车当前位置)到终点(尽可能远的某个采样点)的代价最小路径。
②. DpRoadGraph::GenerateMinCostPath(…)
TODO 仔细分析
bool DpRoadGraph::GenerateMinCostPath(
const std::vector<const Obstacle *> &obstacles,
std::vector<DpRoadGraphNode> *min_cost_path) {
CHECK(min_cost_path != nullptr);
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());
std::list<std::list<DpRoadGraphNode>> graph_nodes;
// find one point from first row
const auto &first_row = path_waypoints.front();
size_t nearest_i = 0;
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();
graph_nodes.back().emplace_back(first_row[nearest_i], nullptr,
ComparableCost());
auto &front = graph_nodes.front().front();
size_t total_level = path_waypoints.size();
for (size_t level = 1; level < path_waypoints.size(); ++level) {
const auto &prev_dp_nodes = graph_nodes.back();
const auto &level_points = path_waypoints[level];
graph_nodes.emplace_back();
std::vector<std::future<void>> results;
for (size_t i = 0; i < level_points.size(); ++i) {
const auto &cur_point = level_points[i];
graph_nodes.back().emplace_back(cur_point, nullptr);
auto msg = std::make_shared<RoadGraphMessage>(
prev_dp_nodes, level, total_level, &trajectory_cost, &front,
&(graph_nodes.back().back()));
if (FLAGS_enable_multi_thread_in_dp_poly_path) {
results.emplace_back(cyber::Async(&DpRoadGraph::UpdateNode, this, msg));
} else {
UpdateNode(msg);
}
}
if (FLAGS_enable_multi_thread_in_dp_poly_path) {
for (auto &result : results) {
result.get();
}
}
}
// find best path
DpRoadGraphNode fake_head;
for (const auto &cur_dp_node : graph_nodes.back()) {
fake_head.UpdateCost(&cur_dp_node, cur_dp_node.min_cost_curve,
cur_dp_node.min_cost);
}
const auto *min_cost_node = &fake_head;
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;
}
③. trajectory_cost(…)
TODO仔细分析
TrajectoryCost trajectory_cost(
config_, reference_line_, reference_line_info_.IsChangeLanePath(),
obstacles, vehicle_config.vehicle_param(), speed_data_, init_sl_point_,
reference_line_info_.AdcSlBoundary());
计算 node 间的 cost 主要在这里
class TrajectoryCost {
public:
TrajectoryCost() = default;
explicit TrajectoryCost(const DpPolyPathConfig &config,
const ReferenceLine &reference_line,
const bool is_change_lane_path,
const std::vector<const Obstacle *> &obstacles,
const common::VehicleParam &vehicle_param,
const SpeedData &heuristic_speed_data,
const common::SLPoint &init_sl_point,
const SLBoundary &adc_sl_boundary);
ComparableCost Calculate(const QuinticPolynomialCurve1d &curve,
const double start_s, const double end_s,
const uint32_t curr_level,
const uint32_t total_level);
private:
......
};
- Calculate 中
ComparableCost TrajectoryCost::Calculate(const QuinticPolynomialCurve1d &curve,
const double start_s,
const double end_s,
const uint32_t curr_level,
const uint32_t total_level) {
ComparableCost total_cost;
// path cost
total_cost +=
CalculatePathCost(curve, start_s, end_s, curr_level, total_level);
// static obstacle cost
total_cost += CalculateStaticObstacleCost(curve, start_s, end_s);
// dynamic obstacle cost
total_cost += CalculateDynamicObstacleCost(curve, start_s, end_s);
return total_cost;
}