- Apollo项目的源码可以从github上获取：ApolloAuto/apollo。
- 本文中贴出的源码取自2018年底（12月27日）的版本(版本：3.5)
介绍
概述
Routing，可以翻译成“路由寻径”，正如其名称所示，其主要作用就是根据请求在地图上搜索出最优的全局路径
文件结构
Routing模块的实现文件结构如下图所示：
大致过程
- apollo的routing模块使用Ａ star算法搜索最优路径。
- 在apollo中，routing模块从base map地图获取道路的拓扑信息，并根据RoutingRequest中包含起点和终点位置，使用A star算法搜索出最终的路由线路，放在RoutingResponse中。
图中是一个RoutingResponse的例子，其中红色表示路由线路中前后连通的车道线，通过它们最终将起点和终点连接起来。地图中实线表示不可换道，虚线表示可以换道，在这个搜索结果中，除了当前红线标出的通道外，其左右可以换道的车道线也可以作为从起点到终点的路线，供后续行为决策时根据实际的路况进行选择。
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，距离等信息。 |
| topo_graph.proto | 描述 |
|---|---|
| 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);
这个接口只有很简洁的两个参数：一个是描述请求的输入参数routing_request，一个是包含结果的输出参数routing_response。它们都是在proto文件中定义的。
RoutingRequest的定义如下（RoutingResponse的结构我们在后文中会看到）：
message LaneWaypoint {
optional string id = 1;
optional double s = 2;
optional apollo.common.PointENU pose = 3;
}
message LaneSegment {
optional string id = 1;
optional double start_s = 2;
optional double end_s = 3;
}
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*算法来搜索路径的。
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：
- 每个节点的cost都可以由相邻节点的cost加上连接至自身的边的cost之和来计算。
double GetCostToNeighbor(const TopoEdge* edge) {
return (edge->Cost() + edge->ToNode()->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;
}
Routing搜索结果
Routing的搜索结果由RoutingResponse描述。RoutingResponse及相关结构定义如下：
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;
}
message RoadSegment {
optional string id = 1;
repeated Passage passage = 2;
}
message Passage {
repeated LaneSegment segment = 1;
optional bool can_exit = 2;
optional ChangeLaneType change_lane_type = 3 [default = FORWARD];
}
message LaneSegment {
optional string id = 1;
optional double start_s = 2;
optional double end_s = 3;
}
enum ChangeLaneType {
FORWARD = 0;
LEFT = 1;
RIGHT = 2;
};
message Measurement {
optional double distance = 1;
}
RoutingResponse中的属性说明如下：
| 名称 | 说明 |
|---|---|
| header | 消息头 |
| road | 具体的路径信息，最重要的数据 |
| measurement | 距离 |
| routing_request | 原始请求 |
| map_version | 地图版本 |
| status | 状态位 |
很显然，这里的RoadSegment road是最重要的数据。这个数据其实是一个三层的结构体嵌套，它们的说明如下：
- RoadSegment：描述道路，一条道路可能包含了并行的几条通路（Passage）。
- Passage：描述通路，通路是直连不含变道的可行驶区域。一个通路可能包含了前后连接的多个车道。
- LaneSegment：描述车道，车道是道路中的一段，自动驾驶车辆会尽可能沿着车道的中心线行驶。
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模块中的主要组件以及它们的交互关系。
1421
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
