Baidu Apollo程式碼解析之Planning的結構與呼叫流程(2)
Apollo5。0的Planning模組是基於Scenario、Stage、Task這樣的層次組織的,針對不同的場景設計不同的演算法細節。Scenario指一個特定的問題或場景,Stage指在一個Scenario下的規劃方法的粗略步驟,Task指一個具體的處理方法。相應地,一個Scenario包含多個Stage,一個Stage包含多個Task。那我們按照由下向上的順序展開說。
//Scenario類中包含Stage
std
::
unordered_map
<
ScenarioConfig
::
StageType
,
const
ScenarioConfig
::
StageConfig
*
,
std
::
hash
<
int
>>
stage_config_map_
;
//Stage類中包含Task
std
::
map
<
TaskConfig
::
TaskType
,
std
::
unique_ptr
<
Task
>>
tasks_
;
std
::
vector
<
Task
*>
task_list_
;
1。 Task
在apollo/modules/planning/tasks資料夾中,Task分為4類:deciders,optimizers,rss,smoothers。task。h定義了Task基類,其中重要的是2個Execute()函式。
virtual
common
::
Status
Execute
(
Frame
*
frame
,
ReferenceLineInfo
*
reference_line_info
);
virtual
common
::
Status
Execute
(
Frame
*
frame
);
TaskFactory類是為了方便的構造Task物件。
decider。h中定義了Decider類,繼承自Task類,對應著繼承而來的2個Execute(),分別定義了2個Process(),即Task的執行是透過Decider::Process()執行的。
apollo
::
common
::
Status
Decider
::
Execute
(
Frame
*
frame
,
ReferenceLineInfo
*
reference_line_info
)
{
Task
::
Execute
(
frame
,
reference_line_info
);
return
Process
(
frame
,
reference_line_info
);
}
apollo
::
common
::
Status
Decider
::
Execute
(
Frame
*
frame
)
{
Task
::
Execute
(
frame
);
return
Process
(
frame
);
}
不同的Decider如CreepDecider類等,都是繼承自Decider類,分別實現自己的Process(),如CreepDecider::Process()。比較特殊的是PathDecider和SpeedDecider類是直接繼承自Task類的,因此要實現自己的Execute()。
Optimizer有3種,都繼承自Task類,分別是:PathOptimizer,SpeedOptimizer,TrajectoryOptimizer。這3種Optimizer實現了各自的Execute(),並定義了純虛擬函式Process()在Execute()中被呼叫,由子類實現具體的Process()。不同的最佳化器,如PiecewiseJerkPathOptimizer是繼承自PathOptimizer的,實現自己的Process()。
RssDecider繼承自Task類,實現了Execute()。
Smoother是個特例,並沒有繼承Task類,關鍵函式Smooth()。
2。 Stage
scenarios資料夾中包含了多種場景,內部的每個資料夾就是一個scenario的定義和解決。首先看Stage類的定義,主要的處理都在Stage::Process()中(此處是純虛擬函式)。
class
Stage
{
public
:
。。。
/**
* @brief Each stage does its business logic inside Process function。
* If the stage want to transit to a different stage after finish,
* it should set the type of ‘next_stage_’。
*/
virtual
StageStatus
Process
(
const
common
::
TrajectoryPoint
&
planning_init_point
,
Frame
*
frame
)
=
0
;
/**
* @brief The sequence of tasks inside the stage。 These tasks usually will be
* executed in order。
*/
const
std
::
vector
<
Task
*>&
TaskList
()
const
{
return
task_list_
;
}
ScenarioConfig
::
StageType
NextStage
()
const
{
return
next_stage_
;
}
protected
:
bool
ExecuteTaskOnReferenceLine
(
const
common
::
TrajectoryPoint
&
planning_start_point
,
Frame
*
frame
);
bool
ExecuteTaskOnOpenSpace
(
Frame
*
frame
);
。。。
protected
:
std
::
map
<
TaskConfig
::
TaskType
,
std
::
unique_ptr
<
Task
>>
tasks_
;
std
::
vector
<
Task
*>
task_list_
;
ScenarioConfig
::
StageType
next_stage_
;
。。。
};
這裡以scenarios/park/valet_parking舉例,定義了繼承自Scenario類的ValetParkingScenario類,並註冊了2個Stage:VALET_PARKING_APPROACHING_PARKING_SPOT和VALET_PARKING_PARKING,分別對應繼承自Stage類的StageApproachingParkingSpot類和StageParking類。即想停車,第一步先將車行駛到接近停車位,第二步停入停車位。我們上面提到了一個Stage包含一個Task List,是在哪設定的呢?其實是在apollo/modules/planning/conf/scenario/valet_parking_config。pb。txt中。在StageApproachingParkingSpot::Process()中,會呼叫Stage::ExecuteTaskOnReferenceLine(),而在Stage::ExecuteTaskOnReferenceLine()中,又會遍歷Task List,對每一個Task執行task->Execute()。
3。 Scenario
再看Scenario類的定義。在Scenario::Process()中,透過呼叫Stage::Process()來處理該stage所包含的task。當該stage處理完成時,就切換到下一個stage。只要當前的stage不是空、有意義,scenario就是“未完成”的狀態,從而可以繼續執行接下來的Stage。當前的stage是空,則所有的stage處理完成了,scenario才處理完畢。
Scenario
::
ScenarioStatus
Scenario
::
Process
(
const
common
::
TrajectoryPoint
&
planning_init_point
,
Frame
*
frame
)
{
。。。
//在Stage中處理Task List,返回Stage的狀態
auto
ret
=
current_stage_
->
Process
(
planning_init_point
,
frame
);
switch
(
ret
)
{
。。。
case
Stage
::
RUNNING
:
{
scenario_status_
=
STATUS_PROCESSING
;
break
;
}
case
Stage
::
FINISHED
:
{
auto
next_stage
=
current_stage_
->
NextStage
();
。。。
if
(
current_stage_
!=
nullptr
&&
current_stage_
->
stage_type
()
!=
ScenarioConfig
::
NO_STAGE
)
{
//只要current_stage_不是空、有意義,scenario_status_就是“未完成”,
//從而可以繼續執行接下來的Stage
scenario_status_
=
STATUS_PROCESSING
;
}
else
{
scenario_status_
=
STATUS_DONE
;
}
break
;
}
。。。
}
return
scenario_status_
;
}
這裡還是以valet_parking為例,給出函式的呼叫與實際執行的層次、次序。其他情況類似。
4。 ScenarioManager
ScenarioManager類用來管理各個Scenario的判別和切換,透過ScenarioManager::Update() 進而呼叫 ScenarioManager::ScenarioDispatch() 來改變當前對應的scenario。