解析百度Apollo之Routing模块

本文是Apollo项目系列文章中的一篇，会结合源码解析其中的Routing模块。

前言

对于刚接触Apollo项目的读者可以阅读我博客中的另外一篇文章 - 《解析百度Apollo自动驾驶平台》 ，那里对Apollo项目做了整体的介绍。建议在阅读本文之前，先浏览一下那篇文章。

Apollo项目的源码可以从github上获取： ApolloAuto/apollo 。

本文中贴出的源码取自2018年底（12月27日）的版本。

模块介绍

Routing模块正如其名称所示，其主要作用就是根据请求生成路由信息。

模块输入：

• 地图数据
• 请求，包括：开始和结束位置

模块输出：

• 路由导航信息

Routing模块的实现文件结构如下图所示：

常量定义

Apollo项目中使用了Google的gflags项目来定义常量。关于gflags可以访问下面两个链接：

• Github: gflags
• How To Use 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，距离等信息。

topo_graph.proto

类型名称 描述 CurvePoint 曲线上的一个点。 CurveRange 曲线上的一段。 Node 车道上的一个节点，包含了所属车道，道路，长度，曲线起止点，中心线等信息。 Edge 连接车道之间的边，包含了起止车道id，代价和方向等信息。 Graph 完整地图的Topo结构，这其中包含了多个Node和Edge。

proto文件不是孤立存在的，每个proto文件都可以通过 import 语法使用定义在其他文件中的结构。

例如，Routing模块以及其他模块都需要用的数据结构就定义在 modules/common/proto/ 目录下。这其中包含的proto文件如下：

.
├── drive_event.proto
├── drive_state.proto
├── error_code.proto
├── geometry.proto
├── header.proto
├── pnc_point.proto
└── vehicle_signal.proto

由于篇幅所限，这里就不继续展开了，有兴趣的读者可以自行浏览这些文件。

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;
}

这段代码虽长，但其实主体逻辑是很清晰的，主要包含了这么几个步骤：

1. 对请求参数进行检查；
2. 判断自身是否处于就绪状态；
3. 初始化请求需要的参数；
4. 执行搜索算法；
5. 组装搜索结果；

搜索结果的组装就是通过 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）。

在运行时刻，框架把融合好的传感器数据和预定义的组件打包在一起形成用户级轻量任务，之后，框架的调度器可以根据资源可用性和任务优先级来派发这些任务。

关于这部分内容不再继续深入，有兴趣的读者可以看下面两个链接：

• Apollo 3.5 各功能模块的启动过程解析
• Apollo Cyber RT framework

Routing模块结构一览

文末，我们通过一幅图来描述Routing模块中的主要组件以及它们的交互关系。

参考资料与推荐读物

• ApolloAuto/apollo
• Apollo规划技术详解
• Apollo 3.5 各功能模块的启动过程解析
• Apollo Cyber RT framework
• Baidu Apollo EM Motion Planner