如何进行Planning 模块源代码分析

本篇文章给大家分享的是有关如何进行Planning 模块源代码分析，小编觉得挺实用的，因此分享给大家学习，希望大家阅读完这篇文章后可以有所收获，话不多说，跟着小编一起来看看吧。

规划（Planning）模块位于命名空间：apollo::planning，其作用在于构建无人车从起点到终点的局部行驶路径，具体而言，就是给定导航地图、导航路径、当前定位点、车辆状态、 周边目标的感知及预测信息，规划模块计算出可供控制模块执行的一条安全且舒适的行驶路径。

规划模块输出的路径是局部路径而非全局路径。举例，如无人车需从长沙智能驾驶研究院行驶至长沙高铁南站，首先需借助Routing模块输出全局导航路径，接下来才是规划模块基于全局导航路径进行一小段、一小段具体行驶路径的规划。

规划模块的作用是根据感知预测的结果，当前的车辆信息和路况规划出一条车辆能够行驶的轨迹，这个轨迹会交给控制模块，控制模块通过油门，刹车和方向盘使得车辆按照规划的轨迹运行。

前言

规划模块的轨迹是短期轨迹，即车辆短期内行驶的轨迹，长期轨迹是Routing模块规划出的导航轨迹，即起点到目的地的轨迹，规划模块会先生成导航轨迹，然后根据导航轨迹和路况的情况，沿着短期轨迹行驶，直到目的地。

规划模块内部结构及其与其他模块的交互示意如下图所示。

模块主入口

根据各功能模块的启动过程的分析，Planning模块的主入口为：

int main(int argc, char** argv) { 2  google::SetUsageMessage("we use this program to load dag and run user apps."); 3 4  // parse the argument 5  ModuleArgument module_args; 6  module_args.ParseArgument(argc, argv); 7 8  // initialize cyber 9  apollo::cyber::Init(argv[0]);1011  // start module12  ModuleController controller(module_args);13  if (!controller.Init()) {14    controller.Clear();15    AERROR << "module start error.";16    return -1;17  }1819  apollo::cyber::WaitForShutdown();20  controller.Clear();21  AINFO << "exit mainboard.";2223  return 0;24}

Main函数十分简单，首先是解析参数，初始化Cyber环境，接下来创建一个ModuleController类对象controller，之后调用controller.Init()启动相关功能模块。进入Cyber RT的消息循环，等待cyber::WaitForShutdown()返回，清理资源并退出Main函数。ModuleController::Init()函数内部调用了ModuleController::LoadAll()函数：

1bool ModuleController::LoadAll() { 2  const std::string work_root = common::WorkRoot(); 3  const std::string current_path = common::GetCurrentPath(); 4  const std::string dag_root_path = common::GetAbsolutePath(work_root, "dag"); 5 6  for (auto& dag_conf : args_.GetDAGConfList()) { 7    std::string module_path = ""; 8    if (dag_conf == common::GetFileName(dag_conf)) { 9      // case dag conf argument var is a filename10      module_path = common::GetAbsolutePath(dag_root_path, dag_conf);11    } else if (dag_conf[0] == '/') {12      // case dag conf argument var is an absolute path13      module_path = dag_conf;14    } else {15      // case dag conf argument var is a relative path16      module_path = common::GetAbsolutePath(current_path, dag_conf);17      if (!common::PathExists(module_path)) {18        module_path = common::GetAbsolutePath(work_root, dag_conf);19      }20    }21    AINFO << "Start initialize dag: ">

上述函数处理一个dag_conf配置文件循环，读取配置文件中的所有dag_conf，并逐一调用bool ModuleController::LoadModule(const std::string& path)函数加载功能模块。

对象的创建过程

进一步展开：

 1#define CLASS_LOADER_REGISTER_CLASS_INTERNAL(Derived, Base, UniqueID)         \ 2  namespace {                                                                 \ 3  struct ProxyType##UniqueID {                                                \ 4    ProxyType##UniqueID() {                                                   \ 5      apollo::cyber::class_loader::utility::RegisterClass(     \ 6          #Derived, #Base);                                                   \ 7    }                                                                         \ 8  };                                                                          \ 9  static ProxyType##UniqueID g_register_class_##UniqueID;                     \10  }

将PlanningComponent代入，最终得到：

1  namespace {                                                                 2  struct ProxyType__COUNTER__ {                                                3    ProxyType__COUNTER__() {                                                   4      apollo::cyber::class_loader::utility::RegisterClass( 5          "PlanningComponent", "apollo::cyber::ComponentBase");                                                   6    }                                                                         7  };                                                                          8  static ProxyType__COUNTER__ g_register_class___COUNTER__;                     9  }

创建一个模板类utility::ClassFactory对象new_class_factrory_obj，为其添加类加载器，设置加载库的路径，将工厂类对象加入到ClassClassFactoryMap对象factory_map统一管理。通过该函数，Cyber使用工厂方法模式完成产品类对象的创建：

动态创建过程

第一部分介绍模块主入口时，提及bool ModuleController::LoadModule(const std::string& path)函数，正是该函数动态创建出了apollo::planning::PlanningComponent类对象。

函数内部调用分析如下：

1bool ModuleController::LoadModule(const std::string& path) {2  DagConfig dag_config;3  if (!common::GetProtoFromFile(path, &dag_config)) {4    AERROR << "Get proto failed, file: ">

上述函数从磁盘配置文件读取配置信息，并调用bool ModuleController::LoadModule(const DagConfig& dag_config)函数加载功能模块：

 1bool ModuleController::LoadModule(const DagConfig& dag_config) { 2  const std::string work_root = common::WorkRoot(); 3 4  for (auto module_config : dag_config.module_config()) { 5    std::string load_path; 6    // ... 7    class_loader_manager_.LoadLibrary(load_path); 8    for (auto& component : module_config.components()) { 9      const std::string& class_name = component.class_name();10      std::shared_ptr base =11          class_loader_manager_.CreateClassObj(class_name);12      if (base == nullptr) {13        return false;14      }1516      if (!base->Initialize(component.config())) {17        return false;18      }19      component_list_.emplace_back(std::move(base));20    }2122    // ...23  }24  return true;25}

工厂类对象指针找到后，使用classobj = factory->CreateObj();就顺理成章地将PlanningComponent类对象创建出来了。

具体规划算法分析

PublicRoadPlanner算法从Routing模块输出的高精地图Lane序列获得全局导航路径。

基于场景、阶段和任务的理念进行规划，优点是能合理有效地应对每种场景，易于扩充，并且基于配置文件动态增减场景、阶段及使用的任务，灵活性强；缺点是可能会遗漏一些特殊场景，但可通过不断扩充新的场景加以解决。

该算法的主要执行流程如下：

可借助GDB调试命令对上述执行流程进行更为深入的理解，例如TrafficLightProtectedStageApproach阶段的PathLaneBorrowDecider任务的调用堆栈，从下往上看，对于任意一个任务的调用流程一目了然：

#0  apollo::planning::PathLaneBorrowDecider::Process (this=0x7f8c28294460, frame=0x7f8c38029f70,  2    reference_line_info=0x7f8c3802b140) at modules/planning/tasks/deciders/path_lane_borrow_decider/path_lane_borrow_decider.cc:39 3#1  0x00007f8c0468b7c8 in apollo::planning::Decider::Execute (this=0x7f8c28294460, frame=0x7f8c38029f70,  4    reference_line_info=0x7f8c3802b140) at modules/planning/tasks/deciders/decider.cc:31 5#2  0x00007f8c065c4a01 in apollo::planning::scenario::Stage::ExecuteTaskOnReferenceLine (this=0x7f8c28293eb0,  6    planning_start_point=..., frame=0x7f8c38029f70) at modules/planning/scenarios/stage.cc:96 7#3  0x00007f8c06e721da in apollo::planning::scenario::traffic_light::TrafficLightProtectedStageApproach::Process ( 8    this=0x7f8c28293eb0, planning_init_point=..., frame=0x7f8c38029f70) at  9    modules/planning/scenarios/traffic_light/protected/stage_approach.cc:4810#4  0x00007f8c067f1732 in apollo::planning::scenario::Scenario::Process (11    this=0x7f8c2801bf20, planning_init_point=..., frame=0x7f8c38029f70) 12    at modules/planning/scenarios/scenario.cc:7613#5  0x00007f8c186e153a in apollo::planning::PublicRoadPlanner::Plan (14    this=0x23093de0, planning_start_point=..., frame=0x7f8c38029f70, 15    ptr_computed_trajectory=0x7f8b9a5fbed0) at modules/planning/planner/public_road/public_road_planner.cc:5116#6  0x00007f8c19ee5937 in apollo::planning::OnLanePlanning::Plan (17    this=0x237f3b0, current_time_stamp=1557133995.3679764, stitching_trajectory=std::vector of length 1, 18    capacity 1 = {...}, ptr_trajectory_pb=0x7f8b9a5fbed0)  at modules/planning/on_lane_planning.cc:43619#7  0x00007f8c19ee40fa in apollo::planning::OnLanePlanning::RunOnce (20    this=0x237f3b0, local_view=..., ptr_trajectory_pb=0x7f8b9a5fbed0) at modules/planning/on_lane_planning.cc:30421#8  0x00007f8c1ab0d494 in apollo::planning::PlanningComponent::Proc (22    this=0x1d0f310, prediction_obstacles=std::shared_ptr (count 4, weak 0) 0x7f8b840164f8, 23    chassis=std::shared_ptr (count 4, weak 0) 0x7f8b84018a08, 24    localization_estimate=std::shared_ptr (count 4, weak 0) 0x7f8b8400d3b8) at modules/planning/planning_component.cc:13425#9  0x00007f8c1abb46c4 in apollo::cyber::Component26    apollo::canbus::Chassis, apollo::localization::LocalizationEstimate, apollo::cyber::NullType>::Process (this=0x1d0f310, 27    msg0=std::shared_ptr (count 4, weak 0) 0x7f8b840164f8, msg1=std::shared_ptr (count 4, weak 0) 0x7f8b84018a08, 28    msg2=std::shared_ptr (count 4, weak 0) 0x7f8b8400d3b8) at ./cyber/component/component.h:29129#10 0x00007f8c1aba2698 in apollo::cyber::Component30    apollo::canbus::Chassis, apollo::localization::LocalizationEstimate, apollo::cyber::NullType>::Initialize(31    apollo::cyber::proto::ComponentConfig const&)::{lambda(std::shared_ptr const&,     32    std::shared_ptr const&, std::shared_ptr const&)#2}::operator()33    (std::shared_ptr const&, std::shared_ptr const&, 34    std::shared_ptr const&) const (__closure=0x2059a430, 35    msg0=std::shared_ptr (count 4, weak 0) 0x7f8b840164f8, msg1=std::shared_ptr (count 4, weak 0) 0x7f8b84018a08,     36    msg2=std::shared_ptr (count 4, weak 0) 0x7f8b8400d3b8) at ./cyber/component/component.h:37837#11 0x00007f8c1abb4ad2 in apollo::cyber::croutine::RoutineFactory apollo::cyber::croutine::CreateRoutineFactory38    ::Initialize(41    apollo::cyber::proto::ComponentConfig const&)::{lambda(std::shared_ptr const&, 42    std::shared_ptr const&, std::shared_ptr const&)#2}&>43    (apollo::cyber::Component::Initialize(apollo::cyber::proto::ComponentConfig const&)::45    {lambda(std::shared_ptr const&, std::shared_ptr const&, 46    std::shared_ptr const&)#2}&, 47    std::shared_ptr > const&)::49    {lambda()#1}::operator()() const::{lambda()#1}::operator()() const (__closure=0x2059a420) at ./cyber/croutine/routine_factory.h:10850#12 0x00007f8c1ac0466a in std::_Function_handler51apollo::cyber::croutine::CreateRoutineFactory::Initialize(apollo::cyber::proto::ComponentConfig const&)::{lambda(std::shared_ptr const&, 54std::shared_ptr const&, std::shared_ptr const&)#2}&>55(apollo::cyber::Component::Initialize(apollo::cyber::proto::ComponentConfig const&)::{lambda(std::shared_ptr const&, 57std::shared_ptr const&, std::shared_ptr const&)#2}&, 58std::shared_ptr > const&)::{lambda()#1}::operator()() const::{lambda()#1}>::_M_invoke(std::_Any_data const&) (__functor=...) at 60/usr/include/c++/4.8/functional:207161#13 0x00007f8c5f5b86e8 in std::function::operator()() const (this=0x205f1160) at /usr/include/c++/4.8/functional:247162#14 0x00007f8c57560cbc in apollo::cyber::croutine::CRoutine::Run (this=0x205f1148) at ./cyber/croutine/croutine.h:14363#15 0x00007f8c5755ff55 in apollo::cyber::croutine::(anonymous namespace)::CRoutineEntry (arg=0x205f1148) at cyber/croutine/croutine.cc:43

所有规划算法共用的流程略去不表，与PublicRoadPlanner规划算法相关的有两处，一处是PublicRoadPlanner::Init，另一处是PublicRoadPlanner::Plan。

下面来看场景更新函数ScenarioManager::Update的代码：

1void ScenarioManager::Update(const common::TrajectoryPoint& ego_point,2                             const Frame& frame) {3  CHECK(!frame.reference_line_info().empty());4  Observe(frame);5  ScenarioDispatch(ego_point, frame);6}

该函数包含两个子函数：ScenarioManager::Observe和ScenarioManager::ScenarioDispatch，其中前者用于更新first_encountered_overlap_map_，代码如下所示：

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

以上就是如何进行Planning 模块源代码分析，小编相信有部分知识点可能是我们日常工作会见到或用到的。希望你能通过这篇文章学到更多知识。更多详情敬请关注行业资讯频道。