查看原文
其他

开发者说丨Planning 模块源代码分析

贺志国 Apollo开发者社区 2022-07-29



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



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

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


本文由社区荣誉布道师——贺志国撰写,对Planning模块源代码进行了详细讲解,希望这篇文给感兴趣的同学带来更多帮助。



以下,ENJOY




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


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




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


1int 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]);
10
11  // start module
12  ModuleController controller(module_args);
13  if (!controller.Init()) {
14    controller.Clear();
15    AERROR << "module start error.";
16    return -1;
17  }
18
19  apollo::cyber::WaitForShutdown();
20  controller.Clear();
21  AINFO << "exit mainboard.";
22
23  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 filename
10      module_path = common::GetAbsolutePath(dag_root_path, dag_conf);
11    } else if (dag_conf[0] == '/') {
12      // case dag conf argument var is an absolute path
13      module_path = dag_conf;
14    } else {
15      // case dag conf argument var is a relative path
16      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: " << module_path;
22    if (!LoadModule(module_path)) {
23      AERROR << "Failed to load module: " << module_path;
24      return false;
25    }
26  }
27  return true;
28}


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





使用工厂模式动态创建apollo::planning::PlanningComponent类对象,应先生成一个与之对应的工厂类,并将其加入到工厂集合类std::map<:string>,>中。



进一步展开:

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<planningcomponent, apollo::cyber::componentbase>( 
5          "PlanningComponent""apollo::cyber::ComponentBase");                                                   
6    }                                                                         
7  };                                                                          
8  static ProxyType__COUNTER__ g_register_class___COUNTER__;                     
9  }
</planningcomponent, apollo::cyber::componentbase>


创建一个模板类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: " << path;
5    return false;
6  }
7  return LoadModule(dag_config);
8}


上述函数从磁盘配置文件读取配置信息,并调用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      }
15
16      if (!base->Initialize(component.config())) {
17        return false;
18      }
19      component_list_.emplace_back(std::move(base));
20    }
21
22    // ...
23  }
24  return true;
25}


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





规划分为两种模式:车道规划导航规划,包含四种具体规划算法:PublicRoadPlanner、LatticePlanner、NaviPlanner、RTKPlanner,具体类图如下所示:




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



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


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



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


1#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:48
10#4  0x00007f8c067f1732 in apollo::planning::scenario::Scenario::Process (
11    this=0x7f8c2801bf20, planning_init_point=..., frame=0x7f8c38029f70
12    at modules/planning/scenarios/scenario.cc:76
13#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:51
16#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:436
19#7  0x00007f8c19ee40fa in apollo::planning::OnLanePlanning::RunOnce (
20    this=0x237f3b0, local_view=..., ptr_trajectory_pb=0x7f8b9a5fbed0) at modules/planning/on_lane_planning.cc:304
21#8  0x00007f8c1ab0d494 in apollo::planning::PlanningComponent::Proc (
22    this=0x1d0f310, prediction_obstacles=std::shared_ptr (count 4, weak 00x7f8b840164f8
23    chassis=std::shared_ptr (count 4, weak 00x7f8b84018a08
24    localization_estimate=std::shared_ptr (count 4, weak 00x7f8b8400d3b8) at modules/planning/planning_component.cc:134
25#9  0x00007f8c1abb46c4 in apollo::cyber::Component<apollo::prediction::predictionobstacles, < span="">
26    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:291
29#10 0x00007f8c1aba2698 in apollo::cyber::Component<apollo::prediction::predictionobstacles, < span="">
30    apollo::canbus::Chassis, apollo::localization::LocalizationEstimate, apollo::cyber::NullType>::Initialize(
31    apollo::cyber::proto::ComponentConfig const&)::{lambda(std::shared_ptr<apollo::prediction::predictionobstacles> const&,     
32    std::shared_ptr<:canbus::chassis> const&, std::shared_ptr<apollo::localization::localizationestimate> const&)#2}::operator()
33    (std::shared_ptr<apollo::prediction::predictionobstacles> const&, std::shared_ptr<:canbus::chassis> const&, 
34    std::shared_ptr<apollo::localization::localizationestimate> 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:378
37#11 0x00007f8c1abb4ad2 in apollo::cyber::croutine::RoutineFactory apollo::cyber::croutine::CreateRoutineFactory
38    <apollo::prediction::predictionobstacles, apollo::canbus::chassis, apollo::localization::localizationestimate, 
39    apollo::cyber::Component<apollo::prediction::predictionobstacles, apollo::canbus::chassis, 
40    apollo::localization::LocalizationEstimate, apollo::cyber::NullType>::Initialize(
41    apollo::cyber::proto::ComponentConfig const&)::{lambda(std::shared_ptr<apollo::prediction::predictionobstacles> const&, 
42    std::shared_ptr<:canbus::chassis> const&, std::shared_ptr<apollo::localization::localizationestimate> const&)#2}&>
43    (apollo::cyber::Component<apollo::prediction::predictionobstacles, apollo::canbus::chassis, 
44    apollo::localization::LocalizationEstimate, apollo::cyber::NullType>::Initialize(apollo::cyber::proto::ComponentConfig const&)::
45    {lambda(std::shared_ptr<apollo::prediction::predictionobstacles> const&, std::shared_ptr<:canbus::chassis> const&, 
46    std::shared_ptr<apollo::localization::localizationestimate> const&)#2}&, 
47    std::shared_ptr<apollo::cyber::data::datavisitor<apollo::prediction::predictionobstacles, 
48    apollo::canbus::Chassis, apollo::localization::LocalizationEstimate, apollo::cyber::NullType> > const&)::
49    {lambda()#1}::operator()() const::{lambda()#1}::operator()() const (__closure=0x2059a420) at ./cyber/croutine/routine_factory.h:108
50#12 0x00007f8c1ac0466a in std::_Function_handler<void (), apollo::cyber::croutine::routinefactory < span="">
51apollo::cyber::croutine::CreateRoutineFactory<apollo::prediction::predictionobstacles, apollo::canbus::chassis, apollo::localization::localizationestimate, 
52apollo::cyber::Component<apollo::prediction::predictionobstacles, apollo::canbus::chassis, apollo::localization::localizationestimate, 
53apollo::cyber::NullType>::Initialize(apollo::cyber::proto::ComponentConfig const&)::{lambda(std::shared_ptr<apollo::prediction::predictionobstacles> const&, 
54std::shared_ptr<:canbus::chassis> const&, std::shared_ptr<apollo::localization::localizationestimate> const&)#2}&>
55(apollo::cyber::Component<apollo::prediction::predictionobstacles, apollo::canbus::chassis, apollo::localization::localizationestimate, 
56apollo::cyber::NullType>::Initialize(apollo::cyber::proto::ComponentConfig const&)::{lambda(std::shared_ptr<apollo::prediction::predictionobstacles> const&, 
57std::shared_ptr<:canbus::chassis> const&, std::shared_ptr<apollo::localization::localizationestimate> const&)#2}&, 
58std::shared_ptr<apollo::cyber::data::datavisitor<apollo::prediction::predictionobstacles, apollo::canbus::chassis, apollo::localization::localizationestimate, 
59apollo::cyber::NullType> > const&)::{lambda()#1}::operator()() const::{lambda()#1}>::_M_invoke(std::_Any_data const&) (__functor=...) at 
60/usr/include/c++/4.8/functional:2071
61#13 0x00007f8c5f5b86e8 in std::function::operator()() const (this=0x205f1160) at /usr/include/c++/4.8/functional:2471
62#14 0x00007f8c57560cbc in apollo::cyber::croutine::CRoutine::Run (this=0x205f1148) at ./cyber/croutine/croutine.h:143
63#15 0x00007f8c5755ff55 in apollo::cyber::croutine::(anonymous namespace)::CRoutineEntry (arg=0x205f1148) at cyber/croutine/croutine.cc:43
</apollo::cyber::data::datavisitor<apollo::prediction::predictionobstacles, apollo::canbus::chassis, apollo::localization::localizationestimate, 
</apollo::localization::localizationestimate></apollo::prediction::predictionobstacles></apollo::prediction::predictionobstacles, apollo::canbus::chassis, apollo::localization::localizationestimate, 
</apollo::localization::localizationestimate></apollo::prediction::predictionobstacles></apollo::prediction::predictionobstacles, apollo::canbus::chassis, apollo::localization::localizationestimate, 
</apollo::prediction::predictionobstacles, apollo::canbus::chassis, apollo::localization::localizationestimate, 
</void (), apollo::cyber::croutine::routinefactory <></apollo::cyber::data::datavisitor<apollo::prediction::predictionobstacles, 
</apollo::localization::localizationestimate></apollo::prediction::predictionobstacles></apollo::prediction::predictionobstacles, apollo::canbus::chassis, 
</apollo::localization::localizationestimate></apollo::prediction::predictionobstacles></apollo::prediction::predictionobstacles, apollo::canbus::chassis, 
</apollo::prediction::predictionobstacles, apollo::canbus::chassis, apollo::localization::localizationestimate, 
</apollo::localization::localizationestimate></apollo::prediction::predictionobstacles></apollo::localization::localizationestimate></apollo::prediction::predictionobstacles></apollo::prediction::predictionobstacles, <></apollo::prediction::predictionobstacles, <>


所有规划算法共用的流程略去不表,与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::ObserveScenarioManager::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}





* 以上内容为开发者原创,不代表百度官方言论。

内容来自CSDN

https://blog.csdn.net/davidhopper/article/details/893603855,欢迎大家收藏点赞。已获开发者授权,原文地址请戳阅读原文。






您可能也对以下帖子感兴趣

文章有问题?点此查看未经处理的缓存