Apollo Routing拓扑地图生成源码学习
参考：
apollo介绍之Routing模块(六)
1.概念学习
Apollo routing模块需要的是一个拓扑结构的图，要想做routing，第一步就是把原始的高精度地图转换成包含拓扑结构的导航地图，apollo中把base_map转换为routing_map。
routing模块的建图在“apollo/modules/routing/topo_creator”中实现。
routing模块的整体工作流程如下：
1.TopoCreate实现读取routing_config配置文件（转弯，变道惩罚系数，设定速度等）以及高精度地图base_map.xml或者是apollo内部的protobuf格式map;
2.TopoCreate读取相关文件后生成routing_map.*拓扑地图用于routing 模块进行路径搜索。
3. routing 模块接受client的routing request，发送routing response给client.
1.1 topo_creator文件目录结构
核心的实现程序是graph_creator.cc
1.2 拓扑地图的生成指令
根据Routing模块的说明文档
Apollo中的routing_map.*路由地图可以通过命令来生成
bash scripts/generate_routing_topo_graph.sh
//源码generate_routing_topo_graph.sh
DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
source "${DIR}/apollo_base.sh"
# generate routing_map.bin in map directory.
${APOLLO_BIN_PREFIX}/modules/routing/topo_creator/topo_creator \
--flagfile=modules/routing/conf/routing.conf \
$@
可以看出这段代码的主要实现根据routing/conf/routing.conf里的参数routing配置文件生成拓扑地图routing_map.bin
1.3 拓扑结构
Apollo中的导航地图路径是车道线级别的，Apollo中节点Node就是一小段车道，edge边就是车道与车道的连接。
一条道路分成了2段，每一段包含3条车道(lane)，下图中车道信息主要标识了车道唯一id，左边界，右边界，参考线，长度，前车道，后车道，左边相邻的车道，右边相邻的车道等这些结构化的信息，就知道车道之间的相互关系，从而规划出一条到达目的地的车道线级别的路线。
1.3.1 Node节点
节点和边的结构在protobuf中定义，在文件"modules/routing/proto/topo_graph.proto"中
message Node { //节点的消息格式
optional string lane_id = 1; //车道段id
optional double length = 2; //车道长度
repeated CurveRange left_out = 3; //左出口？？
repeated CurveRange right_out = 4; //右出口？？
optional double cost = 5; //节点的代价
optional apollo.hdmap.Curve central_curve = 6; //中心线
optional bool is_virtual = 7 [default = true]; //是否可见
optional string road_id = 8; //所属道路ID
}
1.3.2 edge边
message Edge { //边的消息格式
enum DirectionType { //枚举可选的边的方向类型向前，左，右
FORWARD = 0;
LEFT = 1;
RIGHT = 2;
}
optional string from_lane_id = 1; //edge的起始节点
optional string to_lane_id = 2; //edge边指向的节点
optional double cost = 3; //边的代价
optional DirectionType direction_type = 4; //边的方向类型
}
- 拓扑地图生成核心源码解析
2.1 main函数 topo_creator.cc
主要实现调用graph_creator.cc里的Create()建图函数
2.2 建图graph_creator.cc
详细解析该程序。
//定义函数：判断是否允许向指定车道边界变道
bool IsAllowedToCross(const LaneBoundary& boundary) {
for (const auto& boundary_type : boundary.boundary_type()) {
if (boundary_type.types(0) != LaneBoundaryType::DOTTED_YELLOW &&
boundary_type.types(0) != LaneBoundaryType::DOTTED_WHITE) {
return false;
}
}
return true;
}
//对于定义的GraphCreator类对象的构造函数初始化，定义输入、输出地图路径及
//routing_conf配置
GraphCreator::GraphCreator(const std::string& base_map_file_path,
const std::string& dump_topo_file_path,
const RoutingConfig& routing_conf)
: base_map_file_path_(base_map_file_path),
dump_topo_file_path_(dump_topo_file_path),
routing_conf_(routing_conf) {}
//定义函数：获取边的ID
std::string GraphCreator::GetEdgeID(const std::string& from_id,
const std::string& to_id) {
return from_id + "->" + to_id;
}
//定义函数：增加边。其引用的GetPbEdge函数下一节详细说明
void GraphCreator::AddEdge(const Node& from_node,
const RepeatedPtrField<Id>& to_node_vec,
const Edge::DirectionType& type) {
for (const auto& to_id : to_node_vec) {
if (forbidden_lane_id_set_.find(to_id.id()) !=
forbidden_lane_id_set_.end()) {
ADEBUG << "Ignored lane [id = " << to_id.id();
continue;
}
const std::string edge_id = GetEdgeID(from_node.lane_id(), to_id.id());
if (showed_edge_id_set_.count(edge_id) != 0) {
continue;
}
showed_edge_id_set_.insert(edge_id);
const auto& iter = node_index_map_.find(to_id.id());
if (iter == node_index_map_.end()) {
continue;
}
const auto& to_node = graph_.node(iter->second);
edge_creator::GetPbEdge(from_node, to_node, type, routing_conf_,
graph_.add_edge());
}
}
//定义函数：判断是否是有效的掉头，根据最小转弯半径而定
bool GraphCreator::IsValidUTurn(const hdmap::Lane& lane, const double radius) {
if (lane.turn() != hdmap::Lane::U_TURN) { // not a u-turn
return false;
}
// approximate the radius from start point, middle point and end point.
if (lane.central_curve().segment().empty() ||
!lane.central_curve().segment(0).has_line_segment()) {
return false;
}
std::vector<PointENU> points;
for (const auto& segment : lane.central_curve().segment()) {
points.insert(points.end(), segment.line_segment().point().begin(),
segment.line_segment().point().end());
}
if (points.empty()) {
return false;
}
Vec2d p1{points.front().x(), points.front().y()};
const auto& mid = points[points.size() / 2];
Vec2d p2{mid.x(), mid.y()};
Vec2d p3{points.back().x(), points.back().y()};
Vec2d q1 = ((p1 + p2) / 2); // middle of p1---p2
Vec2d q2 = (p2 - p1).rotate(M_PI / 2) + q1; // perpendicular to p1-p2
Vec2d q3 = ((p2 + p3) / 2); // middle of p2 -- p3
Vec2d q4 = (p3 - p2).rotate(M_PI / 2) + q3; // perpendicular to p2-p3
const double s1 = CrossProd(q3, q1, q2);
if (std::fabs(s1) < kMathEpsilon) { // q3 is the circle center
return q3.DistanceTo(p1) >= radius;
}
const double s2 = CrossProd(q4, q1, q2);
if (std::fabs(s2) < kMathEpsilon) { // q4 is the circle center
return q4.DistanceTo(p1) >= radius;
}
if (std::fabs(s1 - s2) < kMathEpsilon) { // parallel case, a wide u-turn
return true;
}
Vec2d center = q3 + (q4 - q3) * s1 / (s1 - s2);
return p1.DistanceTo(center) >= radius;
}
//定义函数：初始化被禁止的车道(类型不是city_driving的)
void GraphCreator::InitForbiddenLanes() {
for (const auto& lane : pbmap_.lane()) {
if (lane.type() != hdmap::Lane::CITY_DRIVING) {
forbidden_lane_id_set_.insert(lane.id().id());
}
}
}
//定义核心函数：建立拓扑地图函数Create()
bool GraphCreator::Create() {
/*判断输入路径下的文件是以"xml"后缀结尾？是则加载到Map类型对象pb_map，否就显示
加载失败信息*/
if (absl::EndsWith(base_map_file_path_, ".xml")) {
if (!hdmap::adapter::OpendriveAdapter::LoadData(base_map_file_path_,
&pbmap_)) {
AERROR << "Failed to load base map file from " << base_map_file_path_;
return false;
}
} else {
//从输入路径加载protobuf格式地图到pbmap_，否就显示加载失败信息
if (!cyber::common::GetProtoFromFile(base_map_file_path_, &pbmap_)) {
AERROR << "Failed to load base map file from " << base_map_file_path_;
return false;
}
}
AINFO << "Number of lanes: " << pbmap_.lane_size();
//graph_为最后保存的拓扑地图，其格式在topo_graph.proto里说明
//设定地图版本，区域？
graph_.set_hdmap_version(pbmap_.header().version());
graph_.set_hdmap_district(pbmap_.header().district());
//清空两个二维数组用来储存道路，车道，节点之间的对应关系
node_index_map_.clear();
road_id_map_.clear();
showed_edge_id_set_.clear();
//遍历base_map中的lane,将lane与road id对应起来
for (const auto& road : pbmap_.road()) {
for (const auto& section : road.section()) {
for (const auto& lane_id : section.lane_id()) {
road_id_map_[lane_id.id()] = road.id().id();
}
}
}
//初始化禁止车道，将类型不为city_driving的车道设置放入set容器里
InitForbiddenLanes();
//加载最小转弯半径\apollo\modules\common\configs\proto\vehicle_config.proto
const double min_turn_radius =
VehicleConfigHelper::GetConfig().vehicle_param().min_turn_radius();
//遍历pbmap_中的车道
for (const auto& lane : pbmap_.lane()) {
const auto& lane_id = lane.id().id();
//如果是禁止车道，属于非city_driving类型车道，则跳过
if (forbidden_lane_id_set_.find(lane_id) != forbidden_lane_id_set_.end()) {
ADEBUG << "Ignored lane id: " << lane_id
<< " because its type is NOT CITY_DRIVING.";
continue;
}
//如果掉头是无效的，转弯半径过小则跳过
if (lane.turn() == hdmap::Lane::U_TURN &&
!IsValidUTurn(lane, min_turn_radius)) {
ADEBUG << "The u-turn lane radius is too small for the vehicle to turn";
continue;
}
AINFO << "Current lane id: " << lane_id;
//存储node index和lane id的关系
node_index_map_[lane_id] = graph_.node_size();
// 从road_id_map里找lane id，在创建节点时指定道路id,没找到则道路id为空，创建节点引
//用函数下一节具体介绍
const auto iter = road_id_map_.find(lane_id);
if (iter != road_id_map_.end()) {
node_creator::GetPbNode(lane, iter->second, routing_conf_,
graph_.add_node());
} else {
AWARN << "Failed to find road id of lane " << lane_id;
node_creator::GetPbNode(lane, "", routing_conf_, graph_.add_node());
}
}
//又遍历pbmap_车道
for (const auto& lane : pbmap_.lane()) {
const auto& lane_id = lane.id().id();
//禁止车道则跳过
if (forbidden_lane_id_set_.find(lane_id) != forbidden_lane_id_set_.end()) {
ADEBUG << "Ignored lane id: " << lane_id
<< " because its type is NOT CITY_DRIVING.";
continue;
}
//对于遍历的每一个车道其对应的节点去创建边，AddEdge函数下一节介绍
const auto& from_node = graph_.node(node_index_map_[lane_id]);
//增加当前节点向前的边
AddEdge(from_node, lane.successor_id(), Edge::FORWARD);
//如果车道长度小于最小变道长度，则直接进入下一个循环
if (lane.length() < FLAGS_min_length_for_lane_change) {
continue;
}
//如果该车道有左边界且允许变道，增加向左边邻居变道的边
if (lane.has_left_boundary() && IsAllowedToCross(lane.left_boundary())) {
AddEdge(from_node, lane.left_neighbor_forward_lane_id(), Edge::LEFT);
}
//如果该车道有右边界且允许变道，增加向右边邻居变道的边
if (lane.has_right_boundary() && IsAllowedToCross(lane.right_boundary())) {
AddEdge(from_node, lane.right_neighbor_forward_lane_id(), Edge::RIGHT);
}
}
//如果输出路径下的文件不是以bin && txt为后缀名，则显示错误信息
if (!absl::EndsWith(dump_topo_file_path_, ".bin") &&
!absl::EndsWith(dump_topo_file_path_, ".txt")) {
AERROR << "Failed to dump topo data into file, incorrect file type "
<< dump_topo_file_path_;
return false;
}
//输出拓扑地图graph_到bin,txt
auto type_pos = dump_topo_file_path_.find_last_of(".") + 1;
std::string bin_file = dump_topo_file_path_.replace(type_pos, 3, "bin");
std::string txt_file = dump_topo_file_path_.replace(type_pos, 3, "txt");
if (!cyber::common::SetProtoToASCIIFile(graph_, txt_file)) {
AERROR << "Failed to dump topo data into file " << txt_file;
return false;
}
AINFO << "Txt file is dumped successfully. Path: " << txt_file;
if (!cyber::common::SetProtoToBinaryFile(graph_, bin_file)) {
AERROR << "Failed to dump topo data into file " << bin_file;
return false;
}
AINFO << "Bin file is dumped successfully. Path: " << bin_file;
return true;
}
2.3 建图引用的创建节点及边的函数
2.3.1 创建节点函数
void GetPbNode(const hdmap::Lane& lane, const std::string& road_id,
const RoutingConfig& routingconfig, Node* const node) {
InitNodeInfo(lane, road_id, node); //初始化节点信息
InitNodeCost(lane, routingconfig, node); //初始化节点代价
}
2.3.2 初始化节点信息函数
//实际上就是对节点消息的各个参数初始化赋值
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() || //没有juntion id或左邻居数量大于0,右邻居数量大于0,就设
//置成非虚拟节点
lane.left_neighbor_forward_lane_id_size() > 0 ||
lane.right_neighbor_forward_lane_id_size() > 0) {
node->set_is_virtual(false);
}
}
2.3.3 节点的代价计算函数
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;
double cost = lane_length * ratio; //根据车道长度和速度限速比来计算代价，越接近限速代价
//越大
if (lane.has_turn()) { //如果有转弯掉头代价就加上惩罚项
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);
}
2.3.4创建边函数
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();
}
//定义一个比例，若变道区域长度小于设定的基本变道长度，按照一定规则增大cost
double ratio = 1.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);
}
}
Apollo Routing拓扑地图生成源码学习相关推荐
- 博通Broadcom SDK源码学习与开发10——Cable Modem IPv6地址
声明:原创作品,严禁用于商业目的. 本系列文章将全面剖析以Bcm33xxx芯片开发Cablemodem产品的SDK源码为例,从编译系统到各个功能模块进行分析与探讨. 文章目录 0.写在前篇 1. 相关 ...
- spring源码学习之路---深入AOP（终）
作者:zuoxiaolong8810(左潇龙),转载请注明出处,特别说明:本博文来自博主原博客,为保证新博客中博文的完整性,特复制到此留存,如需转载请注明新博客地址即可. 上一章和各位一起看了一下sp ...
- JDK源码学习-基础
JDK源码学习 目录 基础 1. 安装 1.1 下载JDK 1.2 配置环境变量 1.3 验证 2. 简单的程序 2.1 编写代码 2.2 编译文件 2.3 执行类 3. java基本类型 基础 1. ...
- ASP.NET Core MVC 源码学习：MVC 启动流程详解
前言 在 上一篇 文章中,我们学习了 ASP.NET Core MVC 的路由模块,那么在本篇文章中,主要是对 ASP.NET Core MVC 启动流程的一个学习. ASP.NET Core 是新一 ...
- Simple-Faster-RCNN源码学习笔记
Simple-Faster-RCNN 源码学习 项目github地址: https://github.com/chenyuntc/simple-faster-rcnn-pytorch 源码 源文件: ...
- MVC系列——MVC源码学习：打造自己的MVC框架（一：核心原理）(转)
阅读目录 一.MVC原理解析 1.MVC原理 二.HttpHandler 1.HttpHandler.IHttpHandler.MvcHandler的说明 2.IHttpHandler解析 3.Mvc ...
- vuex commit 模块_长篇连载：Vuex源码学习（二）脉络梳理
前车之鉴 有了vue-router源码学习的经验,每次看认真钻研源代码的时候都会抽出一小段时间来大体浏览一遍源代码.大体了解这个源代码的脉络,每个阶段做了什么,文件目录的划分.下面我来带大家梳理一下V ...
- MVC系列——MVC源码学习：打造自己的MVC框架（一：核心原理）
前言:最近一段时间在学习MVC源码,说实话,研读源码真是一个痛苦的过程,好多晦涩的语法搞得人晕晕乎乎.这两天算是理解了一小部分,这里先记录下来,也给需要的园友一个参考,奈何博主技术有限,如有理解不妥之 ...
- 博通Broadcom SDK源码学习与开发1——SDK源码探究与Cable Modem 系统编译
声明:原创作品,严禁用于商业目的. 本系列文章将全面剖析以Bcm33xxx芯片开发Cablemodem产品的SDK源码为例,从编译系统到各个功能模块进行分析与探讨. 文章目录 0.写在前篇 1. 博通 ...
最新文章
- 每日记载内容总结32
- docker-compose.yaml的一些坑（趁我还记得赶快记下来）
- 项目中比较常用的数据筛选场景
- Spring Security 之多AuthenticationProvider认证模式实现
- Citrix 桌面云 XenApp_XenDesktop_7.15 部署系列（一）前期准备
- 禁用Chrome Frame
- 【软考-软件设计师精华知识点笔记】考试大纲及题型介绍
- python数据挖掘的基本流程有哪些？
- Python 3.9 新特性速览 - Ricequant米筐量化
- 跨页面的多选功能实现
- 学术研讨会---Micheal Stonebraker的主题演讲【读后感】
- larval助手函数——larval中文文档
- 如何入驻832优选平台
- React Native 后台执行计时器react-native-background-timer
- Ant Design Vue 动态路由
- VMware vCenter Server 7.0U3
- matlab小端模式合并,大端方式和小端模式【YC】
- Linux部署springboot项目（从安装java开始）
- 「小白学Python」Windows安装Python
- 【热门】男孩取名：代表希望与志向的男孩名字