Apollo源码解析：决策规划LaneChangeDecider

Apollo源码解析：决策规划LaneChangeDecider

附赠自动驾驶最全的学习资料和量产经验：链接

一、概述

LaneChangeDecider 是lanefollow 场景下，所调用的第一个task，它的作用主要有两点：

• 判断当前是否进行变道，以及变道的状态，并将结果存在变量lane_change_status中；

• 变道过程中将目标车道的reference line放置到首位，变道结束后将当前新车道的reference line放置到首位

二、LaneChangeDecider的流程图如下：

三、LaneChangeDecider的具体逻辑如下：

1、PublicRoadPlanner 的 LaneFollowStage 配置了以下几个task 来实现具体的规划逻辑，LaneChangeDecider是第一个task：

scenario_type: LANE_FOLLOW
stage_type: LANE_FOLLOW_DEFAULT_STAGE
stage_config: {
  stage_type: LANE_FOLLOW_DEFAULT_STAGE
  enabled: true
  task_type: LANE_CHANGE_DECIDER
  task_type: PATH_REUSE_DECIDER
  task_type: PATH_LANE_BORROW_DECIDER
  task_type: PATH_BOUNDS_DECIDER
  task_type: PIECEWISE_JERK_PATH_OPTIMIZER
  task_type: PATH_ASSESSMENT_DECIDER
  task_type: PATH_DECIDER
  task_type: RULE_BASED_STOP_DECIDER
  task_type: ST_BOUNDS_DECIDER
  task_type: SPEED_BOUNDS_PRIORI_DECIDER
  task_type: SPEED_HEURISTIC_OPTIMIZER
  task_type: SPEED_DECIDER
  task_type: SPEED_BOUNDS_FINAL_DECIDER
  # task_type: PIECEWISE_JERK_SPEED_OPTIMIZER
  task_type: PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZER
  task_type: RSS_DECIDER
}

2、前文讲到在stage阶段会依次调用每个 task 的 Execute() 函数，LaneChangeDecider继承自 Decider 类，Decider继承自基类 task 类，并且override了Execute() 方法；

class Decider : public Task {
 public:
  explicit Decider(const TaskConfig& config);
  Decider(const TaskConfig& config,
          const std::shared_ptr<DependencyInjector>& injector);
  virtual ~Decider() = default;
  
  // Execute 方法有override关键字，需要被重写
  apollo::common::Status Execute(
      Frame* frame, ReferenceLineInfo* reference_line_info) override;
 
  apollo::common::Status Execute(Frame* frame) override;
 
 protected:
  virtual apollo::common::Status Process(
      Frame* frame, ReferenceLineInfo* reference_line_info) {
    return apollo::common::Status::OK();
  }
 
  virtual apollo::common::Status Process(Frame* frame) {
    return apollo::common::Status::OK();
  }
};

重写Execute() 的代码在 decider.cc is coming soon 文件中，即调用当前类的 Process() 方法：

apollo::common::Status Decider::Execute(
    Frame* frame, ReferenceLineInfo* reference_line_info) {
  Task::Execute(frame, reference_line_info);
  return Process(frame, reference_line_info);
}

2、由以上分析可知，LaneChangeDecider 的主要决策逻辑在Process() 方法中，Process() 的代码及注释如下，先上整体代码，再详细讲解其中的每个模块：

Status LaneChangeDecider::Process(
    Frame* frame, ReferenceLineInfo* const current_reference_line_info) {
  // Sanity checks.
  CHECK_NOTNULL(frame);
  
  // 读取配置文件
  const auto& lane_change_decider_config = config_.lane_change_decider_config();
  
  // 从frame 中读取reference_line_info，并检查
  std::list<ReferenceLineInfo>* reference_line_info =
      frame->mutable_reference_line_info();
  if (reference_line_info->empty()) {
    const std::string msg = "Reference lines empty.";
    AERROR << msg;
    return Status(ErrorCode::PLANNING_ERROR, msg);
  }
  
  // 如果配置reckless_change_lane为TRUE，则将变道的目标车道放置为reference line的首位，并返回OK；
  // 需要同时打开配置enable_prioritize_change_lane，才可以调整reference line
  // 默认配置中reckless_change_lane 是关闭的，所以不会执行这个逻辑
  if (lane_change_decider_config.reckless_change_lane()) {
    PrioritizeChangeLane(true, reference_line_info);
    return Status::OK();
  }
 
  // 将变道的状态存储在lane_change_status 这个变量中
  auto* prev_status = injector_->planning_context()
                          ->mutable_planning_status()
                          ->mutable_change_lane();
  double now = Clock::NowInSeconds();
 
  prev_status->set_is_clear_to_change_lane(false);
  if (current_reference_line_info->IsChangeLanePath()) {
    prev_status->set_is