Apollo Planning模块源代码分析

严正声明:本文系作者davidhopper原创,未经允许,严禁转载!

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

一、模块主入口

该模块的主入口为: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类的继承关系见下图:
4
基本时序如下图所示:
5
现在来看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());
  }

  return planner_->Init(config_);
}

apollo::planning::Planning::Init()函数内部,首先获取高精地图指针,之后执行适配器管理者(AdapterManager)对象的初始化状态,接着检查AdapterManager里定位适配器(AdapterManager::GetLocalization())、底盘适配器(AdapterManager::GetChassis())、路由寻径响应适配器(AdapterManager::GetRoutingResponse())、路由寻径请求适配器(AdapterManager::GetRoutingRequest())的初始化状态,若启用预测(FLAGS_enable_prediction)则继续检查预测适配器(AdapterManager::GetPrediction())的初始化状态,若启用交通信号灯(FLAGS_enable_traffic_light)则继续检查交通信号灯适配器(AdapterManager::GetTrafficLightDetection())的初始化状态。接下来,执行参考线提供者(所谓参考线就是一条候选路径)的初始化。此后调用Planning::RegisterPlanners()将RTKReplayPlanner、EMPlanner对象的创建函数注册到计划者工厂类对象planner_factory_中。另外通过读取配置文件中给定的计划者类型,使用planner_factory_.CreateObject(config_.planner_type())动态生成所需的计划者对象(查询配置文件得知,实际生成EMPlanner对象)。最后执行计划者对象的初始化:planner_->Init(config_)apollo::planning::Planning::Init()函数内,适配器管理者的初始化过程值得深入探讨:

AdapterManager::Init(FLAGS_planning_adapter_config_filename);

首先研究初始化传入的参数:FLAGS_planning_adapter_config_filename实际上是利用Google开源库gflags宏:

DEFINE_string(planning_adapter_config_filename,
"modules/planning/conf/adapter.conf", 
"The adapter configuration file");

定义出的一个字符串变量,该变量的缺省值为:“modules/planning/conf/adapter.conf”,关于该变量的描述信息为: “The adapter configuration file”。配置文件:modules/planning/conf/adapter.conf的内容如下:

config {
  type: LOCALIZATION
  mode: RECEIVE_ONLY
  message_history_limit: 1
}
config {
  type: CHASSIS
  mode: RECEIVE_ONLY
  message_history_limit: 1
}
config {
  type: ROUTING_RESPONSE
  mode: RECEIVE_ONLY
  message_history_limit: 1
}
config {
  type: ROUTING_REQUEST
  mode: PUBLISH_ONLY
  message_history_limit: 1
}
config {
  type: PREDICTION
  mode: RECEIVE_ONLY
  message_history_limit: 10
}
config {
  type: PLANNING_TRAJECTORY
  mode: PUBLISH_ONLY
  message_history_limit: 1
}
config {
  type: TRAFFIC_LIGHT_DETECTION
  mode: RECEIVE_ONLY
  message_history_limit: 1
} 
is_ros: true

该文件表明,Planning模块配置以下几种类型的适配器:LOCALIZATION(仅接收)、CHASSIS(仅接收)、ROUTING_RESPONSE(仅接收)、ROUTING_REQUEST(仅发布)、PREDICTION(仅接收)、PLANNING_TRAJECTORY(仅发布)、TRAFFIC_LIGHT_DETECTION(仅接收),且使用ROS环境。
现在进入AdapterManager::Init(const std::string &adapter_config_filename)内部一探究竟:

void AdapterManager::Init(const std::string &adapter_config_filename) {
  // Parse config file
  AdapterManagerConfig configs;
  CHECK(util::GetProtoFromFile(adapter_config_filename, &configs))
      << "Unable to parse adapter config file " << adapter_config_filename;
  AINFO << "Init AdapterManger config:" << configs.DebugString();
  Init(configs);
}

顺藤摸瓜,继续深入AdapterManager::Init(const AdapterManagerConfig &configs)内部,前面我们通过查看配置文件adapter.conf得知,此次配置类别为:LOCALIZATION、CHASSIS、ROUTING_RESPONSE、ROUTING_REQUEST、PREDICTION、PLANNING_TRAJECTORY、TRAFFIC_LIGHT_DETECTION。

void AdapterManager::Init(const AdapterManagerConfig &configs) {
  if (Initialized()) {
    return;
  }

  instance()->initialized_ = true;
  if (configs.is_ros()) {
    instance()->node_handle_.reset(new ros::NodeHandle());
  }

  for (const auto &config : configs.config()) {
    switch (config.type()) {     
      // 省略不相关的配置类别
      ...
      case AdapterConfig::CHASSIS:
        EnableChassis(FLAGS_chassis_topic, config);
        break;
      case AdapterConfig::LOCALIZATION:
        EnableLocalization(FLAGS_localization_topic, config);
        break;
      // 省略不相关的配置类别
      ...
      case AdapterConfig::TRAFFIC_LIGHT_DETECTION:
        EnableTrafficLightDetection(FLAGS_traffic_light_detection_topic,
                                    config);
      // 省略不相关的配置类别
      ...
      case AdapterConfig::ROUTING_REQUEST:
        EnableRoutingRequest(FLAGS_routing_request_topic, config);
        break;
      case AdapterConfig::ROUTING_RESPONSE:
        EnableRoutingResponse(FLAGS_routing_response_topic, config);
        break;
      case AdapterConfig::PLANNING_TRAJECTORY:
        EnablePlanning(FLAGS_planning_trajectory_topic, config);
        break;
      case AdapterConfig::PREDICTION:
        EnablePrediction(FLAGS_prediction_topic, config);
        break;
      // 省略不相关的配置类别
      ...      
      default:
        AERROR << "Unknown adapter config type!";
        break;
}

上述代码中的FLAGS_chassis_topic、FLAGS_localization_topic、FLAGS_traffic_light_detection_topic、FLAGS_routing_request_topic、FLAGS_routing_response_topic、FLAGS_planning_trajectory_topic、FLAGS_prediction_topic是利用DEFINE_string宏定义出来的几个字符串变量,其缺省值分别为:
“/apollo/canbus/chassis”
“/apollo/localization/pose”
“/apollo/perception/traffic_light”
“/apollo/routing_request”
“/apollo/routing_response”
“/apollo/prediction”
代码中出现了几个函数:
EnableChassis、
EnableLocalization、
EnableTrafficLightDetection、
EnableRoutingRequest、
EnableRoutingResponse、
EnablePlanning、
EnablePrediction,
但我们找遍AdapterManager类内部,也无法发现它们的定义,这是从哪里冒出来的呢?其实这是宏
REGISTER_ADAPTER(Chassis)、
REGISTER_ADAPTER(Localization)、
REGISTER_ADAPTER (TrafficLightDetection)、
REGISTER_ADAPTER(RoutingRequest)、
REGISTER_ADAPTER (RoutingResponse)、
REGISTER_ADAPTER(Planning)、
REGISTER_ADAPTER(Prediction)
的杰作。
下面以REGISTER_ADAPTER(Planning)为例进行分析:命名空间Apollo::common::adapter内的适配器管理者类AdapterManager使用宏REGISTER_ADAPTER (Planning)注册了一个PlanningAdapter类对象及与其相关的变量、函数。这个PlanningAdapter类是何方神圣?请看文件:modules/common/adapters/message_adapters.h内的这条语句:

using PlanningAdapter = Adapter<planning::ADCTrajectory>;

原来PlanningAdapter类是模板类Adapter<planning::ADCTrajectory>的一个别名。
再来看宏REGISTER_ADAPTER(Planning)展开后的实际代码,是不是找到了我们关注的函数:EnablePlanning(const std::string &topic_name, const AdapterConfig &config)?分析该函数内部可知其功能有两个:一是订阅接收消息FLAGS_planning_trajectory_topic,并绑定该消息的处理函数为PlanningAdapter::OnReceive;二是将匿名函数(或称Lambda表达式,不熟悉的同学可查阅C++11标准)[this]() { Planning_->Observe(); }作为规划适配器PlanningAdapter的观察函数存储到函数对象数组:std::vector<std::function<void()>> observers_内部,提供给相关代码调用。

public:                                                                       
  static void EnablePlanning(const std::string &topic_name,                      
                           const AdapterConfig &config) {                      
    CHECK(config.message_history_limit() > 0)                                  
        << "Message history limit must be greater than 0";                     
    instance()->InternalEnablePlanning(topic_name, config);                      
  }                                                                            
  static PlanningAdapter *GetPlanning() {                                          
    return instance()->InternalGetPlanning();                                    
  }                                                                            
  static AdapterConfig &GetPlanningConfig() {                                  
    return instance()->Planningconfig_;                                          
  }                                                                            
  static bool FeedPlanningFile(const std::string &proto_file) {                
    if (!instance()->Planning_) {                                                
      AERROR << "Initialize adapter before feeding protobuf";                  
      return false;                                                            
    }                                                                          
    return GetPlanning()->FeedFile(proto_file);                                  
  }                                                                            
  static void PublishPlanning(const PlanningAdapter::DataType &data) {             
    instance()->InternalPublishPlanning(data);                                   
  }                                                                            
  template <typename T>                                                        
  static void FillPlanningHeader(const std::string &module_name, T *data) {    
    static_assert(std::is_same<PlanningAdapter::DataType, T>::value,             
                  "Data type must be the same with adapter's type!");          
    instance()->Planning_->FillHeader(module_name, data);                        
  }                                                                            
  static void AddPlanningCallback(PlanningAdapter::Callback callback) {          
    CHECK(instance()->Planning_)                                                 
        << "Initialize adapter before setting callback";                       
    instance()->Planning_->AddCallback(callback);                                
  }                                                                            
  template <class T>                                                           
  static void AddPlanningCallback(                                             
      void (T::*fp)(const PlanningAdapter::Dssage_history_limit())); 
    if (config.mode() != AdapterConfig::PUBLISH_ONLY && IsRos()) {             
      Planningsubscriber_ =                                                      
          node_handle_->subscribe(topic_name, config.message_history_limit(),  
                                  &PlanningAdapter::OnReceive, Planning_.get());   
    }                                                                          
    if (config.mode() != AdapterConfig::RECEIVE_ONLY && IsRos()) {             
      Planningpublisher_ = node_handle_->advertise<PlanningAdapter::DataType>(     
          topic_name, config.message_history_limit(), config.latch());         
    }                                                                          
                                                                               
    observers_.push_back([this]() { Planning_->Observe(); });                    
    Planningconfig_ = config;                                                    
  }                                                                            
  PlanningAdapter *InternalGetPlanning() { return Planning_.get(); }                 
  void InternalPublishPlanning(const PlanningAdapter::DataType &data) {            
    /* Only publish ROS msg if node handle is initialized. */                  
    if (IsRos()) {                                                             
      if (!Planningpublisher_.getTopic().empty()) {                              
        Planningpublisher_.publish(data);                                        
      } else {                                                                 
        AERROR << "Planning" << " is not valid.";                                   
      }                                                                        
    } else {                                                                   
      /* For non-ROS mode, just triggers the callback. */                      
      if (Planning_) {                                                           
        Planning_->OnReceive(data);                                              
      } else {                                                                 
        AERROR << "Planning" << " is null.";                                        
      }                                                                        
    }                                                                          
    Planning_->SetLatestPublished(data);                                         
  }ataType &data), T *obj) {            
    AddPlanningCallback(std::bind(fp, obj, std::placeholders::_1));            
  }                                                                            
  template <class T>                                                           
  static void AddPlanningCallback(                                             
      void (T::*fp)(const PlanningAdapter::DataType &data)) {                    
    AddPlanningCallback(fp);                                                   
  }                                                                            
  /* Returns false if there's no callback to pop out, true otherwise. */       
  static bool PopPlanningCallback() {                                          
    return instance()->Planning_->PopCallback();                                 
  }                                                                            
                                                                               
 private:                                                                      
  std::unique_ptr<PlanningAdapter> Planning_;                                      
  ros::Publisher Planningpublisher_;                                             
  ros::Subscriber Planningsubscriber_;                                           
  AdapterConfig Planningconfig_;                                                 
                                                                               
  void InternalEnablePlanning(const std::string &topic_name,                     
                            const AdapterConfig &config) {                     
    Planning_.reset(                                                             
        new PlanningAdapter("Planning", topic_name, config.mePublishssage_history_limit())); 
    if (config.mode() != AdapterConfig::PUBLISH_ONLY && IsRos()) {             
      Planningsubscriber_ =                                                      
          node_handle_->subscribe(topic_name, config.message_history_limit(),  
                                  &PlanningAdapter::OnReceive, Planning_.get());   
    }                                                                          
    if (config.mode() != AdapterConfig::RECEIVE_ONLY && IsRos()) {             
      Planningpublisher_ = node_handle_->advertise<PlanningAdapter::DataType>(     
          topic_name, config.message_history_limit(), config.latch());         
    }                                                                          
                                                                               
    observers_.push_back([this]() { Planning_->Observe(); });                    
    Planningconfig_ = config;                                                    
  }                                                                            
  PlanningAdapter *InternalGetPlanning() { return Planning_.get(); }                 
  void InternalPublishPlanning(const PlanningAdapter::DataType &data) {            
    /* Only publish ROS msg if node handle is initialized. */                  
    if (IsRos()) {                                                             
      if (!Planningpublisher_.getTopic().empty()) {                              
        Planningpublisher_.publish(data);                                        
      } else {                                                                 
        AERROR << "Planning" << " is not valid.";                                   
      }                                                                        
    } else {                                                                   
      /* For non-ROS mode, just triggers the callback. */                      
      if (Planning_) {                                                           
        Planning_->OnReceive(data);                                              
      } else {                                                                 
        AERROR << "Planning" << " is null.";                                        
      }                                                                        
    }                                                                          
    Planning_->SetLatestPublished(data);                                         
  }

现在重新回到ApolloApp::Spin()函数的分析。初始化完成后继续调用虚函数:Start()(实际调用apollo::planning::Planning::Start())开始运行;之后使用spinner来监听各种信号,并调用相关处理函数;最后,在收到ros::waitForShutdown()信号后,调用Stop()(实际调用apollo::planning::Planning::Stop())完成资源清理退出。调试信息的输出贯穿于整个函数过程。

二、适配器管理者类(AdapterManager)分析

AdapterManager类不属于规划模块,但它是所有消息适配器的管理者,这里对其作一初步分析。顾名思义,AdapterManager是所有适配器的管理容器。什么是适配器,Apollo项目为什么要使用适配器?第一个问题很好理解,借用网上经常提到的一个例子,中国电压标准是220V,美国电压标准是110V,中国电器要在美国使用,必须借助一个变压器,这个变压器就是适配器。另外一个例子是,中国插座标准是扁孔,欧洲插座标准是圆孔,中国的电源插头到欧洲使用,必须借助一个插孔转接器,这个转接器也是适配器。以上只是对适配器的形象描述,C++中用到的适配器,自然是来自于鼎鼎有名的GOF名著《Design Patterns: Elements of Reusable Object-Oriented Software》中提到的Adapter设计模式。关于该模式的描述,网上有一堆文章,此处不再赘述。关于第二个问题,Apollo项目各模块内部的消息传递使用Google Protocol Buffer格式,各模块之间的底层消息传递目前使用ROS机制,今后可能会替换为百度自研消息机制。不管Apollo项目各模块之间的底层消息传递机制如何改变,我们必须确保各模块内部的消息格式不变,即使用Google Protocol Buffer格式,否则会因为底层消息传递机制的修改,导致各模块内部涉及消息处理的代码被迫修改,这既严重破坏各模块代码的独立性和正确性,也严重影响项目开发效率。这就是Apollo创建众多Adapter及其管理者AdapterManager的根本原因。
Apollo 2.0版本目前定义了以下适配器:

using ChassisAdapter = Adapter<::apollo::canbus::Chassis>;
using ChassisDetailAdapter = Adapter<::apollo::canbus::ChassisDetail>;
using ControlCommandAdapter = Adapter<control::ControlCommand>;
using GpsAdapter = Adapter<apollo::localization::Gps>;
using ImuAdapter = Adapter<localization::Imu>;
using RawImuAdapter = Adapter<apollo::drivers::gnss::Imu>;
using LocalizationAdapter = Adapter<apollo::localization::LocalizationEstimate>;
using MonitorAdapter = Adapter<apollo::common::monitor::MonitorMessage>;
using PadAdapter = Adapter<control::PadMessage>;
using PerceptionObstaclesAdapter = Adapter<perception::PerceptionObstacles>;
using PlanningAdapter = Adapter<planning::ADCTrajectory>;
using PointCloudAdapter = Adapter<::sensor_msgs::PointCloud2>;
using ImageShortAdapter = Adapter<::sensor_msgs::Image>;
using ImageLongAdapter = Adapter<::sensor_msgs::Image>;
using PredictionAdapter = Adapter<prediction::PredictionObstacles>;
using DriveEventAdapter = Adapter<DriveEvent>;
using TrafficLightDetectionAdapter = Adapter<perception::TrafficLightDetection>;
using RoutingRequestAdapter = Adapter<routing::RoutingRequest>;
using RoutingResponseAdapter = Adapter<routing::RoutingResponse>;
using RelativeOdometryAdapter =
    Adapter<calibration::republish_msg::RelativeOdometry>;
using InsStatAdapter = Adapter<drivers::gnss::InsStat>;
using InsStatusAdapter = Adapter<gnss_status::InsStatus>;
using GnssStatusAdapter = Adapter<gnss_status::GnssStatus>;
using SystemStatusAdapter = Adapter<apollo::monitor::SystemStatus>;
using MobileyeAdapter = Adapter<drivers::Mobileye>;
using DelphiESRAdapter = Adapter<drivers::DelphiESR>;
using ContiRadarAdapter = Adapter<drivers::ContiRadar>;
using CompressedImageAdapter = Adapter<sensor_msgs::CompressedImage>;
using GnssRtkObsAdapter = Adapter<apollo::drivers::gnss::EpochObservation>;
using GnssRtkEphAdapter = Adapter<apollo::drivers::gnss::GnssEphemeris>;
using GnssBestPoseAdapter = Adapter<apollo::drivers::gnss::GnssBestPose>;
using LocalizationMsfGnssAdapter =
    Adapter<apollo::localization::LocalizationEstimate>;
using LocalizationMsfLidarAdapter =
    Adapter<apollo::localization::LocalizationEstimate>;
using LocalizationMsfSinsPvaAdapter =
Adapter<apollo::localization::IntegSinsPva>;

注意,AdapterManager类是使用宏DECLARE_SINGLETON(AdapterManager)定义的单实例类,获取该类对象请使用AdapterManager::instance()

(一)数据成员

AdapterManager类的数据成员除了包含上述各个适配器对象及其关联的消息发布、订阅对象外,还有如下重要成员:

  1. ROS节点句柄(std::unique_ptr<ros::NodeHandle> node_handle_
    内置的ROS节点句柄。
  2. 观测回调函数数组(std::vector<std::function<void()>> observers_
    各个适配器提供的回调函数数组。
  3. 初始化标志(bool initialized_
    记录该类是否完成初始化。

(二)重要成员函数

  1. Init函数
    前文已描述,就是完成各类适配器的注册。
  2. Observe函数
    调用各适配器提供的回调函数获取当前各模块的观测数据。
  3. CreateTimer函数
    创建一个定时器,在定时器内定时调用传入的回调函数。例如在Planning模块Planning::Start函数中,创建一个定时器来周期性地执行规划任务。
Status Planning::Start() {
  timer_ = AdapterManager::CreateTimer(
      ros::Duration(1.0 / FLAGS_planning_loop_rate), &Planning::OnTimer, this);
  ReferenceLineProvider::instance()->Start();
  AINFO << "Planning started";
  return Status::OK();
}

三、规划类(Planning)分析

Planning类是规划模块的主要类,它将GPS和IMU提供的信息作为输入,处理后生成规划信息(包括路径和速度信息),提供给控制模块使用。

(一)数据成员

该类数据成员包括:规划者工厂类对象(planner_factory_)、规划配置类对象(config_)、高精地图类对象指针(hdmap_)、帧类对象指针(frame_)、规划者类对象指针(planner_)、可发布轨迹类对象指针(last_publishable_trajectory_)、计时器对象(timer_)。注意frame_、planner_和last_publishable_trajectory_使用C++标准库的唯一性智能指针unique_ptr包装,其优点在于:一是不用人工管理内存;二是上述指针由Planning类对象专享,确保数据的唯一性。

  1. 规划者工厂类对象(apollo::common::util::Factory<PlanningConfig::PlannerType, Planner> planner_factory_
    该对象的职能是注册所有的规划者类(目前注册了两个:RTK重放规划者RTKReplayPlanner,用于轨迹回放,无实际用途;最大期望规划者EMPlanner,可实用),并根据配置信息生成合适的规划者类对象(当前配置文件使用EMPlanner)。
  2. 规划配置类对象(PlanningConfig config_
    该对象的职能是读取配置文件信息,给规划类对象提供合适的参数。
  3. 高精地图类对象指针(hdmap::HDMap* hdmap_
    该指针用于保存高精地图,给规划类对象提供导航地图及车辆定位点等信息。
  4. 帧类对象指针(std::unique_ptr<Frame> frame_
    该指针用于保存帧(Frame)对象,规划类对象所有的决策都在该对象上完成。
  5. 规划者类对象指针(std::unique_ptr<Planner> planner_
    该指针用于保存具体规划者对象。
  6. 可发布轨迹类对象指针(std::unique_ptr<PublishableTrajectory> last_publishable_trajectory_
    该指针用于记录规划路径的位置和速度信息。
  7. 计时器对象(ros::Timer timer_
    该对象用于定期监控各类操作用户,调用相关处理函数。

(二)重要成员函数

该类主要包含三个重载函数:Init、Start、Stop(均为基类ApolloApp的虚函数)和几个功能函数:InitFrame、RunOnce、Plan,其他函数不太重要,如感兴趣可直接查看源代码。

  1. Init函数
    该函数完成以下工作:获取高精地图;读取配置文件获取相关参数;使用适配器管理者(apollo::common::adapterAdapterManager,Apollo项目内的所有模块如定位、底盘、预测、控制、规划等全部归其管理,类似于一个家庭管家)对象获取规划模块所需的定位、底盘、路径、交通信号灯探测、周边障碍物预测等信息;获取参考线(基于导航路径和当前位置计算得到,提供给Planning类决策的候选轨迹线);注册具体的规划者类(目前注册RTKReplayPlanner及EMPlanner);使用工厂创建模式生成具体的规划者类对象(通过查询配置文件获知,实际生成了EMPlanner类对象);最后完成规划者类对象的初始化。
  2. Start函数
    Start函数很简单,主要完成两个工作:一是启动参考线信息的获取;二是创建一个定时器,按给定间隔调用Planning::OnTimer函数,Planning::OnTimer函数的功能更为简单,即调用Planning::RunOnce完成实际的规划工作。
  3. Stop函数
    Stop函数的功能也很简单:一是结束参考线信息的获取;二是清空可发布轨迹类对象指针、帧类对象指针、规划者类对象指针。
  4. RunOnce函数
    现在来观察RunOnce函数完成的具体工作。该函数首先调用AdapterManager类的相关函数来获取规划所需的所需的定位、底盘信息;接着基于上述信息,调用common::VehicleStateProvider::instance()->Update函数更新车辆自身的状态;之后调用Planning::InitFrame函数初始化规划帧;如果一切正常,就调用Planning::Plan函数执行实际的规划;最后调用Planning::PublishPlanningPb函数将计算出的规划路径发布给控制模块。另外,整个函数针对各种可能出现的错误均做出相应处理。
  5. InitFrame函数
    该函数功能比较简单,首先调用Frame::reset函数重新创建一个Frame类对象(如果之前不存在,直接创建;若已经存在一个,则先删除后创建),然后设置预测信息,最后调用Frame::Init函数完成当前帧的初始化。
  6. Plan函数
    Plan函数是规划模块最核心的函数,用来完成实际的路径规划。该函数针对Frame类对象指针frame_提供的多条候选参考线信息:reference_line_info,调用规划者对象的Plan函数(planner_->Plan,当前实现就是调用EMPlanner::Plan函数)决策得到可用参考线信息集(可能不止一条);接着调用Frame::FindDriveReferenceLineInfo函数从可用参考线信息集获取最优的参考线信息:best_reference_line;最后调用ReferenceLineInfo::ExportDecision函数,将best_reference_line转化可供控制模块使用的驾驶决策,包括:路径、速度等。整个函数存在很多输出调试信息的语句,注意不要被他们所干扰,影响对核心思路的理解。

四、期望最大规划者类(EMPlanner)分析

EMPlanner类是具体的规划实施类,它基于高精地图、导航路径及障碍物信息作出实际的驾驶决策,包括路径、速度等方面。首先使用DP(动态规划)方法确定初始的路径和速度,再利用QP(二次规划)方法进一步优化路径和速度,以得到一条更平滑的轨迹,既满足舒适性,又方便车辆操纵。EMPlanner类的继承关系如下图所示:
6

(一)数据成员

该类数据成员包括:任务工厂类对象(task_factory_)、任务类对象数组(tasks_)。

  1. 任务工厂类对象(apollo::common::util::Factory<TaskType, Task> task_factory_
    该对象的负责是注册所有的任务类,并根据配置信息生成合适的任务类对象。
  2. 任务类对象数组(std::vector<std::unique_ptr<Task>> tasks_
    该数组的职能是保存各个任务类对象的智能指针。

(二)重要成员函数

该类主要包含两个重载函数:Init、Plan(基类Planner的虚函数)和两个功能函数:GenerateInitSpeedProfile、GenerateSpeedHotStart,其他函数是辅助函数,如感兴趣可直接查看源代码。

  1. Init函数
    该函数完成以下工作:注册各种任务类(目前已注册:交通决策器TrafficDecider、动态规划多项式路径优化器DpPolyPathOptimizer、路径决策器PathDecider、动态规划ST坐标速度优化器DpStSpeedOptimizer、速度决策器SpeedDecider、二次规划样条路径优化器QpSplinePathOptimizer、二次规划样条ST坐标速度优化器QpSplineStSpeedOptimizer);使用工厂创建模式生成具体的任务类对象(通过查询配置文件modules/planning/conf/planning_config.pb.txt获知,任务类型包括: TRAFFIC_DECIDER、 DP_POLY_PATH_OPTIMIZER、PATH_DECIDER、DP_ST_SPEED_OPTIMIZER、SPEED_DECIDER、QP_SPLINE_ST_SPEED_OPTIMIZER,实际生成的任务对象分别为:TrafficDecider、DpPolyPathOptimizer、PathDecider 、DpStSpeedOptimizer、SpeedDecider、QpSplineStSpeedOptimizer类对象。注意:任务对象的创建可根据修改配置文件动态调整);最后完成各种任务类对象的初始化。决策和优化器的继承关系图如下所示:
    7
  2. Plan函数
    该函数首先对当前参考线信息(reference_line_info,即候选路径)作出判断,若其非向前直行,则加入10000的代价(cost);接下来,基于起始点和当前参考线信息,调用EMPlanner::GenerateInitSpeedProfile函数生成初始速度,若失败则继续调用EMPlanner::GenerateSpeedHotStart生成初始速度;之后是整个函数的核心,针对当前帧frame和参考线信息reference_line_info,逐一调用任务类对象数组tasks_里的优化器(当前配置文件按照以下顺序使用以下决策或优化器:TrafficDecider、DpPolyPathOptimizer、PathDecider 、DpStSpeedOptimizer、SpeedDecider、QpSplineStSpeedOptimizer)进行最优估计,计算当前参考线的最优路径和速度。若针对当前参考线无法计算出最优路径和速度,则将其代价值设为无穷大,表示当前参考线无法使用;若对当前参考线可计算出最优路径和速度,则调用ReferenceLineInfo::CombinePathAndSpeedProfile函数合并路径和速度信息,最后调用ReferenceLineInfo::setTrajectory函数更新当前参考线的轨迹,返回给上一级Planning::Plan函数使用。整个函数存在很多输出调试信息的语句,注意不要被他们所干扰,影响对核心思路的理解。
  3. GenerateInitSpeedProfile函数
    该函数看上去很复杂,实际思想比较简单:首先获取上一帧last_frame的参考线信息last_reference_line_info;之后,对当前参考线信息reference_line_info调用ReferenceLineInfo::IsStartFrom函数来判断其是否起始于上一帧参考线信息last_reference_line_info,若不是直接返回,否则继续;接着,基于上一帧参考线的速度信息生成当前帧参考线的速度信息,注意这里用到了(x, y)坐标到(s, l)坐标的转换。
  4. GenerateSpeedHotStart函数
    该函数很简单,就是将初始点的速度限制在[5.0,FLAGS_planning_upper_speed_limit]范围内,并为当前帧参考线的初始速度信息。

五、帧类(Frame)分析

Frame类用于存储规划模块用到的各类信息,包括起始点、车辆状态、参考线信息列表(即多条候选路径)、驾驶参考线信息(即实际执行的驾驶路径)、障碍物预测等。所有决策都基于当前帧进行。

(一)数据成员

  1. 帧序号(uint32_t sequence_num_
    记录当前帧的序号。
  2. 高精地图类对象指针(hdmap::HDMap* hdmap_
    保存高精地图,给规划类对象提供导航地图及车辆定位点等信息。
  3. 轨迹起始点(common::TrajectoryPoint planning_start_point_
    保存路径的起始点。
  4. 车辆状态类对象(common::VehicleState vehicle_state_
    存储车辆状态信息,包括位置、时间戳、姿态、速度、加速度、底盘及方位等。
  5. 参考线信息列表(std::list<ReferenceLineInfo> reference_line_info_
    存储多条候选路径。
  6. 驾驶参考线信息指针(cconst ReferenceLineInfo *drive_reference_line_info_
    记录实际执行驾驶路径。
  7. 预测障碍物类对象(prediction::PredictionObstacles prediction_
    存储车辆当前驾驶模式下,预测出现的障碍物。
  8. 障碍物列表(prediction::PredictionObstacles prediction_
    存储车辆周边所有的障碍物。

(二)重要成员函数

该类主要包含如下成员函数:Init、FindDriveReferenceLineInfo,其他函数是辅助函数,如感兴趣可直接查看源代码。

  1. Init函数
    该函数被Planning::InitFrame函数调用,用于完成当前帧各类信息的初始化,包括获取高精地图、车辆状态,创建预测障碍、目的地障碍,查找碰撞障碍,初始化参考线信息等。
  2. FindDriveReferenceLineInfo函数
    该函数从可用的参考线信息列表中找到代价(cost)值最低的一条参考线信息,作为最终的驾驶路径。

六、参考线提供者类(ReferenceLineProvider)分析

ReferenceLineProvider类是规划模块中参考线(即候选路径)的提供和管理者,它以Routing模块输出的高精度路径(从起点到终点的Lane片段集)为输入数据,经优化处理后生成平滑的参考线集给后续的动作规划子模块使用。ReferenceLineProvider类是采用C++ 11标准实现的多线程类,每次调用ReferenceLineProvider::Start()函数后,该类就会在内部开启一个新线程,执行参考线的优化。ReferenceLineProvider类是使用宏DECLARE_SINGLETON(ReferenceLineProvider)定义的单实例类,获取该类对象请使用ReferenceLineProvider::instance()。

(一)数据成员

该类数据成员包括:是否初始化(is_initialized_)、是否停止(is_stop_)、线程类对象智能指针(thread_)、参考线平滑类对象智能指针(smoother_)、二维样条求解类对象智能指针(spline_solver_)、二次规划样条参考线平滑配置类对象指针(smoother_config_)、规划控制地图互斥体类对象(pnc_map_mutex_)、规划控制地图类对象智能指针(pnc_map_)、车辆状态互斥体类对象(vehicle_state_mutex_)、车辆状态类对象(vehicle_state_)、路由寻径互斥体类对象(routing_mutex_)、路由寻径响应类对象(routing_)、是否具备路由(has_routing_)、历史片段互斥体类对象(segment_history_mutex_)、历史片段无序映射表类对象(segment_history_)、历史片段ID列表类对象(segment_history_id_)、参考线互斥体类对象(reference_lines_mutex_)、参考线列表类对象(reference_lines_)、路由片段列表类对象(route_segments_)、上次计算时间(last_calculation_time_)。注意该类的指针使用C++标准库的唯一性智能指针unique_ptr包装,其优点在于:一是不用人工管理内存;二是上述指针由ReferenceLineProvider类对象专享,确保数据的唯一性。

  1. 是否初始化(bool is_initialized_
    初始化是否完成标志。
  2. 是否停止(bool is_stop_
    记录当前线程是否停止。
  3. 线程类对象智能指针(std::unique_ptr<std::thread> thread_)
    后台线程的管理者。
  4. 参考线平滑类对象智能指针(std::unique_ptr<ReferenceLineSmoother> smoother_
    执行参考线平滑任务的类对象。
  5. 二维样条求解类对象智能指针(std::unique_ptr<Spline2dSolver> spline_solver_
    执行二维样条求解任务的类对象。
  6. 二次规划样条参考线平滑配置类对象指针(QpSplineReferenceLineSmootherConfig smoother_config_
    读取二次规划样条参考线平滑配置的类对象。
  7. 规划控制地图互斥体类对象(std::mutex pnc_map_mutex_
    允许规划控制地图在多线程环境下安全使用的互斥体类对象。
  8. 规划控制地图类对象智能指针(std::unique_ptr<hdmap::PncMap> pnc_map_
    规划控制模块使用的地图数据类对象。
  9. 车辆状态互斥体类对象(std::mutex vehicle_state_mutex_
    允许车辆状态在多线程环境下安全使用的互斥体类对象。
  10. 车辆状态类对象(common::VehicleState vehicle_state_
    当前的车辆状态。
  11. 路由寻径互斥体类对象(std::mutex routing_mutex_
    允许路由寻径响应在多线程环境下安全使用的互斥体类对象。
  12. 路由寻径响应类对象(routing::RoutingResponse routing_
    Routing模块输出的路由寻径数据。
  13. 是否具备路由(bool has_routing_
    从起点到终点是否具备路由。
  14. 历史片段互斥体类对象(std::mutex segment_history_mutex_
    允许历史片段在多线程环境下安全使用的互斥体类对象。
  15. 历史片段无序映射表类对象(std::unordered_map<std::string, SegmentHistory> segment_history_
    存储多条历史路径片段。
  16. 历史片段ID列表类对象(std::list<std::string> segment_history_id_
    用于检索历史路径片段的ID。
  17. 参考线互斥体类对象(std::mutex reference_lines_mutex_)
    允许参考线在多线程环境下安全使用的互斥体类对象。
  18. 参考线列表类对象(std::list<ReferenceLine> reference_lines_
    存储多条候选路径。
  19. 路由片段列表类对象(std::list<hdmap::RouteSegments> route_segments_
    存储多条路由片段。
  20. 上次计算时间(double last_calculation_time_
    上次计算平滑参考线耗时。

(二)重要成员函数

该类的重要成员函数包括:Init、Start、Stop、GenerateThread、CreateReferenceLine、UpdateReferenceLine,其他函数都是辅助性的功能函数,如感兴趣可直接查看源代码。

  1. Init函数
    创建hdmap::PncMap类对象,清空segment_history_,若启用螺旋参考线(FLAGS_enable_spiral_reference_line),则将参考线平滑类对象(smoother_)创建为SpiralReferenceLineSmoother类对象,否则创建为QpSplineReferenceLineSmoother类对象。将初始化标志is_initialized_置为true。
  2. Start函数
    如果初始化标志is_initialized_置为false,直接返回false。若启用参考线提供者线程(FLAGS_enable_reference_line_provider_thread),则以ReferenceLineProvider:: GenerateThread为入口函数创建一个新线程并返回true,否则直接返回true,不做任何实际工作。
  3. Stop函数
    将线程停止标志is_stop_置为true,判断当前线程是否完成,若未完,则调用thread_->join()强制完成线程任务。
  4. GenerateThread函数
    这是ReferenceLineProvider类线程的入口函数。如果停止标志(is_stop_)置为true,首先调用std::this_thread::yield和std::this_thread::sleep_for函数,让当前线程暂停,并休眠50ms,将调度机会留给其他线程,当操作系统空闲时执行后续代码。我认为这种设计是有问题的。如果操作系统不空闲,则后续代码可能永远不会执行。我在Win10系统上写了一个小测试程序,分别使用VC++ 2017和MinGW-w64编译器构建,VC编译器生成的程序每次都会调用后续代码,MinGW编译器生成的程序有时会调用,有时不会调用,完全依据当时操作系统的空闲状态而定。若当前没有从起点到终点的路由,则进入下一次循环。一切就绪后,先调用CreateReferenceLine函数创建参考线列表,再调用UpdateReferenceLine函数更新参考线,最后计算此次参考线生成所消耗的时间。
void ReferenceLineProvider::GenerateThread() {
  constexpr int32_t kSleepTime = 50;  // milliseconds
  while (!is_stop_) {
    std::this_thread::yield();
    std::this_thread::sleep_for(
        std::chrono::duration<double, std::milli>(kSleepTime));
    double start_time = Clock::NowInSeconds();
    if (!has_routing_) {
      AERROR << "Routing is not ready.";
      continue;
    }
    std::list<ReferenceLine> reference_lines;
    std::list<hdmap::RouteSegments> segments;
    if (!CreateReferenceLine(&reference_lines, &segments)) {
      AERROR << "Fail to get reference line";
      continue;
    }
    UpdateReferenceLine(reference_lines, segments);
    double end_time = Clock::NowInSeconds();
    std::lock_guard<std::mutex> lock(reference_lines_mutex_);
    last_calculation_time_ = end_time - start_time;
  }
}
  1. CreateReferenceLine函数
    首先使用互斥体锁定,分别获取当前车辆状态和路由,接着调用pnc_map_->IsNewRouting(routing)判断当前路由是否为新路由,若是则调用pnc_map_->UpdateRoutingResponse(routing)更新路由。之后,调用CreateRouteSegments函数来创建路由片段。若不需粘合参考线(!FLAGS_enable_reference_line_stitching),则调用SmoothRouteSegment函数来平滑各路由片段列表segments,并将平滑后的路由片段存储到参考线列表reference_lines中,同时将不能平滑的路由片段从segments中删除;若需粘合参考线(FLAGS_enable_reference_line_stitching),则调用ExtendReferenceLine函数来合并不同参考线片段的重合部分,并将粘合后的路由片段保存到参考线列表reference_lines中,同时将不能粘合的路由片段从列表segments中删除。
  2. UpdateReferenceLine函数
    判断新参考线各线段是否与原参考线对应线段相同,若相同则忽略,否则更新。

七、参考线信息类(ReferenceLineInfo)分析

ReferenceLineInfo类用于存储参考线信息。所谓参考线实际就是一条驾驶路径,因此参考线信息实际上就是一条驾驶路径所需的各种信息,包括车辆状态、当前轨迹点、路径数据、速度数据等。

(一)数据成员

该类的数据成员较多,包括:车辆状态类对象(vehicle_state_)、规划点类对象(adc_planning_point_)、参考线类对象(reference_line_)、代价(cost_)、能否驾驶(is_drivable_)、路径决策类对象(path_decision_)、路径数据类对象(path_data_)、速度数据类对象(speed_data_)、离散化轨迹类对象(discretized_trajectory_)、SL边界类对象(adc_sl_boundary_)、隐藏状态类对象(latency_stats_)、车道路径片段类对象(lanes_)。

  1. 车辆状态类对象(const common::VehicleState vehicle_state_)
    从其他地方获取的车辆状态,不可修改。
  2. 规划轨迹点类对象(const common::TrajectoryPoint adc_planning_point_)
    从其他地方获取的规划点,不可修改。
  3. 参考线类对象(const ReferenceLine reference_line_)
    从其他地方获取的参考线,不可修改。
  4. 代价(double cost_)
    参考线代价,越小越好。
  5. 能否驾驶(bool is_drivable_)
    表示参考线能否驾驶。
  6. 路径决策类对象(PathDecision path_decision_)
    判断当前参考线是否有障碍。
  7. 路径数据类对象(PathData path_data_)
    存储参考线包含的路径数据。
  8. 速度数据类对象(SpeedData speed_data_)
    存储参考线包含的速度信息。
  9. 离散化轨迹类对象(DiscretizedTrajectory discretized_trajectory_)
    存储参考线的离散化轨迹点集。
  10. SL边界类对象(SLBoundary adc_sl_boundary_)
    存储参考线的边界信息。
  11. 隐藏状态类对象(LatencyStats latency_stats_)
    存储参考线包含的隐藏状态,包括总时长(ms)、控制器时长(ms)。
  12. 车道路径片段类对象(hdmap::RouteSegments lanes_)
    存储参考线的车道片段集。

(二)重要成员函数

该类主要包含以下重要成员函数:AddObstacles、IsStartFrom、IsChangeLanePath、CombinePathAndSpeedProfile、ExportTurnSignal、ExportDecision、ReachedDestination,其他函数是辅助函数,如感兴趣可直接查看源代码。

  1. AddObstacles函数
    顾名思义,该函数就是在当前参考线中加入障碍,让参考线更为接近实际的驾驶路径。
  2. IsStartFrom函数
    用于判断当前参考线是否起始于上一条参考线。
  3. IsChangeLanePath函数
    用于判断当前参考线是否是一条改变车道的参考线,也就是说,车道当前位置不在此参考线上。。
  4. CombinePathAndSpeedProfile函数
    将当前参考线的路径数据和速度信息综合考虑,形成实际可供驾驶的路径。
  5. ExportTurnSignal函数
    输出调头信号给其他函数调用。
  6. ExportDecision函数
    输出路径决策给其他函数调用。
  7. ReachedDestination函数
    用于判断当前参考线是否抵达终点。

八、交通决策器类(TrafficDecider)分析

TrafficDecider类继承自Task类,其作用是完成与交通相关的决策。Apollo 2.0版本中的道路交通包括:后方车辆、信号灯、人行横道、安全区、重新路由寻径、参考线终点、目的地、停止信号、改变车道等。Task类提供两个虚函数Init、Execute,其中Init函数用于任务对象的初始化,Task类目前未做任何实际工作;Execute用于执行实际的优化,Task类也未实现任何功能。

(一)数据成员

该类包含两个数据成员:规则工厂类对象(rule_factory_),规则配置类对象(rule_configs_)。

  1. 规则工厂类对象(apollo::common::util::Factory<RuleConfig::RuleId, TrafficRule, TrafficRule *(*)(const RuleConfig &config)> rule_factory_
    用于注册和生成相关交通决策规则对象。
  2. 规则配置类对象(google::protobuf::RepeatedPtrField rule_configs_)
    从配置文件中需生成的规则类型,包括:后方车辆(BACKSIDE_VEHICLE)、信号灯(SIGNAL_LIGHT)、人行横道(CROSSWALK)、禁停区(CLEAR_ZONE)、重新路由寻径(REROUTING)、参考线终点(REFERENCE_LINE_END)、目的地(DESTINATION)、停止信号(STOP_SIGN)、改变车道(CHANGE_LANE)等。

(二)重要成员函数

该类重载了Task类的Init、Execute函数。

  1. Init函数
    首先注册多个交通规则,目前已注册BacksideVehicle、SignalLight、Crosswalk、Rerouting、ReferenceLineEnd、Destination、StopSign、ChangeLane等;然后从配置文件获取当前应用的规则,Apollo 2.0版配置的规则包括:BACKSIDE_VEHICLE、SIGNAL_LIGHT、REFERENCE_LINE_END、DESTINATION、CHANGE_LANE。
  2. Execute函数
    首先判断是否启用信号灯决策(FLAGS_enable_traffic_light),若未启动,则跳过该决策。之后调用rule_factory_.CreateObject函数动态创建配置文件中指定的规则对象,Apollo 2.0版创建了如下规则对象:BacksideVehicle、SignalLight、ReferenceLineEnd、Destination、ChangeLane。最后,对上述规则对象依次调用rule->ApplyRule(frame, reference_line_info)来获得交通决策下的参考线信息:reference_line_info。

九、路径决策器类(PathDecider)分析

PathDecider类继承自Task类,其作用是完成与路径相关的决策,Apollo 2.0版本中,该决策器根据当前路线中静态障碍物与车辆的位置,作出忽略(Ignore)、停车(Stop)、左绕行(Left nudge)和右绕行(Right nudge)等决策。Task类提供两个虚函数Init、Execute,其中Init函数用于任务对象的初始化,Task类目前未做任何实际工作;Execute用于执行实际的优化,Task类也未实现任何功能。

(一)数据成员

该类没有数据成员。

(二)重要成员函数

该类重载了Task类的Execute函数,并实现Process、MakeObjectDecision、MakeStaticObstacleDecision几个辅助的功能函数。

  1. Execute函数
    调用Process(reference_line_info->path_data(), reference_line_info->path_decision())来获得路径决策下的参考线信息:reference_line_info。
  2. Process函数
    调用MakeObjectDecision(path_data, path_decision)来获得路径决策:decision。
  3. MakeObjectDecision函数
    调用MakeStaticObstacleDecision(path_data, path_decision)来获得路径决策:decision。
  4. MakeStaticObstacleDecision函数
    根据路径中静态障碍的位置对车辆采取忽略、停车、左绕行和右绕行等决策,并将其保存到:decision。

十、速度决策器类(SpeedDecider)分析

SpeedDecider类继承自Task类,其作用是完成与速度相关的决策。Apollo 2.0版本中,该决策器根据当前路线中障碍物与车辆的位置,作出跟随(Follow)、避让(Yield)、超车(Overtake)、停车(Stop)等决策。Task类提供两个虚函数Init、Execute,其中Init函数用于任务对象的初始化,Task类目前未做任何实际工作;Execute用于执行实际的优化,Task类也未实现任何功能。

(一)数据成员

该类包含以下数据成员:

  1. 动态规划ST坐标速度配置类对象(DpStSpeedConfig dp_st_speed_config_)
    存储从配置文件读取的动态规划ST坐标速度配置信息。
  2. ST坐标边界配置配置类对象(StBoundaryConfig st_boundary_config_)
    存储从配置文件读取的ST坐标边界条件。
  3. SL坐标边界条件类对象(SLBoundary adc_sl_boundary_)
    保存SL坐标下的边界条件。
  4. 初始轨迹点类对象(apollo::common::TrajectoryPoint init_point_)
    记录初始的轨迹点。
  5. 参考线对象指针(const ReferenceLine* reference_line_)
    记录当前的参考线。

(二)重要成员函数

该类重载了Task类的Init、Execute函数,并提供重要的功能函数:MakeObjectDecision。

  1. Init函数
    从配置文件modules\planning\conf\planning_config.pb.txt中读取相关配置信息,并保存到dp_st_speed_config_和st_boundary_config_中。
  2. Execute函数
    调用MakeObjectDecision来获得速度决策下的参考线信息:reference_line_info。
  3. MakeObjectDecision函数
    对当前路径中的障碍逐一判断,分别作出忽略、跟随、避让、超车、停车等决策,并保存到:decision。

十一、动态规划多项式路径优化器类(DpPolyPathOptimizer)分析

DpPolyPathOptimizer类继承自PathOptimizer,后者又继承自Task,其作用是对当前路径进行动态规划多项式优化处理。Task类提供两个虚函数Init、Execute,其中Init函数用于任务对象的初始化,Task类目前未做任何实际工作;Execute用于执行实际的优化,Task类也未实现任何功能。PathOptimizer类重载了Task类的虚函数Execute,同时提供一个保护的纯虚函数Process(由派生类具体实现),在Execute内部调用Process函数完成实际的优化。注意:PathOptimizer是虚基类,不可直接使用它创建类对象,必须从该类派生出具体类后方可实例化。

(一)数据成员

该类包含一个数据成员:动态规划多项式路径配置类对象(config_)。
6. 动态规划多项式路径配置类对象(DpPolyPathConfig config_)
从配置文件中读取相关信息,包括:每步采样点数(sample_points_num_each_level)、步长最大值(step_length_max)、步长最小值(step_length_min)、横向采样偏移(lateral_sample_offset)、横向调整系数(lateral_adjust_coeff)、评估时间间隔(eval_time_interval)、路径分辨率(path_resolution)、障碍忽略距离(obstacle_ignore_distance)、障碍碰撞距离(obstacle_collision_distance)、障碍危险距离(obstacle_risk_distance)、障碍碰撞代价(obstacle_collision_cost)等。

(二)重要成员函数

该类重载了Task类的Init函数,PathOptimizer类的Process函数。
4. Init函数
该函数很简单,就是从配置文件中读取相关信息,获取算法所需的各项参数初始值。
5. Process函数
该函数创建一个DPRoadGraph类对象dp_road_graph,然后调用DPRoadGraph::FindPathTunnel函数寻找最优路径。

十二、动态规划ST坐标速度优化器类(DpStSpeedOptimizer)分析

DpStSpeedOptimizer类继承自SpeedOptimizer,后者又继承自Task,其作用是对当前速度进行动态规划ST坐标优化处理。Task类提供两个虚函数Init、Execute,其中Init函数用于任务对象的初始化,Task类目前未做任何实际工作;Execute用于执行实际的优化,Task类也未实现任何功能。SpeedOptimizer类重载了Task类的虚函数Execute,同时提供一个保护的纯虚函数Process(由派生类具体实现),在Execute内部调用Process函数完成实际的优化,如果优化失败,则调用GenerateStopProfile函数生成停车速度信息。注意:SpeedOptimizer是虚基类,不可直接使用它创建类对象,必须从该类派生出具体类后方可实例化。

(一)数据成员

该类数据成员包括:动态规划ST坐标速度配置类对象(dp_st_speed_config_)、ST坐标边界配置类对象(dp_st_speed_config_)。

  1. 动态规划ST坐标速度配置类对象(DpStSpeedConfig dp_st_speed_config_)
    从配置文件中读取相关信息,包括:路径总长(total_path_length)、总时长(total_time)、距离向矩阵维数(matrix_dimension_s)、时间向矩阵维数(matrix_dimension_t)、速度权重(speed_weight)、加速度权重(accel_weight)、加加速度(即加速度导数)权重(jerk_weight)、障碍权重(obstacle_weight)、参考权重(reference_weight)、向下权重(go_down_buffer)、向上权重(go_up_buffer)、缺省障碍代价(default_obstacle_cost)、障碍代价因子(obstacle_cost_factor)、缺省速度代价(default_speed_cost)、超速惩罚(exceed_speed_penalty)、低速惩罚(low_speed_penalty)、加速度惩罚(accel_penalty)、减速度惩罚(decel_penalty)、正加加速度系数(positive_jerk_coeff)、负加加速度系数(negative_jerk_coeff)、最大加速(max_acceleration)、最大减速(max_deceleration)、ST坐标边界设置(st_boundary_config)、禁停代价因子(keep_clear_cost_factor)等。
  2. ST坐标边界配置类对象(StBoundaryConfig st_boundary_config_)
    从配置文件中读取相关信息,包括:高速向心加速度限制(high_speed_centric_acceleration_limit)、低速向心加速度限制(low_speed_centric_acceleration_limit)、高速阈值(high_speed_threshold)、低速阈值(low_speed_threshold)、最小曲率(minimal_kappa)、点扩展(point_extension)、最低速度(lowest_speed)、平均曲率点数(num_points_to_avg_kappa)等。

(二)重要成员函数

该类重载了Task类的Init函数,PathOptimizer类的Process函数。

  1. Init函数
    该函数很简单,就是从配置文件中读取相关信息,获取算法所需的各项参数初始值。
  2. Process函数
    该函数首先获取路径的边界,然后创建一个DpStGraph类对象st_graph,然后调用DpStGraph::Search函数执行图搜索获取最优速度信息。

十三、二次规划样条路径优化器类(QpSplinePathOptimizer)分析

QpSplinePathOptimizer类继承自PathOptimizer,后者又继承自Task,其作用是对当前路径进行二次规划样条优化处理。Task类提供两个虚函数Init、Execute,其中Init函数用于任务对象的初始化,Task类目前未做任何实际工作;Execute用于执行实际的优化,Task类也未实现任何功能。PathOptimizer类重载了Task类的虚函数Execute,同时提供一个保护的纯虚函数Process(由派生类具体实现),在Execute内部调用Process函数完成实际的优化。注意:PathOptimizer是虚基类,不可直接使用它创建类对象,必须从该类派生出具体类后方可实例化。

(一)数据成员

该类数据成员包括:二次规划样条路径配置类对象(qp_spline_path_config_)、样条生成器类对象指针(spline_generator_)。

  1. 二次规划样条路径配置类对象(QpSplinePathConfig qp_spline_path_config_)
    从配置文件中读取相关信息,包括:样条阶数(spline_order)、最大样条长度(max_spline_length)、时间分辨率(max_spline_length)、正则化权重(regularization_weight)、一次样条权重因子(first_spline_weight_factor)、一阶导数权重(derivative_weight)、二阶导数权重(second_derivative_weight)、三阶导数权重(third_derivative_weight)、参考线权重(reference_line_weight)、输出个数(num_output)、十字路口横向延拓(cross_lane_lateral_extension)、十字路口纵向延拓(cross_lane_longitudinal_extension)、历史路径权重(history_path_weight)、变道中间l值?(lane_change_mid_l)、点限制s位置?(point_constraint_s_position)、变道横向偏移(lane_change_lateral_shift)、调头速度限制(uturn_speed_limit)等。
  2. 样条生成器类对象指针(std::unique_ptr<Spline1dGenerator> spline_generator_
    用于生成二次规划所需的样条曲线。

(二)重要成员函数

该类重载了Task类的Init函数,PathOptimizer类的Process函数。

  1. Init函数
    该函数很简单,就是从配置文件中读取相关信息,获取算法所需的各项参数初始值,同时创建Spline1dGenerator样条生成器对象。
  2. Process函数
    该函数创建一个QpSplinePathGenerator类对象path_generator,然后调用QpSplinePathGenerator::Generate函数寻找最优路径。

十四、二次规划样条ST坐标速度优化器类(QpSplineStSpeedOptimizer)分析

QpSplineStSpeedOptimizer类继承自SpeedOptimizer,后者又继承自Task,其作用是对当前速度进行二次规划样条ST坐标优化处理。Task类提供两个虚函数Init、Execute,其中Init函数用于任务对象的初始化,Task类目前未做任何实际工作;Execute用于执行实际的优化,Task类也未实现任何功能。SpeedOptimizer类重载了Task类的虚函数Execute,同时提供一个保护的纯虚函数Process(由派生类具体实现),在Execute内部调用Process函数完成实际的优化,如果优化失败,则调用GenerateStopProfile函数生成停车速度信息。注意:SpeedOptimizer是虚基类,不可直接使用它创建类对象,必须从该类派生出具体类后方可实例化。

(一)数据成员

该类数据成员包括:二次规划样条ST坐标速度配置类对象(qp_st_speed_config_)、ST坐标边界配置类对象(st_boundary_config_)、样条生成器类对象指针(spline_generator_)。

  1. 二次规划样条ST坐标速度配置类对象(QpStSpeedConfig qp_st_speed_config_)
    从配置文件中读取相关信息,包括:路径总长(total_path_length )、总时长(total_time)、参考最大加速度(referred_max_acceleration)、参考最小减速度(preferred_min_deceleration)、最大加速度(max_acceleration)、最小减速度(min_deceleration) 、二次规划样条配置(qp_spline_config)、二次规划分段配置(qp_piecewise_config)、ST坐标边界配置(st_boundary_config)等。
  2. ST坐标边界配置类对象(StBoundaryConfig st_boundary_config_)
    从配置文件中读取相关信息,包括:边界缓冲(boundary_buffer)、高速向心加速度限制(high_speed_centric_acceleration_limit)、低速向心加速度限制(low_speed_centric_acceleration_limit)、高速阈值(high_speed_threshold)、低速阈值(low_speed_threshold)、最小曲率(minimal_kappa)、点扩展(point_extension)、最低速度(lowest_speed)、平均曲率点数(num_points_to_avg_kappa)等。
  3. 样条生成器类对象指针(std::unique_ptr spline_generator_)
    用于生成二次规划所需的样条曲线。

(二)重要成员函数

该类重载了Task类的Init函数,PathOptimizer类的Process函数。

  1. Init函数
    该函数很简单,就是从配置文件中读取相关信息,获取算法所需的各项参数初始值,同时创建Spline1dGenerator样条生成器对象。
  2. Process函数
    该函数首先获取路径的边界,然后创建一个QpSplineStGraph类对象st_graph,然后调用DpStGraph::Search函数执行图搜索获取最优速度信息。
展开阅读全文

没有更多推荐了,返回首页