这段代码是自动驾驶系统中“Park and Go”场景的“检查”阶段的实现。它主要负责在车辆准备从停车状态转换到行驶状态之前进行一系列检查。下面是对代码的详细解释和注释：
### 包含的头文件
```cpp
#include "modules/planning/scenarios/park_and_go/stage_check.h"
#include "cyber/common/log.h"
#include "modules/planning/planning_base/common/frame.h"
#include "modules/planning/scenarios/park_and_go/context.h"
#include "modules/planning/scenarios/park_and_go/util.h"
```
这些头文件包括了该阶段处理所需的类定义、日志功能、框架和实用工具函数。
### 命名空间
```cpp
namespace apollo {
namespace planning {
```
代码位于`apollo`项目的`planning`命名空间中。
### 使用声明
```cpp
using apollo::common::TrajectoryPoint;
```
使用`apollo::common::TrajectoryPoint`类，无需每次都写完整的命名空间。
### 主处理函数
```cpp
StageResult ParkAndGoStageCheck::Process(
const TrajectoryPoint& planning_init_point, Frame* frame) {
ADEBUG << "stage: Check";
CHECK_NOTNULL(frame);
CHECK_NOTNULL(context_);
```
- `Process`函数是检查阶段的主要逻辑处理函数。
- `ADEBUG`用于输出调试信息。
- `CHECK_NOTNULL`确保`frame`和`context_`指针非空。
```cpp
ADCInitStatus();
frame->mutable_open_space_info()->set_is_on_open_space_trajectory(true);
StageResult result = ExecuteTaskOnOpenSpace(frame);
if (result.HasError()) {
AERROR << "ParkAndGoStageAdjust planning error";
return result.SetStageStatus(StageStatusType::ERROR);
}
```
- `ADCInitStatus()`初始化自动驾驶车辆（ADC）的状态。
- 设置当前帧为开放空间轨迹模式。
- 执行开放空间任务，并检查是否有错误。
```cpp
bool ready_to_cruise =
CheckADCReadyToCruise(injector_->vehicle_state(), frame,
GetContextAs<ParkAndGoContext>()->scenario_config);
return FinishStage(ready_to_cruise);
}
```
- 检查自动驾驶车辆是否准备好巡航。
- 根据检查结果决定完成阶段。
### 完成阶段函数
```cpp
StageResult ParkAndGoStageCheck::FinishStage(const bool success) {
if (success) {
next_stage_ = "PARK_AND_GO_CRUISE";
} else {
next_stage_ = "PARK_AND_GO_ADJUST";
}
injector_->planning_context()
->mutable_planning_status()
->mutable_park_and_go()
->set_in_check_stage(false);
return StageResult(StageStatusType::FINISHED);
}
```
- 根据成功与否决定下一阶段是巡航还是调整。
- 更新规划状态，标记检查阶段结束。
- 返回阶段完成的结果。
### 初始化自动驾驶车辆状态函数
```cpp
void ParkAndGoStageCheck::ADCInitStatus() {
auto* park_and_go_status = injector_->planning_context()
->mutable_planning_status()
->mutable_park_and_go();
park_and_go_status->Clear();
park_and_go_status->mutable_adc_init_position()->set_x(
injector_->vehicle_state()->x());
park_and_go_status->mutable_adc_init_position()->set_y(
injector_->vehicle_state()->y());
park_and_go_status->mutable_adc_init_position()->set_z(0.0);
park_and_go_status->set_adc_init_heading(
injector_->vehicle_state()->heading());
park_and_go_status->set_in_check_stage(true);
}
```
- 清除之前的状态信息。
- 设置自动驾驶车辆的当前位置和朝向。
- 标记当前处于检查阶段。
### 命名空间结束
```cpp
} // namespace planning
} // namespace apollo
```
结束`apollo::planning`命名空间。