Apollo Routing模組原始碼分析
嚴正宣告:本文系作者davidhopper原創,未經允許,嚴禁轉載!
Apollo Routing模組位於名稱空間:apollo::routing,其作用在簡單意義上可以理解為實現無人車軟體系統內部的導航功能,即在巨集觀層面上指導無人車軟體系統的規劃(Planning)模組按照什麼樣的道路行駛,以便順利完成從起點到終點的行駛。值得指出的是,這裡的路由尋徑雖然在一定程式上類似傳統導航,但其在細節上緊密依賴於專門為無人車導航繪製的高精度地圖,與傳統導航有本質差別。傳統導航如百度、高德或谷歌解決的是從A點到B點道路層面的路由尋徑問題,其精度可具體到某一條道路。Apollo Routing模組雖然也是要解決A點到B點的路由尋徑問題,但其路徑規劃的層次要更加深入到無人車所使用的高精地圖的車道(Lane)級別,這裡的Lane比某條道路更為精細。該模組的輸出作為Apollo Planning模組的輸入,提供給規劃模組進行路徑規劃。
Routing模組的主要類圖如下圖所示:
該模組的主要執行流程如下圖所示:
一、模組主入口
該模組的主入口為: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類的繼承關係見下圖:
基本時序如下圖所示:
現在來看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_
)
記錄該類是否完成初始化。
(二)重要成員函式
- Init函式
前文已描述,就是完成各類介面卡的註冊。 - Observe函式
呼叫各介面卡提供的回撥函式獲取當前各模組的觀測資料。 - 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函式
完成路徑的合併。