Routing module function
The routing module of Apollo reads the original information of the high-precision map, and is used to select the closest matching point as the starting point and end point of the navigation track according to the input RoutingRequestinformation , read the generated action according to the basis , and then use the Astar algorithm to search and connect in the topology map The optimal path to the starting point is sent as output.base_mapbase_maprouting_maptopo_graphRoutingResponse
The main steps of the Routing module
When looking for the shortest distance between two points on the map, we can use the road intersection (junction) as a node (Node), the length of the road as an edge (Edge), and use the shortest path search algorithm (Astar/BFS/DFS... ), find the shortest path between two points, and output it.
In general, the map format we obtain is not a topological format suitable for searching the shortest path, so the map needs to be converted. Therefore, the Routing module can be boiled down to the following steps:
- Get the raw data of the map, node and road information.
- With the above information, construct a directed graph.
- Using the shortest path algorithm, find the shortest distance between two points
TopoCreater map building
The nodes of the topological graph constructed by Apollo's mapping are inconsistent with the above-mentioned traditional nodes. For autonomous driving, the construction of roads needs to reach the lane line level, while the traditional way of using roads as edges and intersections as nodes is not applicable.
Apollo adopts the following definition method: the point represents the lane, and the edge is the relative relationship between the lanes , as shown in the figure below.
The figure below shows the concept of road and lane in Apollo.
In Apollo, Node and Edge are defined in protobuf, located modules/routing/proto/topo_graph.protoin . 
Exit : The part corresponding to the dotted line of the lane, or a section of road section that allows lane changes defined by yourself. Road section
cost : speed limit or turning road section will increase the cost, and the cost coefficient is routing_config.pb.txtdefined in
Centerline : virtual, used to generate reference lines
Mapping process
The code for building the map routing/topo_creatoris below , and the file structure is as follows:
.
├── BUILD
├── edge_creator.cc // 建边
├── edge_creator.h
├── graph_creator.cc // 建图
├── graph_creator.h
├── graph_creator_test.cc
├── node_creator.cc // 建节点
├── node_creator.h
└── topo_creator.cc // main函数
The specific process and code explanation of this blog ( https://zhuanlan.zhihu.com/p/65533164 ) has been explained in great detail.
PS1 : There are two formats for apollo map reading, one is the Opendrive format, which is read by OpendriveAdapter, and the other is the format defined by apollo itself.
PS2 : routing_map file, there are two formats txt and bin
PS3 : Summarize the flow of the created graph, first read the road information from the base_map, then traverse the road, first create nodes, then create the edges of the nodes, and then convert the graph ( The information of points and edges) is saved in routing_map, so routing_map is the solidification of grap_protobuf format. Later, the routing module will read the created routing_map and use the Astar algorithm for path planning.
create node
The process of creating a node is mainly GetPbNode()implemented in the function:
void GetPbNode(const hdmap::Lane& lane, const std::string& road_id,
const RoutingConfig& routingconfig, Node* const node) {
// 1. 初始化节点信息
InitNodeInfo(lane, road_id, node);
// 2. 初始化节点代价
InitNodeCost(lane, routingconfig, node);
}
Initialize node information
void InitNodeInfo(const Lane& lane, const std::string& road_id,
Node* const node) {
double lane_length = GetLaneLength(lane); // 长度
node->set_lane_id(lane.id().id()); // 车道ID
node->set_road_id(road_id); // 道路ID
// 根据lane的边界，添加能够变道的路段
AddOutBoundary(lane.left_boundary(), lane_length, node->mutable_left_out());
AddOutBoundary(lane.right_boundary(), lane_length, node->mutable_right_out());
node->set_length(lane_length);
node->mutable_central_curve()->CopyFrom(lane.central_curve());
node->set_is_virtual(true);
if (!lane.has_junction_id() ||
lane.left_neighbor_forward_lane_id_size() > 0 ||
lane.right_neighbor_forward_lane_id_size() > 0) {
node->set_is_virtual(false);
}
}
Initialize node cost: calculate cost based on road length and speed limit
void InitNodeCost(const Lane& lane, const RoutingConfig& routing_config,
Node* const node) {
double lane_length = GetLaneLength(lane);
double speed_limit =
lane.has_speed_limit() ? lane.speed_limit() : routing_config.base_speed();
double ratio = speed_limit >= routing_config.base_speed()
? std::sqrt(routing_config.base_speed() / speed_limit)
: 1.0;
// 1. 根据道路长度和速度限制来计算代价
double cost = lane_length * ratio;
if (lane.has_turn()) {
// left_turn_penalty: 50.0
// right_turn_penalty: 20.0
// uturn_penalty: 100.0
if (lane.turn() == Lane::LEFT_TURN) {
cost += routing_config.left_turn_penalty();
} else if (lane.turn() == Lane::RIGHT_TURN) {
cost += routing_config.right_turn_penalty();
} else if (lane.turn() == Lane::U_TURN) {
cost += routing_config.uturn_penalty();
}
}
node->set_cost(cost);
}
create edge
The process of creating an edge is in the function GetPbEdge():
void GetPbEdge(const Node& node_from, const Node& node_to,
const Edge::DirectionType& type,
const RoutingConfig& routing_config, Edge* edge) {
// 设置起始，终止车道和类型
edge->set_from_lane_id(node_from.lane_id());
edge->set_to_lane_id(node_to.lane_id());
edge->set_direction_type(type);
// 默认代价为0，即直接向前开的代价
edge->set_cost(0.0);
if (type == Edge::LEFT || type == Edge::RIGHT) {
const auto& target_range =
(type == Edge::LEFT) ? node_from.left_out() : node_from.right_out();
double changing_area_length = 0.0;
for (const auto& range : target_range) {
changing_area_length += range.end().s() - range.start().s();
}
// 计算代价
double ratio = 1.0;
// base_changing_length: 50.0
if (changing_area_length < routing_config.base_changing_length()) {
ratio = std::pow(
changing_area_length / routing_config.base_changing_length(), -1.5);
}
edge->set_cost(routing_config.change_penalty() * ratio);
}
}
Routing Component
Each module of Apollo depends on cyber to register relevant information. For viewing each module, you need to start from the component file.
routing_component.h
#pragma once
#include <memory>
#include "modules/routing/routing.h"
namespace apollo {
namespace routing {
class RoutingComponent final
: public ::apollo::cyber::Component<RoutingRequest> {
public:
// default用来控制默认构造函数的生成。显式地指示编译器生成该函数的默认版本。
RoutingComponent() = default;
~RoutingComponent() = default;
public:
bool Init() override;
// 收到routing request的时候触发
bool Proc(const std::shared_ptr<RoutingRequest>& request) override;
private:
// routing消息发布handle
std::shared_ptr<::apollo::cyber::Writer<RoutingResponse>> response_writer_ =
nullptr;
std::shared_ptr<::apollo::cyber::Writer<RoutingResponse>>
response_history_writer_ = nullptr;
// Routing类
Routing routing_;
std::shared_ptr<RoutingResponse> response_ = nullptr;
// 定时器
std::unique_ptr<::apollo::cyber::Timer> timer_;
// 锁
std::mutex mutex_;
};
// 在cyber框架中注册routing模块
CYBER_REGISTER_COMPONENT(RoutingComponent)
} // namespace routing
} // namespace apollo
routing_component.cc
#include "modules/routing/routing_component.h"
#include <utility>
#include "modules/common/adapters/adapter_gflags.h"
#include "modules/routing/common/routing_gflags.h"
DECLARE_string(flagfile);
namespace apollo {
namespace routing {
bool RoutingComponent::Init() {
// 加载并检查config配置文件
// 消息类型定义在modules/routing/proto/routing_config.proto中
RoutingConfig routing_conf;
ACHECK(cyber::ComponentBase::GetProtoConfig(&routing_conf))
<< "Unable to load routing conf file: "
<< cyber::ComponentBase::ConfigFilePath();
AINFO << "Config file: " << cyber::ComponentBase::ConfigFilePath()
<< " is loaded.";
// 设置消息qos，控制流量，创建消息发布response_writer_
apollo::cyber::proto::RoleAttributes attr;
attr.set_channel_name(routing_conf.topic_config().routing_response_topic());
auto qos = attr.mutable_qos_profile();
qos->set_history(apollo::cyber::proto::QosHistoryPolicy::HISTORY_KEEP_LAST);
qos->set_reliability(
apollo::cyber::proto::QosReliabilityPolicy::RELIABILITY_RELIABLE);
qos->set_durability(
apollo::cyber::proto::QosDurabilityPolicy::DURABILITY_TRANSIENT_LOCAL);
response_writer_ = node_->CreateWriter<RoutingResponse>(attr);
// 历史消息发布，和response_writer_类似
apollo::cyber::proto::RoleAttributes attr_history;
attr_history.set_channel_name(
routing_conf.topic_config().routing_response_history_topic());
auto qos_history = attr_history.mutable_qos_profile();
qos_history->set_history(
apollo::cyber::proto::QosHistoryPolicy::HISTORY_KEEP_LAST);
qos_history->set_reliability(
apollo::cyber::proto::QosReliabilityPolicy::RELIABILITY_RELIABLE);
qos_history->set_durability(
apollo::cyber::proto::QosDurabilityPolicy::DURABILITY_TRANSIENT_LOCAL);
response_history_writer_ = node_->CreateWriter<RoutingResponse>(attr_history);
// 创建定时器
std::weak_ptr<RoutingComponent> self =
std::dynamic_pointer_cast<RoutingComponent>(shared_from_this());
timer_.reset(new ::apollo::cyber::Timer(
FLAGS_routing_response_history_interval_ms,
[self, this]() {
auto ptr = self.lock();
if (ptr) {
std::lock_guard<std::mutex> guard(this->mutex_);
if (this->response_ != nullptr) {
auto timestamp = apollo::cyber::Clock::NowInSeconds();
response_->mutable_header()->set_timestamp_sec(timestamp);
this->response_history_writer_->Write(*response_);
}
}
},
false));
timer_->Start();
// 执行Routing类
return routing_.Init().ok() && routing_.Start().ok();
}
// 接收"RoutingRequest"消息，输出"RoutingResponse"响应。
bool RoutingComponent::Proc(const std::shared_ptr<RoutingRequest>& request) {
auto response = std::make_shared<RoutingResponse>();
// 响应routing_请求
if (!routing_.Process(request, response.get())) {
return false;
}
// 填充响应头部信息，并且发布
common::util::FillHeader(node_->Name(), response.get());
response_writer_->Write(response);
{
std::lock_guard<std::mutex> guard(mutex_);
response_ = std::move(response);
}
return true;
}
} // namespace routing
} // namespace apollo
After understanding the general function of routing-component, you can look at the specific implementation of Routing.
Routing specific implementation
The specific implementation of routing is in routing.hand routing.cc.
The information input of the routing module includes two fixed information: the original high-precision map information (base_map) and the generated topology map (routing_map), and an externally input starting point request information (RoutingRequest). Output the result of route navigation.
Routing module initialization
First look at the initialization function of Routing:
// 初始化函数
apollo::common::Status Routing::Init() {
// 读取拓扑地图routing_map的文件位置信息
const auto routing_map_file = apollo::hdmap::RoutingMapFile();
AINFO << "Use routing topology graph path: " << routing_map_file;
// 在Navigator类加载指定的graph图
navigator_ptr_.reset(new Navigator(routing_map_file));
// 通过map模块提供的功能包,读取原始地图信息,即包括点和边的信息
// 据此查找routing request请求的点距离最近的lane,并且返回对应的lane id.
hdmap_ = apollo::hdmap::HDMapUtil::BaseMapPtr();
ACHECK(hdmap_) << "Failed to load map file:" << apollo::hdmap::BaseMapFile();
return apollo::common::Status::OK();
}
Initialization of Navigator
Routing will Navigatorsearch for paths internally. Because a search path is required, Navigatora complete topological map is required.
Navigator::Navigator(const std::string& topo_file_path) {
Graph graph;
if (!cyber::common::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.";
}
In Navigatorthe constructor, the topology map will be loaded. BlackListRangeGeneratorAt the same time, the objects of two classes (blacklist section generation) and ResultGenerator(result generation) are initialized .
bool Navigator::Init(const RoutingRequest& request, const TopoGraph* graph,
std::vector<const TopoNode*>* const way_nodes,
std::vector<double>* const way_s) {
Clear();
// 获取routing请求，对应图中的节点
if (!GetWayNodes(request, graph_.get(), way_nodes, way_s)) {
AERROR << "Failed to find search terminal point in graph!";
return false;
}
// 根据请求生成对应的黑名单lane
black_list_generator_->GenerateBlackMapFromRequest(request, graph_.get(),
&topo_range_manager_);
return true;
}
Blacklist roads and lanes can be specified in the routing request so that the routing request will not count these lanes.
Routing Process main process
The process of executing the main process of Process is as follows:
bool Routing::Process(const std::shared_ptr<RoutingRequest>& routing_request,
RoutingResponse* const routing_response) {
CHECK_NOTNULL(routing_response);
AINFO << "Get new routing request:" << routing_request->DebugString();
// 找到routing_request节点最近的路
// FillLaneInfoIfMissing从地图中选取最佳匹配点
const auto& fixed_requests = FillLaneInfoIfMissing(*routing_request);
double min_routing_length = std::numeric_limits<double>::max();
for (const auto& fixed_request : fixed_requests) {
RoutingResponse routing_response_temp;
// 是否能够找到规划路径
if (navigator_ptr_->SearchRoute(fixed_request, &routing_response_temp)) {
const double routing_length =
routing_response_temp.measurement().distance();
if (routing_length < min_routing_length) {
routing_response->CopyFrom(routing_response_temp);
min_routing_length = routing_length;
}
}
FillParkingID(routing_response);
}
if (min_routing_length < std::numeric_limits<double>::max() &&
SupplementParkingRequest(routing_response)) {
monitor_logger_buffer_.INFO("Routing success!");
return true;
}
AERROR << "Failed to search route with navigator.";
monitor_logger_buffer_.WARN("Routing failed! " +
routing_response->status().msg());
return false;
}
FillLaneInfoIfMissingThe detailed explanation is explained in this blog post https://zhuanlan.zhihu.com/p/459954010
Navigator main function
Navigator itself does not implement a path search algorithm. It just uses other classes to complete the search process of routing paths. Its main function is in SearchRoute(this blog explains it in detail https://paul.pub/apollo-routing/#id-routing_configproto ).
Mainly complete the following tasks:
1. Check the request parameters;
2. Determine whether it is in the ready state;
3. Initialize the parameters required by the request;
4. Execute the search algorithm;
5. Assemble the search results;
SearchRouteThe core is that SearchRouteByStrategythis blog has a detailed explanation https://zhuanlan.zhihu.com/p/65533164
mainly completes the following tasks:
1. Set the path search algorithm (AStar)
2. Traverse each routing_requestnode, set TopoRangeManager( The manager of NodeSRange. You can search, add, sort and merge); AddBlackMapFromTerminaladd a blacklist, divide the lane according to the start point and end point; create a subgraph SubTopoGraph(used by the search algorithm); get the start point and end point; find the best through AStar Path; save the result to node_vec
3. Merge the Route result
AStarStrategy
Navigator::SearchRouteThe method calls SearchRouteByStrategya method of the class itself. In this method, AStarStrategythe search for the path will be done with the help of .
AStarStrategyThe class is an abstract class Strategysubclass, that is, AStar can be replaced by implementing another algorithm.
The basic principle of AStar can refer to this blog: https://blog.csdn.net/sinat_52032317/article/details/127077625
For the specific implementation process of AStar, refer to this blog https://zhuanlan.zhihu.com/p/459954010
and this
https://www.jianshu.com/p/2ee0d6b19b5f
// 输入:起点、终点、读取的拓扑地图以及根据起点终点生成的子拓扑图、到起点到达终点的点集：
bool AStarStrategy::Search(const TopoGraph* graph,
const SubTopoGraph* sub_graph,
const TopoNode* src_node, const TopoNode* dest_node,
std::vector<NodeWithRange>* const result_nodes) {
Clear();
AINFO << "Start A* search algorithm.";
// 该优先列表将f最小的值作为最优点
std::priority_queue<SearchNode> open_set_detail;
// 将地图选取的起点作为搜索的第一个点,计算起点到终点的代价值(曼哈顿距离)并作为f
SearchNode src_search_node(src_node);
src_search_node.f = HeuristicCost(src_node, dest_node);
// 将该点加入到开放列表之中
open_set_detail.push(src_search_node);
open_set_.insert(src_node);
g_score_[src_node] = 0.0;
enter_s_[src_node] = src_node->StartS();
// 定义当前节点
SearchNode current_node;
std::unordered_set<const TopoEdge*> next_edge_set;
std::unordered_set<const TopoEdge*> sub_edge_set;
while (!open_set_detail.empty()) {
// 若open集非空,选取最优的为当前节点,开始搜索
current_node = open_set_detail.top();
const auto* from_node = current_node.topo_node;
// 若当前点等于目标点,结束搜索
if (current_node.topo_node == dest_node) {
// ReconstructL:从终点到起点进行反向搜索，获取轨迹点
if (!Reconstruct(came_from_, from_node, result_nodes)) {
AERROR << "Failed to reconstruct route.";
return false;
}
return true;
}
// 将当前点从搜索点集中清除
open_set_.erase(from_node);
open_set_detail.pop();
// 跳过closed集中添加的点
if (closed_set_.count(from_node) != 0) {
// if showed before, just skip...
continue;
}
// 将当前点添加到closed集中
closed_set_.emplace(from_node);
// if residual_s is less than FLAGS_min_length_for_lane_change, only move
// forward
const auto& neighbor_edges =
(GetResidualS(from_node) > FLAGS_min_length_for_lane_change &&
change_lane_enabled_)
? from_node->OutToAllEdge()
: from_node->OutToSucEdge();
double tentative_g_score = 0.0;
next_edge_set.clear();
for (const auto* edge : neighbor_edges) {
sub_edge_set.clear();
sub_graph->GetSubInEdgesIntoSubGraph(edge, &sub_edge_set);
next_edge_set.insert(sub_edge_set.begin(), sub_edge_set.end());
}
for (const auto* edge : next_edge_set) {
const auto* to_node = edge->ToNode();
// 跳过closed集中的点
if (closed_set_.count(to_node) == 1) {
continue;
}
// 跳过不能换到到达的点
if (GetResidualS(edge, to_node) < FLAGS_min_length_for_lane_change) {
continue;
}
// 将当前节点的g值和能够达到的点的cost值相加
// GetCostToNeighbor返回相邻节点的cost和边edge的cost
tentative_g_score =
g_score_[current_node.topo_node] + GetCostToNeighbor(edge);
// 若下一点需要换道才能到达,cost会减少部分值
if (edge->Type() != TopoEdgeType::TET_FORWARD) {
tentative_g_score -=
(edge->FromNode()->Cost() + edge->ToNode()->Cost()) / 2;
}
// 计算能够达到点的f值
double f = tentative_g_score + HeuristicCost(to_node, dest_node);
if (open_set_.count(to_node) != 0 && f >= g_score_[to_node]) {
continue;
}
// if to_node is reached by forward, reset enter_s to start_s
if (edge->Type() == TopoEdgeType::TET_FORWARD) {
enter_s_[to_node] = to_node->StartS();
} else {
// else, add enter_s with FLAGS_min_length_for_lane_change
double to_node_enter_s =
(enter_s_[from_node] + FLAGS_min_length_for_lane_change) /
from_node->Length() * to_node->Length();
// enter s could be larger than end_s but should be less than length
to_node_enter_s = std::min(to_node_enter_s, to_node->Length());
// if enter_s is larger than end_s and to_node is dest_node
if (to_node_enter_s > to_node->EndS() && to_node == dest_node) {
continue;
}
enter_s_[to_node] = to_node_enter_s;
}
g_score_[to_node] = f;
// 初始化下一节点
SearchNode next_node(to_node);
next_node.f = f;
open_set_detail.push(next_node);
came_from_[to_node] = from_node;
// 将能够达到的点加入open集
if (open_set_.count(to_node) == 0) {
open_set_.insert(to_node);
}
}
}
AERROR << "Failed to find goal lane with id: " << dest_node->LaneId();
return false;
}
Routing module overall structure
Summary : The Routing module first reads the original information of the high-precision map, and uses TopoCreatorthe created points and edges to generate a topological map. After reading RoutingRequestthe message (a series of points), Navigatorthe class uses the topology map and TopoRangeManageruses the AStar algorithm to search for the optimal path connecting the starting point in the topology map RoutingResponse, and sends it out as an output.
reference
[1] Routing module introduced by apollo (6) https://zhuanlan.zhihu.com/p/65533164
[2] Study Notes of Apollo Spark Project—Lecture 7 Principles of Autonomous Driving Planning Technology 1 https://blog.csdn .net/sinat_52032317/article/details/128300053#t6
[3] Apollo navigation module record (routing module) https://zhuanlan.zhihu.com/p/459954010
[4] Cyber RT basic introduction and practice https://apollo .baidu.com/community/article/1093
[5] A must-see for getting started丨Analysis of the Routing module of Baidu Apollo https://mp.weixin.qq.com/s?__biz=MzI1NjkxOTMyNQ==&mid=2247490369&idx=1&sn=3a3f1dafc46782da311a2fc910e609 5a&scene= 21#wechat_redirect
[6] Analysis of the Routing module of Baidu Apollo https://paul.pub/apollo-routing/