【Apollo学习笔记】—— Planning模块_apollo中adctrajectory什么添加档位信息

前言

本文记录学习planning模块时的一些笔记，总体流程参照https://zhuanlan.zhihu.com/p/61982682中的流程图，如上图所示。

planning_component

modules/planning/planning_component.cc

PlanningComponent::Init部分首先完成规划模式的选择：

  if (FLAGS_use_navigation_mode) {
    planning_base_ = std::make_unique<NaviPlanning>(injector_);
  } else {
    planning_base_ = std::make_unique<OnLanePlanning>(injector_);
  }
1
2
3
4
5

再完成相应的消息订阅

routing_reader_ = node_->CreateReader<RoutingResponse>(
traffic_light_reader_ = node_->CreateReader<TrafficLightDetection>(
pad_msg_reader_ = node_->CreateReader<PadMessage>(
story_telling_reader_ = node_->CreateReader<Stories>(
relative_map_reader_ = node_->CreateReader<MapMsg>(
1
2
3
4
5

最后消息发布

  planning_writer_ = node_->CreateWriter<ADCTrajectory>(
  rerouting_writer_ = node_->CreateWriter<RoutingRequest>(
  planning_learning_data_writer_ = node_->CreateWriter<PlanningLearningData>(
1
2
3

PlanningComponent::Proc的主要是检查数据，并且执行注册好的Planning，生成路线并且发布。

bool PlanningComponent::Proc(...) {
  // 1. 检查是否需要重新规划线路。
  CheckRerouting();
  // 2. 数据放入local_view_中，并且检查输入数据。
  ...
  // 3. 执行注册好的Planning，生成线路。
  planning_base_->RunOnce(local_view_, &adc_trajectory_pb);
  // 4. 发布消息
  planning_writer_->Write(std::make_shared<ADCTrajectory>(adc_trajectory_pb));
  // 5. 最后记录
  history->Add(adc_trajectory_pb);
}
1
2
3
4
5
6
7
8
9
10
11
12

RunOnce在NaviPlanning和OnLanePlanning都有，本文以常用的OnLanePlanning为例。

PS1

• prediction_obstacles 从channel中获得的指针，类型为 PredictionObstacles，代码文件在 modules/prediction/proto/prediction_obstacle.proto 中，该类型的消息来自预测模块，包含了以下信息：
  • Header 头结构：包含了这条消息发布的时刻（以秒为单位），目前位置的模块名，该消息的序列号（每个模块各自维护的）等，几乎每个消息都包含有这个头结构，下面就不提及了。
  • PredictionObstacle 预测模块中的若干个障碍物行为：有感知模块中的障碍物（PerceptionObstacle），它包括障碍物的 id，它的三维坐标、速度加速度、边界大小（长高宽）、特殊的形状、类型（行人、非机动车、机动车）、运动轨迹等；还有时间戳，记录 GPS 给出的时刻；还有预测的时间长度；多种可能的运动轨迹；障碍物的运动趋势、优先级等。
  • 开始的时间戳：记录预测开始的时刻
  • 结束时间戳：预测结束的时刻
  • 自动驾驶车辆的运动趋势，停止、正常行驶还是正在变道等
  • Scenery 现在的场景
• chassis 从channel获得的指针，类型为 Chassis，这条消息直接来自总线 CanBus，代码文件在 modules/canbus/proto/chassis.proto 中，该类包含了很多信息，主要是汽车底盘所给出的机械状态信息，比如：
  • 驾驶模式，有手动驾驶、自动驾驶、仅控制方向、仅控制速度以及紧急模式
  • 档位情况、引擎转速、车辆速度、里程表、燃油情况、电池电量等
  • 刹车、油门踏板的力度，方向盘的旋转角度，车轮转速
  • 转向灯、雾灯、大灯的工作情况
• localization_estimate 从channel中获得的指针，类型为 LocalizationEstimate，代码文件在 modules/localization/proto/localization.proto 中，这条消息来自定位模块，该类主要包含了：
  • Pose 位置姿势，包括车头朝向，在地图上的线速度、线加速度和相对位置，角速度、仰角度等
  • 测量上述姿势的时刻
  • 车辆已经经过的轨迹点列
  • MSF 定位状态与质量

PS2

std::mutex是C++11标准中提供的一种互斥锁机制，用于保护共享资源的访问。当多个线程需要同时访问共享资源时，为了避免出现数据竞争等问题，需要使用互斥锁进行同步控制。

std::mutex是一种基本的互斥锁类型，可以通过lock()函数锁定互斥锁，通过unlock()函数释放互斥锁。当一个线程持有互斥锁时，其他线程想要获取该互斥锁将会被阻塞，直到持有该互斥锁的线程将锁释放。

例如，当多个线程需要同时访问同一个共享变量时，可以在访问之前使用std::mutex进行锁定，保证每个线程都可以安全地访问该变量。例如：

#include <mutex>
#include <thread>
#include <iostream>

int shared_data = 0;
std::mutex mutex;

void work(int id) {
    for (int i = 0; i < 10000; ++i) {
        mutex.lock();
        shared_data++;
        mutex.unlock();
    }
    std::cout << "Thread " << id << " finished." << std::endl;
}

int main() {
    std::thread t1(work, 1);
    std::thread t2(work, 2);
    t1.join();
    t2.join();
    std::cout << "Shared data: " << shared_data << std::endl;
    return 0;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24

在上述例子中，两个线程都会对shared_data变量进行10000次的自增操作，为了避免出现数据竞争，我们在对shared_data进行访问之前都使用了std::mutex进行了锁定，保证了线程的安全性。

on_lane_planning

modules/planning/on_lane_planning.cc

OnLanePlanning::Init主要实现分配具体的Planner，启动参考线提供器(reference_line_provider_)。启动参考线提供器，会另启动一个线程，执行一个定时任务，每隔50ms提供一次参考线。注意：分配Planner的部分在OnLanePlanning实例化时就已经完成。

分配Planner的实现部分

modules/planning/planner/on_lane_planner_dispatcher.cc
std::unique_ptr<Planner> OnLanePlannerDispatcher::DispatchPlanner(
    const PlanningConfig& planning_config,
    const std::shared_ptr<DependencyInjector>& injector) {
  return planner_factory_.CreateObject(
      planning_config.standard_planning_config().planner_type(0), injector);
}
1
2
3
4
5
6
7

可以在modules/planning/conf/planning_config.pb.txt配置文件中看到配置Planner的类型：

standard_planning_config {
  planner_type: PUBLIC_ROAD
  planner_public_road_config {
  }
}
1
2
3
4
5

---

OnLanePlanning 的主逻辑在OnLanePlanning::RunOnce中，内容比较多，有些还没仔细看，大致完成了一下的内容：

void OnLanePlanning::RunOnce(const LocalView& local_view,
                             ADCTrajectory* const ptr_trajectory_pb) {
    //  更新汽车状态和参考线的状态，如果状态无效，直接返回
    //  ...  
    // 是否为出现导航路线变换，如果是 初始化 planner
    //  加上预估的规划触发的周期 得到 stitchingTrajectory
  // planning is triggered by prediction data, but we can still use an estimated
  // cycle time for stitching
  status = InitFrame(frame_num, stitching_trajectory.back(), vehicle_state);
    //  判断是否符合交通规则
    //  开始正在的规划 planner 开始规划
  status = Plan(start_timestamp, stitching_trajectory, ptr_trajectory_pb);
    //  记录规划所花费的时间
    //  ...
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15

status = Plan(start_timestamp, stitching_trajectory, ptr_trajectory_pb);中有如下代码，此部分的Plan是指分配的planner的Plan()函数

  auto status = planner_->Plan(stitching_trajectory.back(), frame_.get(),
                               ptr_trajectory_pb);
1
2

PS1

make_unique是一个C++11中的函数模板，用于创建并返回一个独占指针（unique_ptr）。它的作用是将一个裸指针封装成一个unique_ptr对象，并确保该对象是唯一持有它所指向的对象。这可以有效避免内存泄漏和多个指针指向同一个对象的问题。

public_road_planner

modules/planning/planner/public_road/public_road_planner.cc
接着来看public_road_planner的实现。

PublicRoadPlanner::Init实例化一个全局的scenario_manager_对象来进行场景管理。

Status PublicRoadPlanner::Init(const PlanningConfig& config) {
  config_ = config;
  scenario_manager_.Init(config);
  return Status::OK();
}
1
2
3
4
5

PublicRoadPlanner::Plan主要完成以下工作

Status PublicRoadPlanner::Plan(const TrajectoryPoint& planning_start_point,
                               Frame* frame,
                               ADCTrajectory* ptr_computed_trajectory) {
  // 更新场景，决策当前应该执行什么场景
  scenario_manager_.Update(planning_start_point, *frame);
  // 获取当前场景
  scenario_ = scenario_manager_.mutable_scenario();
  // 调用Scenario的Process函数，对具体的场景进行处理
  auto result = scenario_->Process(planning_start_point, frame);

  if (FLAGS_enable_record_debug) {
    auto scenario_debug = ptr_computed_trajectory->mutable_debug()
                              ->mutable_planning_data()
                              ->mutable_scenario();
    scenario_debug->set_scenario_type(scenario_->scenario_type());
    scenario_debug->set_stage_type(scenario_->GetStage());
    scenario_debug->set_msg(scenario_->GetMsg());
  }
  // 当前场景完成
  if (result == scenario::Scenario::STATUS_DONE) {
    // only updates scenario manager when previous scenario's status is
    // STATUS_DONE
    scenario_manager_.Update(planning_start_point, *frame);
  } else if (result == scenario::Scenario::STATUS_UNKNOWN) {
    // 当前场景失败
    return Status(common::PLANNING_ERROR, "scenario returned unknown");
  }
  return Status::OK();
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29

Scenario

Introduction

Apollo规划模块的框架以场景调度为基本决策框架，场景调度本质上是一个双层状态机，顶层是基于场景的管理器，底层是一系列小模块的任务组合，由此来构建出Apollo planning的整体框架。

在执行的每一个场景中，又分为一个或多个阶段，称为stage；而每一个stage又包含一个或多个任务，称为task。执行一个场景，最终就是顺序执行不同阶段的不同任务。由此可见，Scenario在Apollo规划决策中占据着重要的地位。

Scenario 类型

在apollo中，Scenario被定义了十余种类型，在官方文档Planing 2.0综述中有对各个场景的直观描述。例如下图是LANE_FOLLOW场景的示意图。

在apollo8.0中，可以从modules/planning/proto/planning_config.proto文件中看到关于Scenario的相关类型定义:

message ScenarioConfig {

  message StageConfig {
    // 阶段配置
    // 阶段类型
    optional StageType stage_type = 1;
    // 是否启用该阶段配置，默认值为true
    optional bool enabled = 2 [default = true];
    // 运行时使用的任务列表，按照顺序排列
    // 决定了这些任务的运行时顺序
    // 任务类型
    repeated TaskConfig.TaskType task_type = 3;
    // 未排序的任务配置列表
    // 任务配置
    repeated TaskConfig task_config = 4;
  }

  // 场景类型
  optional ScenarioType scenario_type = 1;

  // 场景配置，使用oneof是为了确保只有一个字段被设置
  oneof scenario_config {
    // 车道跟随场景配置
    ScenarioLaneFollowConfig lane_follow_config = 2;
    // 无保护交叉口场景配置
    ScenarioBareIntersectionUnprotectedConfig bare_intersection_unprotected_config = 3;
    // 紧急靠边停车场景配置
    ScenarioEmergencyPullOverConfig emergency_pull_over_config = 4;
    // 紧急停车场景配置
    ScenarioEmergencyStopConfig emergency_stop_config = 5;
    // 学习模型样本场景配置
    ScenarioLearningModelSampleConfig learning_model_sample_config = 6;
    // 窄街道U型转弯场景配置
    ScenarioNarrowStreetUTurnConfig narrow_street_u_turn_config = 7;
    // 停车即走场景配置
    ScenarioParkAndGoConfig park_and_go_config = 8;
    // 靠边停车场景配置
    ScenarioPullOverConfig pull_over_config = 9;
    // 无保护停车标志左转场景配置
    ScenarioStopSignUnprotectedConfig stop_sign_unprotected_config = 10;
    // 交通信号灯保护左转场景配置
    ScenarioTrafficLightProtectedConfig traffic_light_protected_config = 11;
    // 无保护交通信号灯左转场景配置
    ScenarioTrafficLightUnprotectedLeftTurnConfig traffic_light_unprotected_left_turn_config = 12;
    // 无保护交通信号灯右转场景配置
    ScenarioTrafficLightUnprotectedRightTurnConfig traffic_light_unprotected_right_turn_config = 13;
    // 代客停车场景配置
    ScenarioValetParkingConfig valet_parking_config = 14;
    // 让路标志场景配置
    ScenarioYieldSignConfig yield_sign_config = 15;
    // 死胡同场景配置（掉头）
    ScenarioDeadEndTurnAroundConfig deadend_turnaround_config = 18;
  }
  // 运行时使用的阶段列表，第一个是默认阶段
  // 阶段类型
  repeated StageType stage_type = 16;
  // 运行时使用的阶段配置列表，未排序
  // 阶段配置
  repeated StageConfig stage_config = 17;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60

接着来看看Scenario Manager做了些什么。
modules/planning/scenarios/scenario_manager.cc

场景注册

// modules/planning/scenarios/scenario_manager.cc
bool ScenarioManager::Init(const PlanningConfig& planning_config) {
  planning_config_.CopyFrom(planning_config);
  // 注册场景
  RegisterScenarios();
  // 默认为LANE_FOLLOW
  default_scenario_type_ = ScenarioType::LANE_FOLLOW;
  current_scenario_ = CreateScenario(default_scenario_type_);
  return true;
}
1
2
3
4
5
6
7
8
9
10

ScenarioManager负责实际的场景注册，Init函数负责对场景的初始化，首先调用::RegisterScenario这个函数对各个场景配置文件进行读取和注册；在开始创建LANE_FOLLOW作为默认运行场景。

// modules/planning/planner/public_road/public_road_planner.cc
Status PublicRoadPlanner::Init(const PlanningConfig& config) {
  config_ = config;
  scenario_manager_.Init(config);
  return Status::OK();
}
1
2
3
4
5
6

在PublicRoadPlanner初始化时会调用配置文件里的参数来建立这个对象，ScenarioManager会实例化一个全局的scenario_manager_对象来进行场景管理。

场景转换

// 更新场景
void ScenarioManager::Update(const common::TrajectoryPoint& ego_point,
                             const Frame& frame) {
  ACHECK(!frame.reference_line_info().empty());
  // 获取当前要处理的overlaps
  Observe(frame);
  // 场景分发
  ScenarioDispatch(frame);
}
1
2
3
4
5
6
7
8
9

ScenarioManager除了负责场景的配置与注册外也负责对场景进行转换。场景转换在ScenarioManager::Update中实现，这个函数有两个输入，车辆当前的状态信息ego_point，和planning循环计算所需要的其他相关信息frame。ScenarioManager::Update首先调用Observe函数，用以获取当前要处理的overlaps.

// 获取当前要处理的overlaps
void ScenarioManager::Observe(const Frame& frame) {
  // init first_encountered_overlap_map_
  first_encountered_overlap_map_.clear();
  const auto& reference_line_info = frame.reference_line_info().front();
  const auto& first_encountered_overlaps =
      reference_line_info.FirstEncounteredOverlaps();
  for (const auto& overlap : first_encountered_overlaps) {
    if (overlap.first == ReferenceLineInfo::PNC_JUNCTION ||
        overlap.first == ReferenceLineInfo::SIGNAL ||
        overlap.first == ReferenceLineInfo::STOP_SIGN ||
        overlap.first == ReferenceLineInfo::YIELD_SIGN) {
      first_encountered_overlap_map_[overlap.first] = overlap.second;
    }
  }
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16

在apollo里 overlap 是指地图上任意重合的东西，比如PNC_JUNCTION里是道路之间有相互重合，SIGNAL是信号灯与道路有重合，STOP_SIGN是停止标志与道路有重合，YIELD_SIGN是让行标志与道路有重合。

// 通过一个有限状态机，决定当前的场景
// 会根据配置选择基于规则还是基于学习的决策方法。
void ScenarioManager::ScenarioDispatch(const Frame& frame) {
  ACHECK(!frame.reference_line_info().empty());
  ScenarioType scenario_type;

  int history_points_len = 0;
  if (injector_->learning_based_data() &&
      injector_->learning_based_data()->GetLatestLearningDataFrame()) {
    history_points_len = injector_->learning_based_data()
                             ->GetLatestLearningDataFrame()
                             ->adc_trajectory_point_size();
  }
  if ((planning_config_.learning_mode() == PlanningConfig::E2E ||
       planning_config_.learning_mode() == PlanningConfig::E2E_TEST) &&
      history_points_len >= FLAGS_min_past_history_points_len) {
    scenario_type = ScenarioDispatchLearning();
  } else {
    scenario_type = ScenarioDispatchNonLearning(frame);
  }

  ADEBUG << "select scenario: " << ScenarioType_Name(scenario_type);

  // update PlanningContext
  UpdatePlanningContext(frame, scenario_type);

  if (current_scenario_->scenario_type() != scenario_type) {
    current_scenario_ = CreateScenario(scenario_type);
  }
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30

当遇到这些overlaps时，在函数ScenarioDispatch中对其进行具体的处理或者场景切换。ScenarioDispatch会根据配置选择基于规则还是基于学习的决策方法。

接着来看ScenarioDispatchNonLearning函数：

ScenarioType ScenarioManager::ScenarioDispatchNonLearning(const Frame& frame) {
  
  // default: LANE_FOLLOW
  ScenarioType scenario_type = default_scenario_type_;

  
  // Pad Msg scenario(驾驶员意图？)
  scenario_type = SelectPadMsgScenario(frame);

  if (scenario_type == default_scenario_type_) {
    // check current_scenario (not switchable)
    switch (current_scenario_->scenario_type()) {
      case ScenarioType::LANE_FOLLOW:
      case ScenarioType::PULL_OVER:
        break;
      case ScenarioType::BARE_INTERSECTION_UNPROTECTED:
      case ScenarioType::EMERGENCY_PULL_OVER:
      case ScenarioType::PARK_AND_GO:
      case ScenarioType::STOP_SIGN_PROTECTED:
      case ScenarioType::STOP_SIGN_UNPROTECTED:
      case ScenarioType::TRAFFIC_LIGHT_PROTECTED:
      case ScenarioType::TRAFFIC_LIGHT_UNPROTECTED_LEFT_TURN:
      case ScenarioType::TRAFFIC_LIGHT_UNPROTECTED_RIGHT_TURN:
      case ScenarioType::VALET_PARKING:
      case ScenarioType::YIELD_SIGN:
        // must continue until finish
        if (current_scenario_->GetStatus() !=
            Scenario::ScenarioStatus::STATUS_DONE) {
          scenario_type = current_scenario_->scenario_type();
        }
        break;
      default:
        break;
    }
  }
  // ParkAndGo / starting scenario
  if (scenario_type == default_scenario_type_) {
    if (FLAGS_enable_scenario_park_and_go) {
      scenario_type = SelectParkAndGoScenario(frame);
    }
  }
  // intersection scenarios
  if (scenario_type == default_scenario_type_) {
    scenario_type = SelectInterceptionScenario(frame);
  }
  // pull-over scenario
  if (scenario_type == default_scenario_type_) {
    if (FLAGS_enable_scenario_pull_over) {
      scenario_type = SelectPullOverScenario(frame);
    }
  }
  // VALET_PARKING scenario
  if (scenario_type == default_scenario_type_) {
    scenario_type = SelectValetParkingScenario(frame);
  }
  return scenario_type;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57

首先，确定一个默认的场景类型，即LANE_FOLLOW。然后，判断是否有驾驶员意图，如果有，则将场景类型改为相应的意图场景类型。如果没有，则进入下一步逻辑。接着检查当前的场景类型，如果是LANE_FOLLOW或PULL_OVER，直接break；若是其他场景，且其他场景还未执行完毕，则继续执行当前场景。如果当前场景已完成，则进入下一步逻辑。PARK_AND_GO和PULL_OVER若有相关配置，则执行相应的函数。注意：每次切换场景都从默认场景开始，最后回到默认场景。

Scenario::Process

由PublicRoadPlanner::Plan的流程可知，在获取完当前场景之后，会调用Scenario的Process函数，对具体的场景进行处理。接下来便来看看Scenario的Process函数：
modules/planning/scenarios/scenario.cc

Scenario::ScenarioStatus Scenario::Process(
    const common::TrajectoryPoint& planning_init_point, Frame* frame) {
  // stage类型unknow
  if (current_stage_ == nullptr) {
    AWARN << "Current stage is a null pointer.";
    return STATUS_UNKNOWN;
  }
  // stage全部执行完成
  if (current_stage_->stage_type() == StageType::NO_STAGE) {
    scenario_status_ = STATUS_DONE;
    return scenario_status_;
  }
  // 当前处于某一stage，调用这个stage的Process()函数，处理具体规划逻辑
  auto ret = current_stage_->Process(planning_init_point, frame);
  switch (ret) {
    case Stage::ERROR: {
      AERROR << "Stage '" << current_stage_->Name() << "' returns error";
      scenario_status_ = STATUS_UNKNOWN;
      break;
    }
    case Stage::RUNNING: {
      scenario_status_ = STATUS_PROCESSING;
      break;
    }
    // 读取当前stage完成的状态，并对下一个stage进行处理
    case Stage::FINISHED: {
      auto next_stage = current_stage_->NextStage();
      if (next_stage != current_stage_->stage_type()) {
        AINFO << "switch stage from " << current_stage_->Name() << " to "
              << StageType_Name(next_stage);
        if (next_stage == StageType::NO_STAGE) {
          scenario_status_ = STATUS_DONE;
          return scenario_status_;
        }
        if (stage_config_map_.find(next_stage) == stage_config_map_.end()) {
          AERROR << "Failed to find config for stage: " << next_stage;
          scenario_status_ = STATUS_UNKNOWN;
          return scenario_status_;
        }
        current_stage_ = CreateStage(*stage_config_map_[next_stage], injector_);
        if (current_stage_ == nullptr) {
          AWARN << "Current stage is a null pointer.";
          return STATUS_UNKNOWN;
        }
      }
      if (current_stage_ != nullptr &&
          current_stage_->stage_type() != StageType::NO_STAGE) {
        scenario_status_ = STATUS_PROCESSING;
      } else {
        scenario_status_ = STATUS_DONE;
      }
      break;
    }
    default: {
      AWARN << "Unexpected Stage return value: " << ret;
      scenario_status_ = STATUS_UNKNOWN;
    }
  }
  return scenario_status_;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60

Scenario都是顺序执行，只需要判断这一阶段是否结束，然后转到下一个阶段就可以了。CreateStage生成相应的stage，之后再进行stage的process。

Stage

以Lanefollowstage为例进行介绍。
modules/planning/scenarios/lane_follow/lane_follow_stage.cc

Stage::StageStatus LaneFollowStage::Process(
    const TrajectoryPoint& planning_start_point, Frame* frame) {
  bool has_drivable_reference_line = false;

  ADEBUG << "Number of reference lines:\t"
         << frame->mutable_reference_line_info()->size();

  unsigned int count = 0;
  // 遍历所有的参考线，直到找到可用来规划的参考线后退出
  for (auto& reference_line_info : *frame->mutable_reference_line_info()) {
    // TODO(SHU): need refactor
    if (count++ == frame->mutable_reference_line_info()->size()) {
      break;
    }
    ADEBUG << "No: [" << count << "] Reference Line.";
    ADEBUG << "IsChangeLanePath: " << reference_line_info.IsChangeLanePath();
    // 找到可用来行驶的参考线，退出循环
    if (has_drivable_reference_line) {
      reference_line_info.SetDrivable(false);
      break;
    }
    // 执行LaneFollow的规划
    auto cur_status =
        PlanOnReferenceLine(planning_start_point, frame, &reference_line_info);
    // 判断规划结果是否OK
    if (cur_status.ok()) {
      // 如果发生lanechange，判断reference_line的cost
      if (reference_line_info.IsChangeLanePath()) {
        ADEBUG << "reference line is lane change ref.";
        ADEBUG << "FLAGS_enable_smarter_lane_change: "
               << FLAGS_enable_smarter_lane_change;
        if (reference_line_info.Cost() < kStraightForwardLineCost &&
            (LaneChangeDecider::IsClearToChangeLane(&reference_line_info) ||
             FLAGS_enable_smarter_lane_change)) {
          // If the path and speed optimization succeed on target lane while
          // under smart lane-change or IsClearToChangeLane under older version
          has_drivable_reference_line = true;
          reference_line_info.SetDrivable(true);
          LaneChangeDecider::UpdatePreparationDistance(
              true, frame, &reference_line_info, injector_->planning_context());
          ADEBUG << "\tclear for lane change";
        } else {
          LaneChangeDecider::UpdatePreparationDistance(
              false, frame, &reference_line_info,
              injector_->planning_context());
          reference_line_info.SetDrivable(false);
          ADEBUG << "\tlane change failed";
        }
      } else {
      // 如果没有lanechange，stage执行结果为OK，则has_drivable_reference_line置位true
        ADEBUG << "reference line is NOT lane change ref.";
        has_drivable_reference_line = true;
      }
    } else {
      reference_line_info.SetDrivable(false);
    }
  }

  return has_drivable_reference_line ? StageStatus::RUNNING
                                     : StageStatus::ERROR;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61

Process主要是查找可供规划的参考线，之后在PlanOnReferenceLine函数中进行具体的执行。同时还对变道的状况进行规划，如果规划成功后，还需要判断目标车道的变道cost，如果cost太高，那么就会舍弃掉这条目标车道的reference_line, 此时放弃变道的规划，继续循环使用原车道的reference_line进行规划。

PlanOnReferenceLine函数

Status LaneFollowStage::PlanOnReferenceLine(
    const TrajectoryPoint& planning_start_point, Frame* frame,
    ReferenceLineInfo* reference_line_info) {
  // 判断是否有lanechange意图，如果否增加当前参考线的cost？有点疑问，增加了变道的可能
  if (!reference_line_info->IsChangeLanePath()) {
    reference_line_info->AddCost(kStraightForwardLineCost);
  }
  ADEBUG << "planning start point:" << planning_start_point.DebugString();
  ADEBUG << "Current reference_line_info is IsChangeLanePath: "
         << reference_line_info->IsChangeLanePath();
  // 顺序执行stage中的任务
  auto ret = Status::OK();
  for (auto* task : task_list_) {
    const double start_timestamp = Clock::NowInSeconds();
    // 执行每个task的具体逻辑
    ret = task->Execute(frame, reference_line_info);

    const double end_timestamp = Clock::NowInSeconds();
    const double time_diff_ms = (end_timestamp - start_timestamp) * 1000;
    ADEBUG << "after task[" << task->Name()
           << "]:" << reference_line_info->PathSpeedDebugString();
    ADEBUG << task->Name() << " time spend: " << time_diff_ms << " ms.";
    RecordDebugInfo(reference_line_info, task->Name(), time_diff_ms);
    // 如果task执行失败，退出task执行序列，并且记录失败信息
    if (!ret.ok()) {
      AERROR << "Failed to run tasks[" << task->Name()
             << "], Error message: " << ret.error_message();
      break;
    }

    // TODO(SHU): disable reference line order changes for now
    // updated reference_line_info, because it is changed in
    // lane_change_decider by PrioritizeChangeLane().
    // reference_line_info = &frame->mutable_reference_line_info()->front();
    // ADEBUG << "Current reference_line_info is IsChangeLanePath: "
    //        << reference_line_info->IsChangeLanePath();
  }

  RecordObstacleDebugInfo(reference_line_info);

  // check path and speed results for path or speed fallback
  reference_line_info->set_trajectory_type(ADCTrajectory::NORMAL);
  // 如果task执行失败，则使用备用的规划轨迹
  if (!ret.ok()) {
    PlanFallbackTrajectory(planning_start_point, frame, reference_line_info);
  }
  // 对规划的轨迹进行合成，如果合成失败，返回失败状态
  DiscretizedTrajectory trajectory;
  if (!reference_line_info->CombinePathAndSpeedProfile(
          planning_start_point.relative_time(),
          planning_start_point.path_point().s(), &trajectory)) {
    const std::string msg = "Fail to aggregate planning trajectory.";
    AERROR << msg;
    return Status(ErrorCode::PLANNING_ERROR, msg);
  }

  // determine if there is a destination on reference line.
  double dest_stop_s = -1.0;
  for (const auto* obstacle :
       reference_line_info->path_decision()->obstacles().Items()) {
    if (obstacle->LongitudinalDecision().has_stop() &&
        obstacle->LongitudinalDecision().stop().reason_code() ==
            STOP_REASON_DESTINATION) {
      SLPoint dest_sl = GetStopSL(obstacle->LongitudinalDecision().stop(),
                                  reference_line_info->reference_line());
      dest_stop_s = dest_sl.s();
    }
  }
  // 增加障碍物的代价
  for (const auto* obstacle :
       reference_line_info->path_decision()->obstacles().Items()) {
    if (obstacle->IsVirtual()) {
      continue;
    }
    if (!obstacle->IsStatic()) {
      continue;
    }
    if (obstacle->LongitudinalDecision().has_stop()) {
      bool add_stop_obstacle_cost = false;
      if (dest_stop_s < 0.0) {
        add_stop_obstacle_cost = true;
      } else {
        SLPoint stop_sl = GetStopSL(obstacle->LongitudinalDecision().stop(),
                                    reference_line_info->reference_line());
        if (stop_sl.s() < dest_stop_s) {
          add_stop_obstacle_cost = true;
        }
      }
      if (add_stop_obstacle_cost) {
        static constexpr double kReferenceLineStaticObsCost = 1e3;
        reference_line_info->AddCost(kReferenceLineStaticObsCost);
      }
    }
  }

  if (FLAGS_enable_trajectory_check) {
    if (ConstraintChecker::ValidTrajectory(trajectory) !=
        ConstraintChecker::Result::VALID) {
      const std::string msg = "Current planning trajectory is not valid.";
      AERROR << msg;
      return Status(ErrorCode::PLANNING_ERROR, msg);
    }
  }

  reference_line_info->SetTrajectory(trajectory);
  reference_line_info->SetDrivable(true);
  return Status::OK();
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108

ret = task->Execute(frame, reference_line_info);依次调用task_list中的task。

Task

还是以Lanefollow为例，Task的配置部分在此处。modules/planning/conf/scenario/lane_follow_config.pb.txt

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: 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
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18

参考以及推荐阅

[1] Apollo星火计划学习笔记——第七讲自动驾驶规划技术原理1
[2] Apollo Planning决策规划代码详细解析 (1)：Scenario选择
[3] apollo介绍之planning模块(四)
[4] 解析百度Apollo之决策规划模块
[5] Apollo规划模块（一）：scenario
[6] Apollo星火计划学习笔记——Apollo决策规划技术详解及实现（以交通灯场景检测为例）
[7] Apollo Planning 模块