Apollo Planning模块源代码分析

原创

已于 2022-05-14 17:16:26 修改 · 3.8w 阅读

439

标签

#apollo #Planning #源代码 #分析

于 2018-01-26 21:00:59 首次发布

本文深入剖析Apollo自动驾驶项目中的规划模块，介绍了其核心组件、工作流程、配置细节等内容。

严正声明：本文系作者davidhopper原创，未经允许，严禁转载！

Apollo项目（https://github.com/ApolloAuto/apollo）规划（Planning）模块位于命名空间：apollo::planning，其作用在于构建无人车从起点到终的局部行驶路径，具体而言，就是给定导航地图、导航路径、当前定位点、车辆状态（包括：位置、速度、加速度、底盘）、周边目标的感知及预测信息（如交通标志和障碍物等），规划模块计算出可供控制模块（Controller）执行的一条安全且舒适的行驶路径。注意，规划模块输出的路径是局部路径而非全局路径。举个简单示例加以说明，假如无人车需从长沙智能驾驶研究院行驶至长沙高铁南站，首先需借助Routing（路由寻径）模块输出全局导航路径，接下来才是规划模块基于全局导航路径进行一小段、一小段具体行驶路径的规划。规划模块内部结构及其与其他模块的交互示意如下图所示。

规划模块内部的主要类成员如下图所示：

该模块的主要执行流程如下图所示：

一、模块主入口

该模块的主入口为：modules/planning/main.cc：

APOLLO_MAIN(apollo::planning::Planning)

该宏展开后为：

 int main(int argc, char **argv) {
   
                               
    google::InitGoogleLogging(argv[0]);                        
    google::ParseCommandLineFlags(&argc, &argv, true);         
    signal(SIGINT, apollo::common::apollo_app_sigint_handler); 
    apollo::planning::Planning apollo_app_;                                           
    ros::init(argc, argv, apollo_app_.Name());                 
    apollo_app_.Spin();                                        
    return 0;                                                  
  }

Main函数完成以下工作：始化Google日志工具，使用Google命令行解析工具解析相关参数，注册接收中止信号“SIGINT”的处理函数：apollo::common::apollo_app_sigint_handler（该函数的功能十分简单，就是收到中止信号“SIGINT”后，调用ros::shutdown()关闭ROS），创建apollo::planning::Planning对象：apollo_app_，初始化ROS环境，调用apollo_app_.Spin()函数开始消息处理循环。

int ApolloApp::Spin() {
   
   
  ros::AsyncSpinner spinner(callback_thread_num_);
  auto status = Init();
  if (!status.ok()) {
   
   
    AERROR << Name() << " Init failed: " << status;
    ReportModuleStatus(apollo::hmi::ModuleStatus::UNINITIALIZED);
    return -1;
  }
  ReportModuleStatus(apollo::hmi::ModuleStatus::INITIALIZED);
  status = Start();
  if (!status.ok()) {
   
   
    AERROR << Name() << " Start failed: " << status;
    ReportModuleStatus(apollo::hmi::ModuleStatus::STOPPED);
    return -2;
  }
  ExportFlags();
  ReportModuleStatus(apollo::hmi::ModuleStatus::STARTED);
  spinner.start();
  ros::waitForShutdown();
  Stop();
  ReportModuleStatus(apollo::hmi::ModuleStatus::STOPPED);
  AINFO << Name() << " exited.";
  return 0;
}

apollo::planning::Planning类的继承关系见下图：

基本时序如下图所示：

现在来看ApolloApp::Spin()函数内部，首先创建ros::AsyncSpinner对象spinner，监控用户操作。之后调用虚函数：Init()（实际调用apollo::planning::Planning::Init()）执行初始化。

Status Planning::Init() {
   
   
  hdmap_ = apollo::hdmap::HDMapUtil::BaseMapPtr();
  CHECK(hdmap_) << "Failed to load map file:" << apollo::hdmap::BaseMapFile();

  CHECK(apollo::common::util::GetProtoFromFile(FLAGS_planning_config_file,
                                               &config_))
      << "failed to load planning config file " << FLAGS_planning_config_file;
  if (!AdapterManager::Initialized()) {
   
   
    AdapterManager::Init(FLAGS_planning_adapter_config_filename);
  }
  if (AdapterManager::GetLocalization() == nullptr) {
   
   
    std::string error_msg("Localization is not registered");
    AERROR << error_msg;
    return Status(ErrorCode::PLANNING_ERROR, error_msg);
  }
  if (AdapterManager::GetChassis() == nullptr) {
   
   
    std::string error_msg("Chassis is not registered");
    AERROR << error_msg;
    return Status(ErrorCode::PLANNING_ERROR, error_msg);
  }
  if (AdapterManager::GetRoutingResponse() == nullptr) {
   
   
    std::string error_msg("RoutingResponse is not registered");
    AERROR << error_msg;
    return Status(ErrorCode::PLANNING_ERROR, error_msg);
  }
  if (AdapterManager::GetRoutingRequest() == nullptr) {
   
   
    std::string error_msg("RoutingRequest is not registered");
    AERROR << error_msg;
    return Status(ErrorCode::PLANNING_ERROR, error_msg);
  }
  if (FLAGS_enable_prediction && AdapterManager::GetPrediction() == nullptr) {
   
   
    std::string error_msg("Enabled prediction, but no prediction not enabled");
    AERROR << error_msg;
    return Status(ErrorCode::PLANNING_ERROR, error_msg);
  }
  if (FLAGS_enable_traffic_light &&
      AdapterManager::GetTrafficLightDetection() == nullptr) {
   
   
    std::string error_msg("Traffic Light Detection is not registered");
    AERROR << error_msg;
    return Status(ErrorCode::PLANNING_ERROR, error_msg);
  }
  ReferenceLineProvider::instance()->Init(
      hdmap_, config_.qp_spline_reference_line_smoother_config());

  RegisterPlanners();
  planner_ = planner_factory_.CreateObject(config_.planner_type());
  if (!planner_) {
   
   
    return Status(
        ErrorCode::PLANNING_ERROR,
        "planning is not initialized with config : " + config_.DebugString());

最低0.47元/天解锁文章