決策規劃planning模塊簡要分析
入口
初始化
Planning模塊的初始化見Planning::Init()
(apollo/modules/planning/planning.cc)。
業務邏輯
Planning模塊的業務邏輯見Planning::RunOnce()
(apollo/modules/planning/planning.cc),該函數每隔一段時間執行一次(100ms?),由定時器Planning::OnTimer
定時調用。
定時任務Planning::RunOnce()大體步驟如下:
- 獲取定位數據與底盤數據
AdapterManager::GetLocalization()->GetLatestObserved()
AdapterManager::GetChassis()->GetLatestObserved()
- 計算拼接軌跡
TrajectoryStitcher::ComputeStitchingTrajectory
- 根據拼接軌跡進行決策
Planning::Plan()
決策Planning::Plan()大體步驟:
- 調用
planner
子模塊Planner::Plan()
進行實際決策 - 輸出決策結果
ReferenceLineInfo::ExportDecision()
子模塊分析
Planner
apollo::planning::Planner
apollo::planning::EMPlanner
apollo::planning::LatticePlanner
apollo::planning::RTKReplayPlanner
子模塊planner
的基類是Planner
類,其包括3個具體實現類(如上)。Planner會加載若干Task,如以EMPlanner
爲例,主要加載Decider
、Optimizer
兩類:
- 決策器Decider
- TrafficDecider
- PathDecider
- SpeedDecider
- 優化器Optimizer
- DpPolyPathOptimizer
- DpStSpeedOptimizer
- QpSplineStSpeedOptimizer
- PolyStSpeedOptimizer
EMPlanner::Plan()
方法中會調用EMPlanner::PlanOnReferenceLine()
,然後這些註冊了的Tasks均會被執行,各Task負責各自業務。比如DpPolyPathOptimizer
通過動態規劃來進行路徑規劃。
Task
apollo::planning::Task
apollo::planning::PathDecider
apollo::planning::PathOptimizer
apollo::planning::DpPolyPathOptimizer
apollo::planning::QpSplinePathOptimizer
apollo::planning::SpeedDecider
apollo::planning::SpeedOptimizer
apollo::planning::DpStSpeedOptimizer
apollo::planning::PolyStSpeedOptimizer
apollo::planning::QpSplineStSpeedOptimizer
apollo::planning::TrafficDecider
以DpPolyPathOptimizer
爲例,DpPolyPathOptimizer
類繼承自 PathOptimizer
類,PathOptimizer::Execute()
方法中調用PathOptimizer::Process()
虛方法,DpPolyPathOptimizer::Process()
方法具體實現如下:
Status DpPolyPathOptimizer::Process(const SpeedData &speed_data,
const ReferenceLine &,
const common::TrajectoryPoint &init_point,
PathData *const path_data) {
if (!is_init_) {
AERROR << "Please call Init() before Process().";
return Status(ErrorCode::PLANNING_ERROR, "Not inited.");
}
CHECK_NOTNULL(path_data);
DPRoadGraph dp_road_graph(config_, *reference_line_info_, speed_data);
dp_road_graph.SetDebugLogger(reference_line_info_->mutable_debug());
if (!dp_road_graph.FindPathTunnel(
init_point,
reference_line_info_->path_decision()->path_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();
}
bool DPRoadGraph::FindPathTunnel(
const common::TrajectoryPoint &init_point,
const std::vector<const PathObstacle *> &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;
}
if (!CalculateFrenetPoint(init_point_, &init_frenet_frame_point_)) {
AERROR << "Fail to create init_frenet_frame_point_ from : "
<< init_point_.DebugString();
return false;
}
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 = init_sl_point_.s();
const double path_resolution = config_.path_resolution();
for (std::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;
}
更多信息可參考:http://www.fzb.me/apollo/module_planning.html