解析百度Apollo之Routing模組

百度谷歌 · 發表 2019-01-27 18:36:05

摘要：本文是Apollo專案系列文章中的一篇，會結合原始碼解析其中的Routing模組。前言對於剛接觸Apollo專案的讀者可以閱讀我部落格中的另外一篇文章 - 《解析百度Apollo自動駕駛平臺》，那裡對Apollo專案做了整體的介紹。建議在閱讀本文之前，先瀏覽一下那篇文章。 ...

本文是Apollo專案系列文章中的一篇，會結合原始碼解析其中的Routing模組。

前言

對於剛接觸Apollo專案的讀者可以閱讀我部落格中的另外一篇文章 - 《解析百度Apollo自動駕駛平臺》，那裡對Apollo專案做了整體的介紹。建議在閱讀本文之前，先瀏覽一下那篇文章。

Apollo專案的原始碼可以從github上獲取： ApolloAuto/apollo 。

本文中貼出的原始碼取自2018年底（12月27日）的版本。

模組介紹

Routing模組正如其名稱所示，其主要作用就是根據請求生成路由資訊。

模組輸入：

地圖資料
請求，包括：開始和結束位置

模組輸出：

路由導航資訊

Routing模組的實現檔案結構如下圖所示：

常量定義

Apollo專案中使用了Google的gflags專案來定義常量。關於gflags可以訪問下面兩個連結：

Routing中的相關程式碼如下。它們分別位於標頭檔案和實現檔案中。

routing_gflags.h 檔案內容：

#pragma once

#include "gflags/gflags.h"

DECLARE_string(routing_conf_file);
DECLARE_string(routing_node_name);

DECLARE_double(min_length_for_lane_change);
DECLARE_bool(enable_change_lane_in_result);
DECLARE_uint32(routing_response_history_interval_ms);

routing_gflags.cc 檔案內容：

#include "modules/routing/common/routing_gflags.h"

DEFINE_string(routing_conf_file,
"/apollo/modules/routing/conf/routing_config.pb.txt",
"default routing conf data file");

DEFINE_string(routing_node_name, "routing", "the name for this node");

DEFINE_double(min_length_for_lane_change, 1.0,
"meters, which is 100 feet.Minimum distance needs to travel on "
"a lane before making a lane change. Recommended by "
"https://www.oregonlaws.org/ors/811.375");

DEFINE_bool(enable_change_lane_in_result, true,
"contain change lane operator in result");

DEFINE_uint32(routing_response_history_interval_ms, 3000,
"ms, emit routing resposne for this time interval");

即便沒有接觸過gflags，這段程式碼也應該很容易理解，這裡就是定義了5個常量並指定了它們的值，同時還包含了每個常量的描述。這5個常量分別是：

routing_conf_file
routing_node_name
min_length_for_lane_change
enable_change_lane_in_result
routing_response_history_interval_ms

將模組常用的幾個常量定義在一起可以方便修改，也方便模組裡面使用。

例如，Routing模組的程式碼中通過 FLAGS_routing_conf_file 便可以讀取到值 “/apollo/modules/routing/conf/routing_config.pb.txt“ 。

這是Routing模組的配置檔案路徑。其檔案內容如下：

base_speed: 4.167
left_turn_penalty: 50.0
right_turn_penalty: 20.0
uturn_penalty: 100.0
change_penalty: 500.0
base_changing_length: 50.0

後文中我們會提到這個配置檔案的作用。

Proto資料結構

Apollo專案中的很多資料結構都是通過 Protocol Buffers 定義的。所以你看不到這些類的C++檔案，因為C++需要的相關檔案是在編譯時通過proto檔案自動生成的。

Protocol Buffers 是Google的開源專案。它具有語言無關，平臺無關的特性，並且有很好的可擴充套件性。Protocol Buffers通常用於序列化結構化資料。

Apollo使用Protocol Buffers的一個很重要的作用是，用它來將結構化資料匯出到物理檔案中，並且也可以很方便的從物理檔案中讀取資訊。例如，Routing模組需要的Topo地圖就是proto結構匯出的。另外，如果匯出的是文字形式的檔案，也可以方便的進行人為的修改。例如，上面提到的 routing_config.pb.txt 。

proto檔案都位於名稱為proto的資料夾中，你可以通常下面這條命令在apollo原始碼的根目錄下找到所有的proto資料夾：

apollo$ find . -name proto

這其中自然就包含了Routing模組的proto資料夾： modules/routing/proto 。

這個目錄中包含了4個proto檔案，每個檔案中又包含了若干個結構，這些結構描述如下：

poi.proto

型別名稱	描述
Landmark	地圖上的一個點，包含了名稱和位置資訊。
POI	Point of interest的縮寫，一個POI中可以包含多個Landmark。

routing_config.proto

型別名稱	描述
RoutingConfig	描述了Routing模組的配置資訊，上面提到的 `routing_config.pb.txt` 檔案就是這個格式的。

routing.proto

型別名稱	描述
LaneWaypoint	道路上的路徑點，包含了id，長度和位置點資訊。
LaneSegment	道路的一段，包含了id和起止點資訊。
RoutingRequest	描述了路由請求的資訊，一次路由請求可以包含多個路徑點。詳細結構見下文。
Measurement	描述測量的距離。
ChangeLaneType	道路的型別，有FORWARD，LEFT，RIGHT三種取值。
Passage	一段通路，其中可以包含多個LaneSegment，以及ChangeLaneType。
RoadSegment	道路的一段，擁有一個id，並可以包含多個Passage。
RoutingResponse	路由請求的響應結果，可以包含多個RoadSegment，距離等資訊。

型別名稱	描述
CurvePoint	曲線上的一個點。
CurveRange	曲線上的一段。
Node	車道上的一個節點，包含了所屬車道，道路，長度，曲線起止點，中心線等資訊。
Edge	連線車道之間的邊，包含了起止車道id，代價和方向等資訊。
Graph	完整地圖的Topo結構，這其中包含了多個Node和Edge。

Topo地圖

為了計算路由路徑，在Routing模組中包含一系列的類用來描述Topo地圖的詳細結構。

這些類的定義位於 modules/routing/graph/ 目錄下。它們的說明如下：

類名	描述
TopoNode	Topo地圖中的一個節點。包含了所屬Lane和Road等資訊。很顯然，這是Topo地圖中的核心資料結構。
TopoEdge	連線TopoNode之間的邊，該結構中包含了起止TopoNode等資訊。
NodeSRange	描述節點的某一段範圍。一個TopoNode可以分為若干個NodeSRange。
NodeWithRange	描述節點及其範圍，該類是NodeSRange的子類。
TopoRangeManager	NodeSRange的管理器。可以進行查詢，新增，排序和合並操作。
SubTopoGraph	Topo子圖，由搜尋演算法所用（目前是A*搜尋演算法）。
TopoGraph	對應了整個Topo地圖。其建構函式需要一個Proto結構匯出的地圖檔案，它將從地圖檔案中讀取完整的Topo結構。

簡單來說，Topo地圖中最重要的就是節點和邊，節點對應了道路，邊對應了道路的連線關係。如下圖所示：

從原始碼中可以看到，Routing模組需要的地圖結構通過 TopoGraph 來描述，而TopoGraph的初始化需要一個地圖檔案。但該地圖檔案與其他模組需要的地圖檔案並不一樣，這裡的地圖檔案是Proto結構匯出的資料。之所以這樣做是因為：Routing模組不僅需要地圖的Topo結構，還需要知道每條路線的行駛代價。在Proto結構中包含了這些資訊。在下面的內容中，我們將看到這個行駛代價是從哪裡來的。

很顯然，兩個地點的導航路徑結果通常會有多個。而計算導航路徑的時候需要有一定的傾向，這個傾向就是行駛的代價越小越好。我們很自然的想到，影響行駛代價最大的因素就是行駛的距離。

但實際上，影響行駛代價的因素遠不止距離這一個因素。距離只是巨集觀上的考慮，而從微觀的角度來看，行駛過程中，需要進行多少次轉彎，多少次掉頭，多少變道，這些都是影響行駛代價的因素。所以，在計算行駛代價的時候，需要綜合考慮這些因素。

再從另外一個角度來看，（在路線已經確定的情況下）行駛的距離是一個物理世界客觀存在的結果，這是我們無法改變的。不過，對於行駛過程中， 有多在意 轉彎，掉頭和變道，每個人或者每個場景下的偏好就不一樣了。而這，就是上文中提到的配置檔案 “/apollo/modules/routing/conf/routing_config.pb.txt“ 存在的意義了。這裡面配置了上面提到的這些動作的懲罰基數，而這些基數會影響路線時的計算代價。

通過將這種偏好以配置檔案的形式儲存在程式碼之外，可以在不用重新編譯程式碼的情況下，直接調整導航搜尋的結果。並且可以方便的為不同的場景進行策略的配置（例如：高速環境和城市道路，這些引數的值很可能就是不一樣的）。

Topo地圖本質上是一系列的Topo節點以及它們的連線關係。因此TopoNode就要能夠描述這些資訊。在這個類中，包含了許多的屬性來儲存這些連線關係，如下所示：

// topo_node.cc

std::vector<NodeSRange> left_out_sorted_range_;
std::vector<NodeSRange> right_out_sorted_range_;

std::unordered_set<const TopoEdge*> in_from_all_edge_set_;
std::unordered_set<const TopoEdge*> in_from_left_edge_set_;
std::unordered_set<const TopoEdge*> in_from_right_edge_set_;
std::unordered_set<const TopoEdge*> in_from_left_or_right_edge_set_;
std::unordered_set<const TopoEdge*> in_from_pre_edge_set_;
std::unordered_set<const TopoEdge*> out_to_all_edge_set_;
std::unordered_set<const TopoEdge*> out_to_left_edge_set_;
std::unordered_set<const TopoEdge*> out_to_right_edge_set_;
std::unordered_set<const TopoEdge*> out_to_left_or_right_edge_set_;
std::unordered_set<const TopoEdge*> out_to_suc_edge_set_;

std::unordered_map<const TopoNode*, const TopoEdge*> out_edge_map_;
std::unordered_map<const TopoNode*, const TopoEdge*> in_edge_map_;

有了這些資訊之後，在進行路徑搜尋時，可以方便的查詢線路。

TopoCreator

與人類開車時所使用的導航系統不一樣，自動駕駛需要包含更加細緻資訊的高精地圖，高精地圖描述了整個行駛過程中物理世界的詳細資訊，例如：道路的方向，寬度，曲率，紅綠燈的位置等等。而物理世界的這些狀態是很容易會發生改變的，例如，添加了一條新的道路，或者是新的紅綠燈。這就要求高精地圖也要頻繁的更新。

那麼Routing模組需要的地圖檔案也需要一起配套的跟著變化，這就很自然的需要有一個模組能夠完成從原先的高精地圖生成Routing模組的Proto格式地圖這一轉換工作。而完成這一工作的，就是 TopoCreator 模組。

TopoCreator的原始碼位於 modules/routing/topo_creator/ 目錄下，這是一個可執行程式。其main函式程式碼如下：

int main(int argc, char **argv) {
google::InitGoogleLogging(argv[0]);
google::ParseCommandLineFlags(&argc, &argv, true);

apollo::routing::RoutingConfig routing_conf;

CHECK(apollo::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.";

const auto base_map = apollo::hdmap::BaseMapFile();
const auto routing_map = apollo::hdmap::RoutingMapFile();

apollo::routing::GraphCreator creator(base_map, routing_map, routing_conf);
CHECK(creator.Create()) << "Create routing topo failed!";

AINFO << "Create routing topo successfully from " << base_map << " to "
<< routing_map;
return 0;
}

這裡的邏輯很簡單，就是先讀取配置檔案中的資訊到RoutingConfig中，然後通過GraphCreator根據高清地圖檔案生成Routing模組需要的Topo地圖。

配置檔案（routing_config.pb.txt）中的值的調整將影響這裡生成的Topo地圖的計算代價，而在Routing模組真正執行路線搜尋的時候，又會考慮這些代價，於是就會影響最終的導航計算結果。整個流程如下圖所示：

Routing模組初始化

Routing模組通過 Init 方法來初始化。在初始化時，會建立Navigator物件以及載入地圖，相關程式碼如下：

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();

return apollo::common::Status::OK();
}

Navigator的初始化

Routing內部會通過 Navigator 來搜尋路徑。因為需要搜尋路徑，所以Navigator需要完整的Topo地圖。在其建構函式中，會完成Topo地圖的載入。

相關程式碼如下：

Navigator::Navigator(const std::string& topo_file_path) {
Graph graph;
if (!common::util::GetProtoFromFile(topo_file_path, &graph)) {
AERROR << "Failed to read topology graph from " << topo_file_path;
return;
}

graph_.reset(new TopoGraph());
if (!graph_->LoadGraph(graph)) {
AINFO << "Failed to init navigator graph failed! File path: "
<< topo_file_path;
return;
}
black_list_generator_.reset(new BlackListRangeGenerator);
result_generator_.reset(new ResultGenerator);
is_ready_ = true;
AINFO << "The navigator is ready.";
}

這裡除了載入地圖還初始化了下面兩個類的物件：

BlackListRangeGenerator
ResultGenerator

路由請求

處理路由請求的介面是下面這個：

bool Routing::Process(const std::shared_ptr<RoutingRequest> &routing_request,
RoutingResponse* const routing_response);

這個介面只有很簡潔的兩個引數：一個是描述請求的輸入引數，一個是包含結果的輸出引數。它們都是在proto檔案中定義的。

RoutingRequest 的定義如下：

message RoutingRequest {
optional apollo.common.Header header = 1;
repeated LaneWaypoint waypoint = 2;
repeated LaneSegment blacklisted_lane = 3;
repeated string blacklisted_road = 4;
optional bool broadcast = 5 [default = true];
optional apollo.hdmap.ParkingSpace parking_space = 6;
}

這裡最關鍵的資訊就是下面這個：

repeated LaneWaypoint waypoint = 2;

它描述了一次路由請求的路徑點， repeated 表示這個資料可以出現多次，因此是Routing模組是支援一次搜尋多個途經點的。

BlackMap

在一些情況下，地圖可能會有資訊缺失。在這種情況下，Routing模組支援動態的新增一些資訊。這個邏輯主要是通過 BlackListRangeGenerator 和 TopoRangeManager 兩個類完成的。這其中，前者提供了新增資料的介面，而後者則負責儲存這些資料。

BlackListRangeGenerator 類的定義如下：

class BlackListRangeGenerator {
 public:
BlackListRangeGenerator() = default;
~BlackListRangeGenerator() = default;

void GenerateBlackMapFromRequest(const RoutingRequest& request,
const TopoGraph* graph,
TopoRangeManager* const range_manager) const;

void AddBlackMapFromTerminal(const TopoNode* src_node,
const TopoNode* dest_node, double start_s,
double end_s,
TopoRangeManager* const range_manager) const;
};

從這個定義中可以看到，它提供了兩個介面來新增資料：

GenerateBlackMapFromRequest ：是從 RoutingRequest 包含的資料中新增。
AddBlackMapFromTerminal ：是從終端新增資料。

這兩個介面最後都會通過 TopoRangeManager::Add 介面來新增資料。該方法程式碼如下：

void TopoRangeManager::Add(const TopoNode* node, double start_s, double end_s) {
NodeSRange range(start_s, end_s);
range_map_[node].push_back(range);
}

TopoRangeManager 中的資料最終會被 ResultGenerator 在組裝搜尋結果的時候用到。

路由搜尋過程

前面我們提到了Navigator。如果你瀏覽了這個類的程式碼就會發現。Navigator本身並沒有實現路徑搜尋的演算法。它僅僅是藉助其他類來完成路由路徑的搜尋過程。

相關邏輯在 Navigator::SearchRoute 方法中。該方法程式碼如下：

bool Navigator::SearchRoute(const RoutingRequest& request,
RoutingResponse* const response) {
if (!ShowRequestInfo(request, graph_.get())) { ①
SetErrorCode(ErrorCode::ROUTING_ERROR_REQUEST,
"Error encountered when reading request point!",
response->mutable_status());
return false;
}

if (!IsReady()) { ②
SetErrorCode(ErrorCode::ROUTING_ERROR_NOT_READY, "Navigator is not ready!",
response->mutable_status());
return false;
}
std::vector<const TopoNode*> way_nodes;
std::vector<double> way_s;
if (!Init(request, graph_.get(), &way_nodes, &way_s)) { ③
SetErrorCode(ErrorCode::ROUTING_ERROR_NOT_READY,
"Failed to initialize navigator!", response->mutable_status());
return false;
}

std::vector<NodeWithRange> result_nodes;
if (!SearchRouteByStrategy(graph_.get(), way_nodes, way_s, &result_nodes)) { ④
SetErrorCode(ErrorCode::ROUTING_ERROR_RESPONSE,
"Failed to find route with request!",
response->mutable_status());
return false;
}
if (result_nodes.empty()) {
SetErrorCode(ErrorCode::ROUTING_ERROR_RESPONSE, "Failed to result nodes!",
response->mutable_status());
return false;
}
result_nodes.front().SetStartS(request.waypoint().begin()->s());
result_nodes.back().SetEndS(request.waypoint().rbegin()->s());

if (!result_generator_->GeneratePassageRegion( ⑤
graph_->MapVersion(), request, result_nodes, topo_range_manager_,
response)) {
SetErrorCode(ErrorCode::ROUTING_ERROR_RESPONSE,
"Failed to generate passage regions based on result lanes",
response->mutable_status());
return false;
}
SetErrorCode(ErrorCode::OK, "Success!", response->mutable_status());

PrintDebugData(result_nodes);
return true;
}

這段程式碼雖長，但其實主體邏輯是很清晰的，主要包含了這麼幾個步驟：

對請求引數進行檢查；
判斷自身是否處於就緒狀態；
初始化請求需要的引數；
執行搜尋演算法；
組裝搜尋結果；

搜尋結果的組裝就是通過 ResultGenerator 藉助搜尋的結果 std::vector<NodeWithRange> 以及 TopoRangeManager 來進行組裝的。

前面我們提到，搜尋的結果 RoutingResponse 型別也是在proto檔案中的定義的，其內容如下：

message RoutingResponse {
optional apollo.common.Header header = 1;
repeated RoadSegment road = 2;
optional Measurement measurement = 3;
optional RoutingRequest routing_request = 4;
optional bytes map_version = 5;
optional apollo.common.StatusPb status = 6;
}

AStarStrategy

Navigator::SearchRoute 方法的第四步呼叫了類自身的 SearchRouteByStrategy 方法。在這個方法中，會藉助 AStarStrategy 來完成路徑的搜尋。

AStarStrategy 類是抽象類 Strategy 子類，這兩個類的結構如下圖所示：

很顯然，這裡是 Strategy設計模式的應用。定義了 Strategy 基類的作用是：今後可以很容易的實現另外一種演算法將原先的A*演算法替換掉。

從 AStarStrategy 這個類的名稱我們就可以看出，這個類的實現是通過A*演算法來搜尋路徑的。關於A*演算法我們已經在另外一篇文章中詳細講解過了（《路徑規劃之 A* 演算法》），因此這裡不再贅述。

對於不瞭解A*演算法的讀者可以先閱讀那篇文章，這裡僅僅對Apollo中A*實現的關鍵概念做一點說明。

AStarStrategy由 a_star_strategy.cc 實現，對應的標頭檔案為 a_star_strategy.h 。其類定義如下：

class AStarStrategy : public Strategy {
 public:
explicit AStarStrategy(bool enable_change);
~AStarStrategy() = default;

virtual bool Search(const TopoGraph* graph, const SubTopoGraph* sub_graph,
const TopoNode* src_node, const TopoNode* dest_node,
std::vector<NodeWithRange>* const result_nodes);

 private:
void Clear();
double HeuristicCost(const TopoNode* src_node, const TopoNode* dest_node);
double GetResidualS(const TopoNode* node);
double GetResidualS(const TopoEdge* edge, const TopoNode* to_node);

 private:
bool change_lane_enabled_;
std::unordered_set<const TopoNode*> open_set_;
std::unordered_set<const TopoNode*> closed_set_;
std::unordered_map<const TopoNode*, const TopoNode*> came_from_;
std::unordered_map<const TopoNode*, double> g_score_;
std::unordered_map<const TopoNode*, double> enter_s_;
};

A*演算法實現最關鍵的就是計算Cost，因為Cost會影響最終的搜尋結果。而影響Cost的一個關鍵因素就是啟發函式的選取。下面我們就看看Apollo中是如何實現的。

關於Cost ：前面已經提到，Proto格式的Topo地圖中存有節點之間的cost（Topo地圖由TopoCreator生成，在生成的時候會根據配置檔案設定cost值），因此在這裡直接讀取即可。下面這個函式就是讀取了節點之間的cost：

double GetCostToNeighbor(const TopoEdge* edge) {
return (edge->Cost() + edge->ToNode()->Cost());
}

每個節點的cost都可以由相鄰節點的cost加上連線至自身的邊的cost之和來計算。

關於啟發函式 ：Routing模組中的A*演算法使用TopoNode錨點的座標差值做作為啟發函式，相關程式碼如下：

double AStarStrategy::HeuristicCost(const TopoNode* src_node,
const TopoNode* dest_node) {
const auto& src_point = src_node->AnchorPoint();
const auto& dest_point = dest_node->AnchorPoint();
double distance = fabs(src_point.x() - dest_point.x()) +
fabs(src_point.y() - dest_point.y());
return distance;
}

Cyber RT與模組啟動

好奇的讀者可能會發現，講到這裡，我們都沒有提到Routing模組是如何啟動的。

如果你檢視Routing模組根目錄下的 BUILD 檔案。你會發現該模組的編譯產物其實是一個動態庫（ so 檔案），而非一個可執行檔案。

那麼這個模組到底是如何啟動的呢？答案就是 Cyber RT 。

Apollo 3.5徹底摒棄了ROS，改用自研的Cyber作為底層通訊與排程平臺。Apollo Cyber RT 系統是Apollo開源軟體平臺層的一部分，作為執行時計算框架，處於實時作業系統（RTOS）和應用模組之間。Apollo Cyber RT作為基礎平臺，支援流暢高效的執行所有應用模組。

Cyber RT的工作流如下圖所示：

簡單來說，在Apollo 3.5中，各個模組（這也包括了： Localization 、 Perception 、 Prediction 、 Planning 、 Control ）的啟動都是由Cyber RT這個執行時來處理的。

如果你瀏覽Routing模組的原始碼，你會發現一個dag檔案，其內容如下：

# Define all coms in DAG streaming.
module_config {
module_library : "/apollo/bazel-bin/modules/routing/librouting_component.so"
components {
class_name : "RoutingComponent"
config {
name : "routing"
flag_file_path: "/apollo/modules/routing/conf/routing.conf"
readers: [
{
channel: "/apollo/routing_request"
qos_profile: {
depth : 10
}
}
]
}
}
}

Apollo Cyber RT 框架核心理念是基於的元件，元件有預先設定的輸入輸出。實際上，每個元件就代表一個專用得演算法模組。框架可以根據所有預定義的元件生成有向無環圖（DAG）。

在執行時刻，框架把融合好的感測器資料和預定義的元件打包在一起形成使用者級輕量任務，之後，框架的排程器可以根據資源可用性和任務優先順序來派發這些任務。

關於這部分內容不再繼續深入，有興趣的讀者可以看下面兩個連結：

Routing模組結構一覽

文末，我們通過一幅圖來描述Routing模組中的主要元件以及它們的互動關係。