Apollo Routing模块源代码分析

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

Apollo Routing模块位于命名空间:apollo::routing,其作用在简单意义上可以理解为实现无人车软件系统内部的导航功能,即在宏观层面上指导无人车软件系统的规划(Planning)模块按照什么样的道路行驶,以便顺利完成从起点到终点的行驶。值得指出的是,这里的路由寻径虽然在一定程序上类似传统导航,但其在细节上紧密依赖于专门为无人车导航绘制的高精度地图,与传统导航有本质差别。传统导航如百度、高德或谷歌解决的是从A点到B点道路层面的路由寻径问题,其精度可具体到某一条道路。Apollo Routing模块虽然也是要解决A点到B点的路由寻径问题,但其路径规划的层次要更加深入到无人车所使用的高精地图的车道(Lane)级别,这里的Lane比某条道路更为精细。该模块的输出作为Apollo Planning模块的输入,提供给规划模块进行路径规划。
Routing模块的主要类图如下图所示:
1
该模块的主要执行流程如下图所示:
2

一、模块主入口

该模块的主入口为:modules/routing/main.cc:

APOLLO_MAIN(apollo::routing::Routing)

该宏展开后为:

int main(int argc, char **argv) {                            
    google::InitGoogleLogging(argv[0]);                        
    google::ParseCommandLineFlags(&argc, &argv, true);         
    signal(SIGINT, apollo::common::apollo_app_sigint_handler); 
    apollo::routing::Routing 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::routing::Routing对象: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::routing::Routing类的继承关系见下图:
3
基本时序如下图所示:
4
现在来看ApolloApp::Spin()函数内部,首先创建ros::AsyncSpinner对象spinner,监控用户操作。之后调用虚函数:Init()(实际调用apollo::routing::Routing::Init()),执行初始化。

apollo::common::Status Routing::Init() {
  const auto routing_map_file = apollo::hdmap::RoutingMapFile();
  AINFO << "Use routing topology graph path: " << routing_map_file;
  navigator_ptr_.reset(new Navigator(routing_map_file));
  CHECK(common::util::GetProtoFromFile(FLAGS_routing_conf_file, &routing_conf_))
      << "Unable to load routing conf file: " + FLAGS_routing_conf_file;

  AINFO << "Conf file: " << FLAGS_routing_conf_file << " is loaded.";

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

  AdapterManager::Init(FLAGS_routing_adapter_config_filename);
  AdapterManager::AddRoutingRequestCallback(&Routing::OnRoutingRequest, this);
  return apollo::common::Status::OK();
}

apollo::routing::Routing::Init函数内部,首先获取高精地图指针,之后执行适配器管理者(AdapterManager)对象的初始化状态和回调函数注册,下面两条语句值得深入探讨:

语句(1)

AdapterManager::Init(FLAGS_routing_adapter_config_filename);

语句(2)

AdapterManager::AddRoutingRequestCallback(&Routing::OnRoutingRequest, this);

首先研究语句(1),传入的参数:FLAGS_routing_adapter_config_filename实际上是利用Google开源库gflags内提供的一个宏:
DEFINE_string(routing_adapter_config_filename,
"modules/routing/conf/adapter.conf",
"The adapter config filename")

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

config {
  type: ROUTING_REQUEST
  mode: RECEIVE_ONLY
  message_history_limit: 10
}
config {
  type: ROUTING_RESPONSE
  mode: PUBLISH_ONLY
  latch: true
}
config {
  type: MONITOR
  mode: DUPLEX
  message_history_limit: 1
}
is_ros: true

该文件表明,Routing模块配置三种类型的适配器:ROUTING_REQUEST(仅接收)、ROUTING_RESPONSE(仅发布)、MONITOR(既接收又发布),且使用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,此次配置类别为:ROUTING_REQUEST、ROUTING_RESPONSE、MONITOR。

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::ROUTING_REQUEST:
        EnableRoutingRequest(FLAGS_routing_request_topic, config);
        break;
      case AdapterConfig::ROUTING_RESPONSE:
        EnableRoutingResponse(FLAGS_routing_response_topic, config);
        break;
      // 省略不相关的配置类别
      ...
      case AdapterConfig::MONITOR:
        EnableMonitor(FLAGS_monitor_topic, config);
        break;RoutingRequestAdapter
      // 省略不相关的配置类别
      ...
      default:
        AERROR << "Unknown adapter config type!";
        break;
}

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

using RoutingRequestAdapter = Adapter<routing::RoutingRequest>;

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

 public:                                                                       
  static void EnableRoutingRequest(const std::string &topic_Namest,                      
                           const AdapterConfig &config) {                      
    CHECK(config.message_history_limit() > 0)                                  
        << "Message history limit must be greater than 0";                     
    instance()->InternalEnableRoutingRequest(topic_Namest, config);                      
  }                                                                            
  static RoutingRequestAdapter *GetRoutingRequest() {                                          
    return instance()->InternalGetRoutingRequest();                                    
  }                                                                            
  static AdapterConfig &GetRoutingRequestConfig() {                                  
    return instance()->RoutingRequestconfig_;                                          
  }                                                                            
  static bool FeedRoutingRequestFile(const std::string &proto_file) {                
    if (!instance()->RoutingRequest_) {                                                
      AERROR << "Initialize adapter before feeding protobuf";                  
      return false;                                                            
    }                                                                          
    return GetRoutingRequest()->FeedFile(proto_file);                                  
  }                                                                            
  static void PublishRoutingRequest(const RoutingRequestAdapter::DataType &data) {             
    instance()->InternalPublishRoutingRequest(data);                                   
  }                                                      
  template <typeRoutingRequest T>                                                        
  static void FillRoutingRequestHeader(const std::string &module_RoutingRequest, T *data) {    
    static_assert(std::is_same<RoutingRequestAdapter::DataType, T>::value,             
                  "Data type must be the same with adapter's type!");          
    instance()->RoutingRequest_->FillHeader(module_RoutingRequest, data);                        
  }                                                                            
  static void AddRoutingRequestCallback(RoutingRequestAdapter::Callback callback) {          
    CHECK(instance()->RoutingRequest_)                                                 
        << "Initialize adapter before setting callback";                       
    instance()->RoutingRequest_->AddCallback(callback);                                
  }                                                                            
  template <class T>                                                           
  static void AddRoutingRequestCallback(                                             
      void (T::*fp)(const RoutingRequestAdapter::DataType &data), T *obj) {            
    AddRoutingRequestCallback(std::bind(fp, obj, std::placeholders::_1));            
  }                                                                            
  template <class T>                                                           
  static void AddRoutingRequestCallback(                                             
      void (T::*fp)(const RoutingRequestAdapter::DataType &data)) {                    
    AddRoutingRequestCallback(fp);                                                   
  }                                                                            
  /* Returns false if there's no callback to pop out, true otherwise. */       
  static bool PopRoutingRequestCallback() {                                          
    return instance()->RoutingRequest_->PopCallback();                                 
  }                                                                            

 private:                                                                      
  std::unique_ptr<RoutingRequestAdapter> RoutingRequest_;                                      
  ros::Publisher RoutingRequestpublisher_;                                             
  ros::Subscriber RoutingRequestsubscriber_;                                           
  AdapterConfig RoutingRequestconfig_;                                                 

  void InternalEnableRoutingRequest(const std::string &topic_Namest,                     
                            const AdapterConfig &config) {                     
    RoutingRequest_.reset(                                                             
        new RoutingRequestAdapter(“RoutingRequest”, topic_Namest, config.message_history_limit())); 
    if (config.mode() != AdapterConfig::PUBLISH_ONLY && IsRos()) {             
      RoutingRequestsubscriber_ =                                                      
          node_handle_->subscribe(topic_Namest, config.message_history_limit(),  
                                  &RoutingRequestAdapter::OnReceive, RoutingRequest_.get());   
    }                                                                          
    if (config.mode() != AdapterConfig::RECEIVE_ONLY && IsRos()) {             
      RoutingRequestpublisher_ = node_handle_->advertise<RoutingRequestAdapter::DataType>(     
          topic_Namest, config.message_history_limit(), config.latch());         
    }                                                                          

    observers_.push_back([this]() { RoutingRequest_->Observe(); });                    
    RoutingRequestconfig_ = config;                                                    
  }                                                                            
  RoutingRequestAdapter *InternalGetRoutingRequest() { return RoutingRequest_.get(); }                 
  void InternalPublishRoutingRequest(const RoutingRequestAdapter::DataType &data) {            
    /* Only publish ROS msg if node handle is initialized. */                  
    if (IsRos()) {                                                             
      if (!RoutingRequestpublisher_.getTopic().empty()) {                              
        RoutingRequestpublisher_.publish(data);                                        
      } else {                                                                 
        AERROR << #RoutingRequest << " is not valid.";                                   
      }                                                                        
    } else {                                                                   
      /* For non-ROS mode, just triggers the callback. */                      
      if (RoutingRequest_) {                                                           
        RoutingRequest_->OnReceive(data);                                              
      } else {                                                                 
        AERROR << #RoutingRequest << " is null.";                                        
      }                                                                        
    }                                                                          
    RoutingRequest_->SetLatestPublished(data);                                         
  }

接下来研究语句(2)AdapterManager::AddRoutingRequestCallback(&Routing:: OnRoutingRequest, this),其作用是将路由寻径响应函数Routing::OnRoutingRequest注册为AdapterManager类的回调函数。但我们找遍AdapterManager类内部,也无法找到AddRoutingRequestCallback函数,它是从哪里冒出来的?这同样是宏REGISTER_ADAPTER (RoutingRequest)的杰作。查看宏展开代码后发现,有三个AddRoutingRequestCallback函数,分别为:

版本(1)

 static void AddRoutingRequestCallback(RoutingRequestAdapter::Callback callback) {          
    CHECK(instance()->RoutingRequest_)                                                 
        << "Initialize adapter before setting callback";                       
    instance()->RoutingRequest_->AddCallback(callback);                                
  }  

版本(2)

template <class T>                                                           
  static void AddRoutingRequestCallback(                                             
      void (T::*fp)(const RoutingRequestAdapter::DataType &data), T *obj) {            
    AddRoutingRequestCallback(std::bind(fp, obj, std::placeholders::_1));            
  }  

版本(3)

template <class T>                                                           
  static void AddRoutingRequestCallback(                                             
      void (T::*fp)(const RoutingRequestAdapter::DataType &data)) {                    
    AddRoutingRequestCallback(fp);                                                   
  }   

现在存在两个疑问:一是RoutingRequestAdapter::Callback是什么?二是AdapterManager::AddRoutingRequestCallback(&Routing::OnRoutingRequest, this)调用的是哪个版本?
关于第一个问题,查看文件modules/common/adapters/adapter.h得知,模板类Adapter<D>(D是数据类型)内部是这样定义Callback的:

typedef typename std::function<void(const D&)> Callback;

前已述及,RoutingRequestAdapter类是模板类Adapter<routing::RoutingRequest>的一个别名,因此RoutingRequestAdapter::Callback实际上就是函数std::function<void(const routing::RoutingRequest&)>的一个别名,其作用就是声明一个回调函数类型,该函数接受一个const routing::RoutingRequest&参数,没有返回值。
关于第二个问题,通过AdapterManager::AddRoutingRequestCallback(&Routing:: OnRoutingRequest, this)提供的参数可知,显然是调用了版本(2)版本(2)借助C++标准库提供的std::bind函数绑定参数后,继续调用版本(1)。在版本(1)中,RoutingRequest是一个封装后的std::unique_ptr<RoutingRequestAdapter>类对象指针,在实际使用时,除了不用操心指针的内存分配和释放外,它与裸指针RoutingRequestAdapter*的使用基本无差别。因此版本(1)实际上是调用Adapter<routing::RoutingRequest>::AddCallback函数来将Routing:: OnRoutingRequest加入回调函数数组std::vector< Callback> receive_callbacks中。

  template <class D>
  void Adapter<D>::AddCallback(Callback callback) {
    receive_callbacks_.push_back(callback);
  }

现在的问题来了,该回调函数被注册后,它又是在哪里被调用的呢?要回答该问题,必须重新回到语句(1)中。我们注意到,在语句(1)的初始化代码中,订阅接收消息FLAGS_routing_request_topic,并绑定其处理函数为RoutingRequestAdapter::OnReceive,而RoutingRequestAdapter实际上就是Adapter<routing:: RoutingRequest>,继续查看Adapter<routing:: RoutingRequest>::OnReceive函数代码,结果最终水落石出,它从回调函数数组std::vector<Callback> receive_callbacks_中取出我们在语句(2)中加入的回调函数:Routing:: OnRoutingRequest,并调用之。也就是说:只要Routing模块收到FLAGS_routing_request_topic消息,它都会调用Routing:: OnRoutingRequest函数进行处理。

template <class D>
  void Adapter<D>::OnReceive(const D& message) {
    last_receive_time_ = apollo::common::time::Clock::NowInSeconds();
    EnqueueData(message);
    FireCallbacks(message);
  }

  template <class D>
  void Adapter<D>::EnqueueData(const D& data) {
    if (enable_dump_) {
      DumpMessage<D>(data);
    }

    // Don't try to copy data and enqueue if the message_num is 0
    if (message_num_ == 0) {
      return;
    }

    // Lock the queue.
    std::lock_guard<std::mutex> lock(mutex_);
    if (data_queue_.size() + 1 > message_num_) {
      data_queue_.pop_back();
    }
    data_queue_.push_front(std::make_shared<D>(data));
  }

  template <class D>
  void Adapter<D>::FireCallbacks(const D& data) {
    for (const auto& callback : receive_callbacks_) {
      callback(data);
    }
  }

还有一个问题必须解决:消息接收处理函数有了,但没找到消息发布函数。Apollo 2.0版本中,除测试模块外,另有两处发布该消息,一处是在dreamview模块,一处在planning模块。dreamview模块是在SimulationWorldUpdater类的构造函数内中调用AdapterManager::PublishRoutingRequest(routing_request)发布FLAGS_routing_request_topic消息:

SimulationWorldUpdater::SimulationWorldUpdater(WebSocketHandler *websocket, SimControl *sim_control, const MapService *map_service, bool routing_from_file)
    : sim_world_service_(map_service, routing_from_file),
      map_service_(map_service),
      websocket_(websocket),
      sim_control_(sim_control) {
 // ..
 websocket_->RegisterMessageHandler(
      "SendRoutingRequest",
      [this](const Json &json, WebSocketHandler::Connection *conn) {
        RoutingRequest routing_request;

        bool succeed = ConstructRoutingRequest(json, &routing_request);
        if (succeed) {
          AdapterManager::FillRoutingRequestHeader(
                FLAGS_dreamview_module_name, &routing_request);
          AdapterManager::PublishRoutingRequest(routing_request);
        }
 // …

Planning模块是在Frame::Rerouting函数内调用AdapterManager::PublishRoutingRequest(request)发布FLAGS_routing_request_topic消息。而Frame::Rerouting函数仅在Rerouting:: ApplyRule函数内被调用(实际通过Rerouting:: ChangeLaneFailRerouting调用)。Rerouting(重新路由)是交通决策器(TrafficDecider)里包含的一项规则,Apollo 2.0版本的配置文件并未使用该项规则(planning\conf\planning_config.pb.txt文件中关于规则的定义如下所示,并未包含REROUTING规则):

rule_config : {
    rule_id: BACKSIDE_VEHICLE
}
rule_config : {
    rule_id: SIGNAL_LIGHT
}
rule_config : {
    rule_id: REFERENCE_LINE_END
}
rule_config : {
    rule_id: DESTINATION
}
rule_config : {
    rule_id: CHANGE_LANE
}
rule_config : {
    rule_id: CROSSWALK
}
rule_config : {
    rule_id: STOP_SIGN
}

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

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

AdapterManager类不属于Routing模块,但它是所有消息适配器的管理者,这里对其作一初步分析。顾名思义,AdapterManager是所有适配器的管理容器。为什么要使用适配器?Apollo项目各功能模块之间必须相互通信才能顺利完成任务,为了使各功能模块彼此理解对方的消息,有必要使用统一的消息机制。目前,Apollo项目采用ROS消息机制,适配器就是保证各功能模块正常接收、发送ROS消息的一种转接器。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();
}

三、路由寻径类(Routing)分析

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

(一)数据成员

该类数据成员包括:导航器指针(navigator_ptr_)、监控日志类对象(monitor_logger_)、路由配置类对象(routing_conf_)、高精地图指针(hdmap_)。注意navigator_ptr_使用C++标准库的唯一性智能指针unique_ptr包装,其优点在于:一是不用人工管理内存;二是该指针由Routing类对象专享,确保数据的唯一性。
1. 导航器指针(std::unique_ptr<Navigator> navigator_ptr_
存储导航器类对象指针,Navigator是用于路由寻径的核心类。
2. 监控日志类对象(apollo::common::monitor::MonitorLogger monitor_logger_
监控记录各类运行信息。
3. 路由配置类对象(RoutingConfig routing_conf_
通过读取配置文件获取路由寻径所需的各类参数,包括:基本速度(base_speed,单位:m/s)、左转惩罚(left_turn_penalty ,单位:m)、右转惩罚(right_turn_penalty ,单位:m)、调头惩罚(uturn_penalty,单位:m)、变道惩罚(change_penalty,单位:m)、基本改变长度(base_changing_length,单位:m)。
4. 高精地图指针(const hdmap::HDMap* hdmap_
存储高精地图指针。

(二)重要成员函数

该类主要包含三个重载函数:Init、Start、Stop(均为基类ApolloApp的虚函数)和功能函数:OnRoutingRequest。
4. Init函数
该函数完成以下工作:获取用于路由寻径的高精地图,并以此为基础创建导航器对象;读取配置文件获取相关参数;获取高清地图;完成AdapterManager初始化,将Routing::OnRoutingRequest注册为FLAGS_routing_request_topic消息的响应函数(详细分析见前文)。
5. Start函数
检查导航器及监控日志缓存区是否准备好。
6. Stop函数
不完成任何工作。
7. OnRoutingRequest函数
这是FLAGS_routing_request_topic消息的响应函数。首先创建RoutingResponse 对象:routing_response并创建监控日志缓存区,然后调用FillLaneInfoIfMissing函数填充可能缺失的车道信息;核心代码是调用navigator_ptr_->SearchRoute(fixed_request, &routing_response)执行实际的路径搜索。最后调用 AdapterManager::PublishRoutingResponse (routing_response)函数发布导航路径。任何一个注册了FLAGS_routing_response_topic消息的模块均可使用此路径。

四、导航者类(Navigator)分析

Navigator类是具体的路由寻径类,它基于高清地图和路由寻径起点、终点,基于ASTAR算法获得一条最优导航路径。

(一)数据成员

该类包含以下数据成员:
1. 就绪标志(bool is_ready_
表明Navigator类对象已准备好执行导航任务。
2. 拓扑图类对象指针(std::unique_ptr<TopoGraph> graph_
存储实际导航图。
3. 拓扑距离管理者(TopoRangeManager topo_range_manager_
管理图内的拓扑距离。
4. 黑名单距离生成器类对象指针(std::unique_ptr<BlackListRangeGenerator> black_list_generator_
用于生成黑名单距离。
5. 结果生成器类对象指针(std::unique_ptr<ResultGenerator> result_generator_
用于生成导航结果。

(二)重要成员函数

该类的主要成员函数包括:SearchRoute、Init、SearchRouteByStrategy、MergeRoute,其他函数是辅助函数,如感兴趣可直接查看源代码。
1. SearchRoute函数
完成路由寻径功能。首先调用Init完成内部初始化,其次调用SearchRouteByStrategy进行实际的路由寻径;最后调用result_generator_->GeneratePassageRegion函数生成实际的导航路由。
2. Init函数
首先获取路由寻径请求信息中的路径控制点,之后调用black_list_generator_->GenerateBlackMapFromReques从路由寻径请求信息生成黑名单地图(即需避开的路线)。
3. SearchRouteByStrategy函数
该函数采用A*算法执行路径搜索。首先生成AStarStrategy对象,接着对控制点数组中的每一条片段调用strategy_ptr->Search执行搜索,最后调用MergeRoute合并路径。
4. MergeRoute函数
完成路径的合并。

展开阅读全文

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