自动驾驶实战系列(七)——高精地图制作流程详解与实践

2020.05.14修改:文章逻辑以及内容修改

​ 高精地图作为一个单独的模块,给感知,规划等模块提供支持,涉及到的算法内容繁杂且工程难度较大,目前尚无太多系统性的开源代码可以参考。下图是我修改的Autoware高精地图模块的可视化效果,包含元素类别按照算法而言,我们主要可以分为以下三大类,

  • 地面标志:车道线、停止线、路标等
  • 空中物体:红绿灯、路灯、限速牌/路牌
  • 各种路沿、灯杆、排水沟等

在这些类别中,最重要的部分是车道线、路沿、红绿灯、地标、停止线和人行横道,更多的语义类别我们后期再继续考虑。这里我将给出示例高精地图数据,可以作为单独的模块直接在catkin工作空间下编译使用。

​ 本篇我们将参考Autoware Vector Map,熟悉其格式结构,并进行一定程度的修改,根据我们当前测试场地的一些元素,制作自己的高精地图。在本篇之前,需要对高精地图的概念有一定程度的了解,这部分可以参考我之前整理的几篇博客。

image-20200120124307911

HDMap建图流程
HDMap图层
Apollo高精地图概述

制图流程概述

​ 目前高精地图的制作是需要有测绘资质的,所以一般把握在图商手中和一些自动驾驶公司。对于园区这种非公共区域,制图是没问题的。高精地图作为一种语义地图,概括地说,就是在点云地图上,对我们所用到的元素进行分类和提取,添加语义标签、地图POI矢量化、质量校验,最后用一套地图引擎来存储并支撑其他模块的需求

​ 高精地图制作完成后,其数据量其实是非常小的。地图数据可以存储在云端,也可以直接直接在本地,大地图还有可以用数据库来存储。先暂且不论地图格式是Lanelet2还是OpenDrive以及其他,完整的理解高精地图的元素结构,以及数据存储和使用则为重中之重,后续的格式要求可按自己的需求来适配。

小地图手工标注方法

​ 对于3km以内的小地图而言,一般可以采取手工标注点云地图的方法来制图。目前我了解到的以下软件均可,比如

  • RoadRunner

    ​ 最新版可直接标注点云PCD文件,添加语义,可直接导出标准Opendrive,缺点就是目前语义类别不太完善,导出格式只支持标准OpenDrive。在使用上,高校等科研单位申请免费,每所学校一个密钥。

  • LGSVL Map Annotation(Unity版,需要手动编译)

    ​ 支持目前的主流地图格式,可导入导出如Apollo Opendrive/标准Opendrive/Autoware VectorMap/Lanelet2等,缺点是目前只能标注LGSVL提供的一些虚拟环境,使用上是完全开源的。目前2020.03的版本已经开始支持直接标注点云pcd

  • Autocore MapToolBox插件

    AutoCore提供的开源工具,用于标注生成Autoware矢量地图,更新速度极快,目前可标注车道线,路沿,红绿灯,即将全面支持Lanelet2 ,项目完全开源,本篇将着重于此。

  • Assure mapping tools

    Github@hatem-darweesh开源的标注工具,格式支持齐全,包括Google Earth、KML、opendrive、Lanelet、Vector Map等,标注上自动Fix height、smooth line,并集成了一些自动化的提取算法和。功能相当齐全,就是性能较差,大地图加载进来很卡,需要配合GPU使用,另外有些功能也可能是目前我还没有研究透彻。

    image-20200510110457244

  • 51VR、四维图新等高精地图地图服务提供商(收费)

    一般是图商直接用自家的制图车进行采集数据制作,按次数收费,目前一般城区道路收费5k-1w/每公里。少数厂商会提供高精地图标注软件及相关仿真系统服务,成本较高,目前就试用情况来看也不是十分完善。

地图格式规范差异

​ 值得注意的是,目前各家地图格式规范基本都不是统一的,自动驾驶公司、图商、仿真软件公司等在地图格式标准上有一定的差异。以车道在高精地图中的存储为例,自动驾驶公司偏向于以坐标序列来存储,比如Apollo OpenDrvie和Autoware,部分图商和仿真软件公司又偏向于以传统的曲线方程的形式存储,比如Carla等自动驾驶仿真器只支持标准OpenDrive

​ 对于规划器和控制器而言,方程式表达的更利于规划和控制,坐标序列表达当然也可以,这一点非主要问题,开源可以参考的方法相当多。但在各种地图格式的转换中,必然会损失一些信息,比如围栏、排水沟等复杂元素的表达,但主要的车道,车道线,路标等基本元素信息还在。

制图流程实践

构建点云底图

构建点云地图可以用一些主流的激光SLAM算法来完成,比如LOAM系列(ALOAM/VLOAM/LEGO-LOAM/LIOM),可以去KITTI Odometry排行榜上查找排行前列的一些算法。注意我们需要构建的是,比如LOAM系列,也可以参考我的第一篇博客,注意此博客只做SLAM入门的同学看,不适合实际的大场景建图,主要因为一是无回环,二是没有去动态障碍物等

自动驾驶实战系列(一)——利用NDT算法构建点云地图

这里需要注意的是,底图建议使用稠密地图,并且是拼接关键帧而成(非特征点地图),需要有足够好的强度信息,这样使得纹理效果更明显,方便标注。如下图是velodyne 16线激光雷达构建的稠密点云地图,地标和车道线还算明显,建议还是需要用高线束激光雷达建图

点云地图标注

自动标注

​ 因为自动标注的方法目前步骤繁琐,难度大,涉及到的算法相当的复杂并且效果各异,对于一般的小团队而言,不太现实。目前我本人也在研究这部分的工作,在精力和时间允许的情况下,后续的博客主要也是集中在这一块。

​ 自动标注涉及到的算法相当多,流程大致如下:

  1. 点云地图提取路面信息GroundExtraction(强度图、密度图、高程图)
  2. 路面信息部分,激光点云提取路沿、车道线、各种路标、停止线、人行横道等,非路面部分提取路牌,红绿灯,路灯,灯杆等。(各种多层强度阈值过滤方法)
  3. 视觉语义分割方法,提取各道路元素(在自己标注的数据集上可能表现更好)。将视觉检测得到的像素点转换到地理坐标系下(通过内参->外参参转换),去补充激光提取出来的不完整的部分
  4. 对每一类的线段部分分别进行分段直线/曲线拟合,有序保留端点和均匀的采样点,按位置进行排序。将有序的线段再次分类,比如左车道线,右车道线,中心线(可根据直线截距或者斜率阈值过滤),计算同一条车道线的各段线段的方向,连接端点,完成矢量化
  5. 对每一类的多边形部分,比如路标,进行轮廓计算,从点云轮廓中提取出矩形或者箭头端点,按逆时针/顺时针存储(有内外圈的区分)。
  6. 用Lanelet2等地图框架,根据之前的各种点云分类,添加语义及车道的关联关系,构建地图索引和数据存储,生成高精地图数据文件,后面的博客有介绍Lanelet2用法。
  7. 用地图编辑软件打开地图并手工修正相应的轮廓,或者补充缺失部分。

自动标注方法可以参考我解析的两篇paper

Machine Learning Assisted High-Definition Map Creation

Automated Road Markings Extraction ,Classification and vetorization From Mobile Laser Scanning Data

​ 对于小团队而言,手工标注比较合适,优势在于手工标注基本不会有遗漏,缺点就是工作量较大,依赖于标注软件,可能会有一些人为标注错误,需要做严格的质量校验。不管是自动标注或者手工标注的方法,都需要标注软件去,因为目前而言机器算法无法做到百分之百,而且实际的道路由于路标遮挡、磨损等原因,都需要使用标注软件去手工修正。一个功能强大的标注软件是高精地图生态必不可少的一部分。

手工标注

​ 这里我们采用Autoware的地图标注插件,Unity + MapToolBox进行标注,插件地址如下:https://github.com/autocore-ai/MapToolbox,这里注意下载的Unity版本需2019.3+,在Windows环境下使用,最新版的priview6已经加入对road mark和crosswalk的支持。

How to make Autoware vector maps with Unity:

标注完成的元素都是有序的点,这个对比自动提取的算法而言非常方便。这之后会生成一系列的cvs文件,这些就是高精地图的数据文件,格式和Lanelet2有一定区别。目前Autoware社区已经增加了对Lanelet2的支持,后续此标注插件也会跟上。

我自己粗略的标注了楼下的一段路口,有车道线、地面标志、停止线、路沿和人行横道,在RVIZ中可视化效果如下:

image-20200509185503779

目前手工标注类软件存在的大问题还是,z轴方向上不太好把握,仅仅xy坐标可信。有些插件提供了一些自动吸附点云高度的功能,但都做得不好。

制作Apollo OpenDrive地图

目前私信我问的最多的就是如何制作Apollo或者Autoware上可用的高精地图。本篇主要讲解了如何制作Autoware高精地图,对于Apollo Opendrive高精地图的话,我也有一些思路提供在下面。

  • 可以根据上面Autoware开源的MapToolbox,这俩都是C#项目,前者可以改写后端,存储标准点的时候按照Apollo Opendrive的格式去存。都是坐标序列描述,改写难度不大。

  • LGSVL Map Annotation,目前支持直接导入PCD点云标注(2020.03版新功能不完善),也可导入已有的Apollo Opendrive/标准Opendrive/Lanelet2,可导出Apollo Opendrvie/Autoware Vector Map/Lanelet2/标准Opendrive等格式的地图。其问题主要在于性能较差,对硬件要求高。

    image-20200510210637258

  • 利用RoadRunner标注点云PCD,导出标准Opendrive地图,采用LGSVL模拟器将标准Opendrive转换到Apollo Opendrive地图。此步骤已验证,示例标注的Apollo Opendrive地图如下

  • 由于目前支撑Opendrive的开源lib库很少,虽然自己写也不是特别费劲,可以利用其他的开源地图框架,生成高精地图之后再转换为Apollo Opendrive格式。这里目前可选择的仅仅是Liblanelets/Lanlets2,按照Lanlets2的接口规范可以生成OSM地图,在用开源的JSOM编辑器编辑完OSM地图后,再转换成Apollo Opendrive地图,一步转换流程有开源的代码可以参考,测试效果如下图所示,第一张是Lanelet2地图,第二张是转换之后的标准opendrive地图。可以看到此套代码,目前主要是就车道部分进行了转换,其他元素暂未考虑,不过主要问题已经解决了,修改下转换代码,将车道部分在曲线上采样,即可从标准Opendrive转换到Apollo Opendrive。

https://github.com/JHMeusener/osm2xodr.git

lanlet

open

地图存储索引

Autoware的矢量地图元素的数据结构定义是参考liblanelet设计的,这里示例的高精地图是通过csv来存储的,可类比关系型数据库。其中

  • vector_map::Point类型是最底层的结构,point.csv文件里面按顺序存储了高精地图中所有的点的空间位置,世界坐标/经纬高,每个点对应唯一的id供其他元素引用。
  • vector_map::Vector格式则是来描述向量数据,可以理解成两/三个点+方向,比如红绿灯、路灯等。vector.csv文件是存储这些矢量及其相关联的Point.
  • vector_map::Line则为最重要的,可以用来描述车道线、停止线、路标、人行横道、斑马区等等线状元素,存于line.csv中,两个point点的连线即为line
  • vector_map::Pole则是在vector基础上组织的结构,用于描述所有和杆相关的,比如红绿灯杆、路灯杆、路牌杆等,存于pole.csv文件中。

这里简单说了下几种结构,后文会详细解析。Autoware里面的元素分类还是比较多的。上一节的map tool box目前还不支持标注如此多的元素种类。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
point.csv
vector.csv
line.csv
area.csv
pole.csv
box.csv
dtlane.csv
node.csv
lane.csv
wayarea.csv
roadedge.csv
gutter.csv
curb.csv
whiteline.csv
stopline.csv
zebrazone.csv
crosswalk.csv
road_surface_mark.csv
poledata.csv
roadsign.csv
signaldata.csv
streetlight.csv
utilitypole.csv
guardrail.csv
sidewalk.csv
driveon_portion.csv
intersection.csv
sidestrip.csv
curvemirror.csv
wall.csv
fence.csv
railroad_crossing.csv

Vector Map可视化及数据发布

这里我们对整个高精地图的数据组织形式做一个简单的说明。

将点云进行分类和下采样完成后,把所有的点按照我们定义的类型重新组织,其中point每个点进行唯一编号,包含地理坐标(经纬高)和投影坐标(平面坐标xyz),根据分类好的元素,按照line、vector、area分别组织数据。

通用Marker描述

矢量地图字面意思就是运用点、矢量线段、多边形来描述地图元素,在rviz中可视化的时候,我们一般用visualization_msgs::Marker来表示,对应的marker中的形状类型有SPHERE(球体),ARROW(箭头),LINE_STRIP(线段)CYLINDER(圆柱体)。我们首先来做一些元素形状通用的定义,具体的的position和orientation、scale放在具体的元素中来描述。

1
2
3
4
5
6
7
8
9
10
11
12
visualization_msgs::Marker createMarker(const std::string &ns, int id, int type) {
visualization_msgs::Marker marker;

marker.header.frame_id = "map";
marker.ns = ns;
marker.id = id;
marker.type = type;
marker.lifetime = ros::Duration();
marker.frame_locked = true;
disableMarker(marker); //action:delete
return marker;
}

disableMarker表示marker的action

1
2
3
4
5
6
7
8
9
void enableMarker(visualization_msgs::Marker &marker) {
marker.action = visualization_msgs::Marker::ADD; // 新增marker
}
void disableMarker(visualization_msgs::Marker &marker) {
marker.action = visualization_msgs::Marker::DELETE; // 删除marker
}
bool isValidMarker(const visualization_msgs::Marker &marker) {
return marker.action == visualization_msgs::Marker::ADD;
}

后续所有的点都用utm坐标(其他投影坐标也可以)来描述,这里做一个转换

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
double convertDegreeToRadian(double degree) {
return degree * M_PI / 180;
}
double convertRadianToDegree(double radian) {
return radian * 180 / M_PI;
}

// 将点转化成geometry_msgs::Point类型,坐标还是用平面坐标描述
geometry_msgs::Point convertPointToGeomPoint(const Point &point) {
geometry_msgs::Point geom_point;
geom_point.x = point.ly;
geom_point.y = point.bx;
geom_point.z = point.h;
return geom_point;
}
Point convertGeomPointToPoint(const geometry_msgs::Point &geom_point) {
Point point;
point.bx = geom_point.y;
point.ly = geom_point.x;
point.h = geom_point.z;
return point;
}

geometry_msgs::Quaternion convertVectorToGeomQuaternion(const Vector &vector) {
double pitch = convertDegreeToRadian(vector.vang - 90); // convert vertical angle to pitch 垂直角度
double yaw = convertDegreeToRadian(-vector.hang + 90); // convert horizontal angle to yaw 水平角度
return tf::createQuaternionMsgFromRollPitchYaw(0, pitch, yaw);
}

Vector convertGeomQuaternionToVector(const geometry_msgs::Quaternion &geom_quaternion) {
tf::Quaternion quaternion(geom_quaternion.x, geom_quaternion.y, geom_quaternion.z, geom_quaternion.w);
double roll, pitch, yaw;
tf::Matrix3x3(quaternion).getRPY(roll, pitch, yaw);
Vector vector;
vector.vang = convertRadianToDegree(pitch) + 90;
vector.hang = -convertRadianToDegree(yaw) + 90;
return vector;
}

元素格式及ROS MSG描述

Autoware Vector Map在手工标注完成之后,会生成各种csv文件,这些地图数据文件是按照key-value的方式进行关联。这里高精地图可视化模块肯定是需要首先加载地图数据文件,之后主要任务有两个:

  1. 用marker来描述各种元素的形状,在rviz中可视化,比如用Arrow来表示红绿灯。
  2. 将高精地图中各元素的数据pub到ros中,方便其他模块订阅。这里有一些元素是表示各语义的关联关系,是无法可视化看到的,比如车道Lane关联当前车道内所有的红绿灯,交通标志,地面标志,停止线等。

Idx

csv文件编号,用来统计数据文件,无用

image-20200120140044415

Point

记录所有的高精度地图点的地理坐标和投影坐标值,是最底层的数据文件 ,每个Point用pid来唯一编号,这里xy为投影坐标,可以是utm坐标(也可以是enu坐标),最后几个字段为保留字段。

csv字段 ros msg 意义
PID pid point id,记录位置信息的唯一标志
B b 地理坐标,表示point的纬度
L l 经度
H h 地理坐标,点的高度,一般误差在几米不定,不可信
Bx bx 投影坐标,点的x坐标
Ly ly 投影坐标,点的y坐标
Ref ref 暂不了解,可评论补充

image-20200120141229855

image-20200120140835642

可视化时,我们用球体来表示point,需要给maker的position、orientation、scale和color分别赋值。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
visualization_msgs::Marker createPointMarker(const std::string &ns, int id, Color color, const Point &point) {
visualization_msgs::Marker marker = createMarker(ns, id, visualization_msgs::Marker::SPHERE);
if (point.pid == 0) //pid>0
return marker;

marker.pose.position = convertPointToGeomPoint(point);//position用平面坐标来描述
marker.pose.orientation = convertVectorToGeomQuaternion(Vector());//方向用矢量描述
marker.scale.x = MAKER_SCALE_POINT;
marker.scale.y = MAKER_SCALE_POINT;
marker.scale.z = MAKER_SCALE_POINT;
marker.color = createColorRGBA(color);
if (marker.color.a == COLOR_VALUE_MIN)
return marker;

enableMarker(marker);
return marker;
}

Vector

image-20200401144207819

用vid唯一标志 ,vector表示矢量,即有方向,就是当前点到原点的向量,其中hang和vang分别表示此矢量的水平角度和垂直角度和姿态之间的转换关系。在marker中一般用箭头来描述,用来表示红绿灯等元素。hang和vang字段与yaw和pitch的对应关系如下。

1
2
hang=90-yaw
vang=pitch+90

image-20200120141319355

image-20200120140236396

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
visualization_msgs::Marker createVectorMarker(const std::string &ns, int id, Color color, const VectorMap &vmap, const Vector &vector) {
visualization_msgs::Marker marker = createMarker(ns, id, visualization_msgs::Marker::ARROW);//箭头
if (vector.vid == 0) // 从1开始编号
return marker;

Point point = vmap.findByKey(Key<Point>(vector.pid));//找到终点
if (point.pid == 0) // point也是从1开始编号
return marker;

// marker的形状设定
marker.pose.position = convertPointToGeomPoint(point);
marker.pose.orientation = convertVectorToGeomQuaternion(vector);
marker.scale.x = MAKER_SCALE_VECTOR_LENGTH;
marker.scale.y = MAKER_SCALE_VECTOR;
marker.scale.z = MAKER_SCALE_VECTOR;
marker.color = createColorRGBA(color);
if (marker.color.a == COLOR_VALUE_MIN)
return marker;

enableMarker(marker);
return marker;
}

Line

image-20200401144257280

line表示线段,图中的黄线,白线,路沿均以此来描述,用lid来唯一标志,每条line只有两个点,如果是一段路沿包括多段line的,可以通过blid和和flid按顺序查询。bpid和fpid指的是连线的两个端点的id,blid和flid表示线段之间的关联关系,分别是上一条和下一条线段的lid,若blid=0的话需要设定起点,flid是其关联的下一条线的id。

csv 字段 ros msg 意义
LID lid line编号,唯一标注,根据此来索引
BPID bpid line的起点point id
FPID fpid line的终点point id
BLID blid 关联的上一条line的id,为0则表示本段路沿的起始line
FLID flid 关联的下一条line的id,为0则表示本段路沿的终点

image-20200120141440248

image-20200120140343772

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
visualization_msgs::Marker createLineMarker(const std::string &ns, int id, Color color, const VectorMap &vmap, const Line &line) {
visualization_msgs::Marker marker = createMarker(ns, id, visualization_msgs::Marker::LINE_STRIP);//线带
if (line.lid == 0) // line从1开始编号
return marker;

Point bp = vmap.findByKey(Key<Point>(line.bpid));//找到起点和终点,相邻点连线
if (bp.pid == 0)
return marker;

Point fp = vmap.findByKey(Key<Point>(line.fpid));// 终点
if (fp.pid == 0)
return marker;

marker.points.push_back(convertPointToGeomPoint(bp));
marker.points.push_back(convertPointToGeomPoint(fp));

marker.scale.x = MAKER_SCALE_LINE;
marker.color = createColorRGBA(color);
if (marker.color.a == COLOR_VALUE_MIN)
return marker;

enableMarker(marker);
return marker;
}

Area

area用来描述区域,由连续的按顺序排列的线段组成,其中aid是其唯一标识,slid和elid分别为当前area的start_line和end_line的id,中间的线段按顺序查找line的关联id即可.

image-20200120141455949

image-20200120140110874

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
visualization_msgs::Marker createAreaMarker(const std::string &ns, int id, Color color, const VectorMap &vmap, const Area &area) {
visualization_msgs::Marker marker = createMarker(ns, id, visualization_msgs::Marker::LINE_STRIP);
if (area.aid == 0)
return marker;

Line line = vmap.findByKey(Key<Line>(area.slid)); // line id 找到line
if (line.lid == 0)
return marker;
if (line.blid != 0) // must set beginning line
return marker;

while (line.flid != 0) {//从这条线开始,找到所有的关联的线
Point bp = vmap.findByKey(Key<Point>(line.bpid));
if (bp.pid == 0)
return marker;

Point fp = vmap.findByKey(Key<Point>(line.fpid));
if (fp.pid == 0)
return marker;

marker.points.push_back(convertPointToGeomPoint(bp));
marker.points.push_back(convertPointToGeomPoint(fp));

line = vmap.findByKey(Key<Line>(line.flid));//寻找其相关联的下一条线段的id
if (line.lid == 0)
return marker;
}

// 最后一根线的两点
Point bp = vmap.findByKey(Key<Point>(line.bpid));
if (bp.pid == 0)
return marker;

Point fp = vmap.findByKey(Key<Point>(line.fpid));
if (fp.pid == 0)
return marker;

marker.points.push_back(convertPointToGeomPoint(bp));
marker.points.push_back(convertPointToGeomPoint(fp));

marker.scale.x = MAKER_SCALE_AREA;
marker.color = createColorRGBA(color);
if (marker.color.a == COLOR_VALUE_MIN)
return marker;

enableMarker(marker);
return marker;
}

Pole

image-20200401144500793

ploe用来表示杆状物体,杆也有多种分类,比如路灯杆、交通灯杆和路牌杆,在marker中用圆柱体来描述。plid指pole的唯一id,vid指pole对应的vector id,length指pole的高度,dim指pole的底面圆半径

image-20200120150312880

image-20200120150250909

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
visualization_msgs::Marker createPoleMarker(const std::string &ns, int id, Color color, const VectorMap &vmap,const Pole &pole) {
visualization_msgs::Marker marker = createMarker(ns, id, visualization_msgs::Marker::CYLINDER);//圆柱
if (pole.plid == 0)
return marker;
// XXX: The following conditions are workaround for pole.csv of Nagoya University's campus.
if (pole.length == 0 || pole.dim == 0)
return marker;

Vector vector = vmap.findByKey(Key<Vector>(pole.vid));//
if (vector.vid == 0)
return marker;
// XXX: The visualization_msgs::Marker::CYLINDER is difficult to display other than vertical pole.
if (vector.vang != 0) //不垂直
return marker;

Point point = vmap.findByKey(Key<Point>(vector.pid));
if (point.pid == 0)
return marker;

geometry_msgs::Point geom_point = convertPointToGeomPoint(point);
geom_point.z += pole.length / 2;
marker.pose.position = geom_point;
vector.vang -= 90;
marker.pose.orientation = convertVectorToGeomQuaternion(vector);
marker.scale.x = pole.dim;
marker.scale.y = pole.dim;
marker.scale.z = pole.length;
marker.color = createColorRGBA(color);
if (marker.color.a == COLOR_VALUE_MIN)
return marker;

enableMarker(marker);
return marker;
}

Box

因为还未见过可视化出来的形状及具体实例数据,目测是底面四个顶点+高度描述。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
visualization_msgs::Marker createBoxMarker(const std::string &ns, int id, Color color, const VectorMap &vmap, const Box &box) {
visualization_msgs::Marker marker = createMarker(ns, id, visualization_msgs::Marker::LINE_STRIP);
if (box.bid == 0)
return marker;

Point p1 = vmap.findByKey(Key<Point>(box.pid1));
if (p1.pid == 0)
return marker;

Point p2 = vmap.findByKey(Key<Point>(box.pid2));
if (p2.pid == 0)
return marker;

Point p3 = vmap.findByKey(Key<Point>(box.pid3));
if (p3.pid == 0)
return marker;

Point p4 = vmap.findByKey(Key<Point>(box.pid4));
if (p4.pid == 0)
return marker;

std::vector <geometry_msgs::Point> bottom_points(4);//底面四个顶点
bottom_points[0] = convertPointToGeomPoint(p1);
bottom_points[1] = convertPointToGeomPoint(p2);
bottom_points[2] = convertPointToGeomPoint(p3);
bottom_points[3] = convertPointToGeomPoint(p4);

std::vector <geometry_msgs::Point> top_points(4);//顶部四个顶点
for (size_t i = 0; i < 4; ++i) {
top_points[i] = bottom_points[i];
top_points[i].z += box.height;
}

// ?
for (size_t i = 0; i < 4; ++i) {
marker.points.push_back(bottom_points[i]);
marker.points.push_back(top_points[i]);
marker.points.push_back(top_points[i]);
if (i != 3) {
marker.points.push_back(top_points[i + 1]);
marker.points.push_back(top_points[i + 1]);
marker.points.push_back(bottom_points[i + 1]);
} else {
marker.points.push_back(top_points[0]);
marker.points.push_back(top_points[0]);
marker.points.push_back(bottom_points[0]);
}
}
for (size_t i = 0; i < 4; ++i) {
marker.points.push_back(bottom_points[i]);
if (i != 3)
marker.points.push_back(bottom_points[i + 1]);
else
marker.points.push_back(bottom_points[0]);
}

marker.scale.x = MAKER_SCALE_BOX;
marker.color = createColorRGBA(color);
if (marker.color.a == COLOR_VALUE_MIN)
return marker;

enableMarker(marker);
return marker;
}

Node

image-20200120141556745

image-20200120140309261

Lane

描述车道相关的信息,非常重要。这里lanetype就是车道类型,直行左转还是右转,在标注地图的时候添加语义标签,确定的方向。lanevfggfg这个字段目前我还未用到。下面的Ver 1.00和Ver 1.20是指vector map的版本,具体字段意义看名称。

image-20200120141212628

image-20200120140443505

Dtlane

主要是对lane数据的一些补充描述,主要是记录当前lane的长度、方向、弧度等

image-20200120141147560

image-20200120140550324

语义类别描述

目前考虑以下元素

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
white line //车道线 黄线/白线/实线/虚线
stop line // 人行横道,交叉路口前的停止线

crosswalk  //人行横道
zebra zone //斑马线区域

road mark // 地面的路标
road sign //道路指向 路牌

road pole // 信号灯杆
signal // 信号灯

curb //道路边界
gutter //排水沟
road edge //道路边界缺口部分

streetlight //路灯
utility ploe //电线杆

white line

image-20200401144645972

车道线类型包括实线、空心虚线、实心虚线,颜色有白色和黄色,在rviz中用LINE_STRIP来描述.

image-20200120123722059

image-20200120124218585

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
visualization_msgs::MarkerArray createWhiteLineMarkerArray(const VectorMap &vmap, Color white_color, Color yellow_color) {
visualization_msgs::MarkerArray marker_array;
int id = 0;
for (const auto &white_line : vmap.findByFilter([](const WhiteLine &white_line) { return true; })) {
if (white_line.lid == 0) { //line无效
ROS_ERROR_STREAM("[createWhiteLineMarkerArray] invalid white_line: " << white_line);
continue;
}
if (white_line.type == WhiteLine::DASHED_LINE_BLANK) // if invisible line 空心虚线
continue;

Line line = vmap.findByKey(Key<Line>(white_line.lid));//找到起始的第一条线
if (line.lid == 0) {//确认lid>0
ROS_ERROR_STREAM("[createWhiteLineMarkerArray] invalid line: " << line);
continue;
}

if (line.blid == 0) // if beginning line 找到后续的线
{
visualization_msgs::Marker marker;
switch (white_line.color) {//颜色有白线和黄线,黄线表示两侧的车辆反向
case 'W':
marker = createLinkedLineMarker("white_line", id++, white_color, vmap, line);
break;
case 'Y':
marker = createLinkedLineMarker("white_line", id++, yellow_color, vmap, line);
break;
default:
ROS_ERROR_STREAM("[createWhiteLineMarkerArray] unknown white_line.color: " << white_line);
continue;
}
// XXX: The visualization_msgs::Marker::LINE_STRIP is difficult to deal with white_line.width.
if (isValidMarker(marker)) //add
marker_array.markers.push_back(marker);
else
ROS_ERROR_STREAM("[createWhiteLineMarkerArray] failed createLinkedLineMarker: " << line);
}
}
return marker_array;
}

Stop line

image-20200401144708135

id是stop line的唯一标志,stopline也是用line来描述,rviz中是LINE_STRIP.其中lid指对应的line id,这里我们如何创建一条stop_line,并在rviz中观察到上图的可视化效果呢?

image-20200120133852596

image-20200120133931513

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
visualization_msgs::MarkerArray createStopLineMarkerArray(const VectorMap &vmap, Color color) {
visualization_msgs::MarkerArray marker_array;
int id = 0;
for (const auto &stop_line : vmap.findByFilter([](const StopLine &stop_line) { return true; })) {
if (stop_line.lid == 0) {
ROS_ERROR_STREAM("[createStopLineMarkerArray] invalid stop_line: " << stop_line);
continue;
}
//根据每一条stop_line对应line 的id查找相应的line
Line line = vmap.findByKey(Key<Line>(stop_line.lid));
if (line.lid == 0) {//line id > 0
ROS_ERROR_STREAM("[createStopLineMarkerArray] invalid line: " << line);
continue;
}

if (line.blid == 0) // if beginning line 是第一条line,后续的line是相互关联的
{
visualization_msgs::Marker marker = createLinkedLineMarker("stop_line", id++, color, vmap, line);//显示每条线
if (isValidMarker(marker))
marker_array.markers.push_back(marker);
else
ROS_ERROR_STREAM("[createStopLineMarkerArray] failed createLinkedLineMarker: " << line);
}
}
return marker_array;
}

road mark

image-20200401144751289

路标有四种类型,arrow、mark、character、sign,最终也是通过area来描述

image-20200120134514385

image-20200120134656640

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
visualization_msgs::MarkerArray createRoadMarkMarkerArray(const VectorMap &vmap, Color color) {
visualization_msgs::MarkerArray marker_array;
int id = 0;
for (const auto &road_mark : vmap.findByFilter([](const RoadMark &road_mark) { return true; })) {
if (road_mark.aid == 0) {
ROS_ERROR_STREAM("[createRoadMarkMarkerArray] invalid road_mark: " << road_mark);
continue;
}

Area area = vmap.findByKey(Key<Area>(road_mark.aid));
if (area.aid == 0) {
ROS_ERROR_STREAM("[createRoadMarkMarkerArray] invalid area: " << area);
continue;
}

visualization_msgs::Marker marker = createAreaMarker("road_mark", id++, color, vmap, area);
if (isValidMarker(marker))
marker_array.markers.push_back(marker);
else
ROS_ERROR_STREAM("[createRoadMarkMarkerArray] failed createAreaMarker: " << area);
}
return marker_array;
}

cross walk

image-20200401144425841

人行横道有三种类型,闭合线、条纹图案和自行车道。cross walk一般也用area来描述,aid是cross_walk的唯一标志,type表示cross walk的类型,

image-20200120134008724

image-20200120134118776

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
visualization_msgs::MarkerArray createCrossWalkMarkerArray(const VectorMap &vmap, Color color) {
visualization_msgs::MarkerArray marker_array;
int id = 0;
for (const auto &cross_walk : vmap.findByFilter([](const CrossWalk &cross_walk) { return true; })) {
if (cross_walk.aid == 0) {
ROS_ERROR_STREAM("[createCrossWalkMarkerArray] invalid cross_walk: " << cross_walk);
continue;
}

Area area = vmap.findByKey(Key<Area>(cross_walk.aid));//通过aid寻找area
if (area.aid == 0) {
ROS_ERROR_STREAM("[createCrossWalkMarkerArray] invalid area: " << area);
continue;
}

visualization_msgs::Marker marker = createAreaMarker("cross_walk", id++, color, vmap, area);
if (isValidMarker(marker))
marker_array.markers.push_back(marker);
else
ROS_ERROR_STREAM("[createCrossWalkMarkerArray] failed createAreaMarker: " << area);
}
return marker_array;
}

zebra zone

依然用area描述

image-20200120134234134

image-20200120134309863

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
visualization_msgs::MarkerArray createZebraZoneMarkerArray(const VectorMap &vmap, Color color) {
visualization_msgs::MarkerArray marker_array;
int id = 0;
for (const auto &zebra_zone : vmap.findByFilter([](const ZebraZone &zebra_zone) { return true; })) {
if (zebra_zone.aid == 0) {
ROS_ERROR_STREAM("[createZebraZoneMarkerArray] invalid zebra_zone: " << zebra_zone);
continue;
}

Area area = vmap.findByKey(Key<Area>(zebra_zone.aid));
if (area.aid == 0) {
ROS_ERROR_STREAM("[createZebraZoneMarkerArray] invalid area: " << area);
continue;
}

visualization_msgs::Marker marker = createAreaMarker("zebra_zone", id++, color, vmap, area);
if (isValidMarker(marker))
marker_array.markers.push_back(marker);
else
ROS_ERROR_STREAM("[createZebraZoneMarkerArray] failed createAreaMarker: " << area);
}
return marker_array;
}

road sign 

image-20200401144852776

表示道路上带杆的方向指示牌,方向用vector描述,杆用pole表示,一般有两种类型,一个是停止标志,另一个是NULL

image-20200120134737174

image-20200120134827929

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
 visualization_msgs::MarkerArray  createRoadSignMarkerArray(const VectorMap &vmap, Color sign_color, Color pole_color) {
visualization_msgs::MarkerArray marker_array;
int id = 0;
for (const auto &road_sign : vmap.findByFilter([](const RoadSign &road_sign) { return true; })) {
if (road_sign.vid == 0) {
ROS_ERROR_STREAM("[createRoadSignMarkerArray] invalid road_sign: " << road_sign);
continue;
}
//找到vector
Vector vector = vmap.findByKey(Key<Vector>(road_sign.vid));
if (vector.vid == 0) {
ROS_ERROR_STREAM("[createRoadSignMarkerArray] invalid vector: " << vector);
continue;
}
//找到pole
Pole pole;
if (road_sign.plid != 0) {
pole = vmap.findByKey(Key<Pole>(road_sign.plid));
if (pole.plid == 0) {
ROS_ERROR_STREAM("[createRoadSignMarkerArray] invalid pole: " << pole);
continue;
}
}
//创建vector箭头
visualization_msgs::Marker vector_marker = createVectorMarker("road_sign", id++, sign_color, vmap, vector);
if (isValidMarker(vector_marker))
marker_array.markers.push_back(vector_marker);
else
ROS_ERROR_STREAM("[createRoadSignMarkerArray] failed createVectorMarker: " << vector);
//创建pole
if (road_sign.plid != 0) {
visualization_msgs::Marker pole_marker = createPoleMarker("road_sign", id++, pole_color, vmap, pole);
if (isValidMarker(pole_marker))
marker_array.markers.push_back(pole_marker);
else
ROS_ERROR_STREAM("[createRoadSignMarkerArray] failed createPoleMarker: " << pole);
}
}
return marker_array;
}

road pole

image-20200120134859587

image-20200120134943132

image-20200120135002861

signal

image-20200401144917512

image-20200521142039722

vid指的是此信号灯对应的vector,plid指其关联的pole,type指其语义,linkid关联的是

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
visualization_msgs::MarkerArray createSignalMarkerArray(const VectorMap &vmap, Color red_color, Color blue_color,
Color yellow_color, Color other_color, Color pole_color) {
visualization_msgs::MarkerArray marker_array;
int id = 0;
for (const auto &signal : vmap.findByFilter([](const Signal &signal) { return true; })) {
if (signal.vid == 0) {
ROS_ERROR_STREAM("[createSignalMarkerArray] invalid signal: " << signal);
continue;
}

Vector vector = vmap.findByKey(Key<Vector>(signal.vid));//用vector来描述红绿灯
if (vector.vid == 0) {
ROS_ERROR_STREAM("[createSignalMarkerArray] invalid vector: " << vector);
continue;
}

Pole pole;
if (signal.plid != 0) {
pole = vmap.findByKey(Key<Pole>(signal.plid));//找到相关联的pole
if (pole.plid == 0) {
ROS_ERROR_STREAM("[createSignalMarkerArray] invalid pole: " << pole);
continue;
}
}

visualization_msgs::Marker vector_marker;
switch (signal.type) { //红绿灯颜色 红黄蓝
case Signal::RED:
case Signal::PEDESTRIAN_RED:
vector_marker = createVectorMarker("signal", id++, red_color, vmap, vector);
break;
case Signal::BLUE:
case Signal::PEDESTRIAN_BLUE:
vector_marker = createVectorMarker("signal", id++, blue_color, vmap, vector);
break;
case Signal::YELLOW:
vector_marker = createVectorMarker("signal", id++, yellow_color, vmap, vector);
break;
case Signal::RED_LEFT:
vector_marker = createVectorMarker("signal", id++, Color::LIGHT_RED, vmap, vector);
break;
case Signal::BLUE_LEFT:
vector_marker = createVectorMarker("signal", id++, Color::LIGHT_GREEN, vmap, vector);
break;
case Signal::YELLOW_LEFT:
vector_marker = createVectorMarker("signal", id++, Color::LIGHT_YELLOW, vmap, vector);
break;
case Signal::OTHER:
vector_marker = createVectorMarker("signal", id++, other_color, vmap, vector);
break;
default:
ROS_WARN_STREAM("[createSignalMarkerArray] unknown signal.type: " << signal.type
<< " Creating Marker as OTHER.");
vector_marker = createVectorMarker("signal", id++, Color::GRAY, vmap, vector);
break;
}
if (isValidMarker(vector_marker))// action:ADD
marker_array.markers.push_back(vector_marker);
else
ROS_ERROR_STREAM("[createSignalMarkerArray] failed createVectorMarker: " << vector);

if (signal.plid != 0) {//创建pole
visualization_msgs::Marker pole_marker = createPoleMarker("signal", id++, pole_color, vmap, pole);
if (isValidMarker(pole_marker))
marker_array.markers.push_back(pole_marker);
else
ROS_ERROR_STREAM("[createSignalMarkerArray] failed createPoleMarker: " << pole);
}
}
return marker_array;
}

image-20200120135034537

image-20200120135115439

curb

image-20200120135213315

image-20200120135232695

road edge

image-20200120135325666

image-20200120135258133

gutter

image-20200120135345714

image-20200120135408597

street light

image-20200120135629055

image-20200120135503322

utility pole

image-20200120135653528

image-20200120135524580

后面的这几个元素比较简单,就不一一描述代码了。这些基础的元素定义好其ros msg之后,后续在感知和规划模块需要用到相关的信息,需要我们把整个高精地图数据通过ROS Pub出来,由各模块去订阅相应的节点使用即可。

ROS发布地图数据

这部分正确的顺序应该是,我们首先加载地图数据CSV文件,然后可视化并发布出来。各元素数据在地图里面都有很多,比如很多条white_line,所以ros msg,我们还需要定义xxARRAY,如下图。

image-20200510203336365

发布地图元素数据

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
ros::Publisher point_pub = nh.advertise<PointArray>("vector_map_info/point", 1, true);
ros::Publisher vector_pub = nh.advertise<VectorArray>("vector_map_info/vector", 1, true);
ros::Publisher line_pub = nh.advertise<LineArray>("vector_map_info/line", 1, true);
ros::Publisher area_pub = nh.advertise<AreaArray>("vector_map_info/area", 1, true);
ros::Publisher pole_pub = nh.advertise<PoleArray>("vector_map_info/pole", 1, true);
ros::Publisher box_pub = nh.advertise<BoxArray>("vector_map_info/box", 1, true);
ros::Publisher dtlane_pub = nh.advertise<DTLaneArray>("vector_map_info/dtlane", 1, true);
ros::Publisher node_pub = nh.advertise<NodeArray>("vector_map_info/node", 1, true);
ros::Publisher lane_pub = nh.advertise<LaneArray>("vector_map_info/lane", 1, true); // 发布lane信息
ros::Publisher way_area_pub = nh.advertise<WayAreaArray>("vector_map_info/way_area", 1, true);
ros::Publisher road_edge_pub = nh.advertise<RoadEdgeArray>("vector_map_info/road_edge", 1, true);
ros::Publisher gutter_pub = nh.advertise<GutterArray>("vector_map_info/gutter", 1, true);
ros::Publisher curb_pub = nh.advertise<CurbArray>("vector_map_info/curb", 1, true);
ros::Publisher white_line_pub = nh.advertise<WhiteLineArray>("vector_map_info/white_line", 1, true);//白线
ros::Publisher stop_line_pub = nh.advertise<StopLineArray>("vector_map_info/stop_line", 1, true);//停止线
ros::Publisher zebra_zone_pub = nh.advertise<ZebraZoneArray>("vector_map_info/zebra_zone", 1, true);
ros::Publisher cross_walk_pub = nh.advertise<CrossWalkArray>("vector_map_info/cross_walk", 1, true);
ros::Publisher road_mark_pub = nh.advertise<RoadMarkArray>("vector_map_info/road_mark", 1, true);
ros::Publisher road_pole_pub = nh.advertise<RoadPoleArray>("vector_map_info/road_pole", 1, true);
ros::Publisher road_sign_pub = nh.advertise<RoadSignArray>("vector_map_info/road_sign", 1, true);
ros::Publisher signal_pub = nh.advertise<SignalArray>("vector_map_info/signal", 1, true);//信号灯
ros::Publisher street_light_pub = nh.advertise<StreetLightArray>("vector_map_info/street_light", 1, true);
ros::Publisher utility_pole_pub = nh.advertise<UtilityPoleArray>("vector_map_info/utility_pole", 1, true);
ros::Publisher guard_rail_pub = nh.advertise<GuardRailArray>("vector_map_info/guard_rail", 1, true);
ros::Publisher side_walk_pub = nh.advertise<SideWalkArray>("vector_map_info/side_walk", 1, true);
ros::Publisher drive_on_portion_pub = nh.advertise<DriveOnPortionArray>("vector_map_info/drive_on_portion", 1,
true);
ros::Publisher cross_road_pub = nh.advertise<CrossRoadArray>("vector_map_info/cross_road", 1, true);
ros::Publisher side_strip_pub = nh.advertise<SideStripArray>("vector_map_info/side_strip", 1, true);
ros::Publisher curve_mirror_pub = nh.advertise<CurveMirrorArray>("vector_map_info/curve_mirror", 1, true);
ros::Publisher wall_pub = nh.advertise<WallArray>("vector_map_info/wall", 1, true);
ros::Publisher fence_pub = nh.advertise<FenceArray>("vector_map_info/fence", 1, true);
ros::Publisher rail_crossing_pub = nh.advertise<RailCrossingArray>("vector_map_info/rail_crossing", 1, true);

发布地图可视化,各元素的可视化方法已经在上节做出了描述,这里直接跳过。

1
ros::Publisher marker_array_pub = nh.advertise<visualization_msgs::MarkerArray>("vector_map", 1, true);

加载地图数据文件,并将其pub出来

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
vector_map::category_t category = Category::NONE;
for (const auto &file_path : file_paths) {
std::string file_name(basename(file_path.c_str()));
if (file_name == "idx.csv") { ;
} else if (file_name == "point.csv") {
point_pub.publish(createObjectArray<Point, PointArray>(file_path));
category |= Category::POINT;
} else if (file_name == "vector.csv") {
vector_pub.publish(createObjectArray<Vector, VectorArray>(file_path));
category |= Category::VECTOR;
} else if (file_name == "line.csv") {
line_pub.publish(createObjectArray<Line, LineArray>(file_path));
category |= Category::LINE;
} else if (file_name == "area.csv") {
area_pub.publish(createObjectArray<Area, AreaArray>(file_path));
category |= Category::AREA;
} else if (file_name == "pole.csv") {
pole_pub.publish(createObjectArray<Pole, PoleArray>(file_path));
category |= Category::POLE;
} else if (file_name == "box.csv") {
box_pub.publish(createObjectArray<Box, BoxArray>(file_path));
category |= Category::BOX;
} else if (file_name == "dtlane.csv") {
dtlane_pub.publish(createObjectArray<DTLane, DTLaneArray>(file_path));
category |= Category::DTLANE;
} else if (file_name == "node.csv") {
node_pub.publish(createObjectArray<Node, NodeArray>(file_path));
category |= Category::NODE;
} else if (file_name == "lane.csv") {
lane_pub.publish(createObjectArray<Lane, LaneArray>(file_path));
category |= Category::LANE;
} else if (file_name == "wayarea.csv") {
way_area_pub.publish(createObjectArray<WayArea, WayAreaArray>(file_path));
category |= Category::WAY_AREA;
} else if (file_name == "roadedge.csv") {
road_edge_pub.publish(createObjectArray<RoadEdge, RoadEdgeArray>(file_path));
category |= Category::ROAD_EDGE;
} else if (file_name == "gutter.csv") {
gutter_pub.publish(createObjectArray<Gutter, GutterArray>(file_path));
category |= Category::GUTTER;
} else if (file_name == "curb.csv") {
curb_pub.publish(createObjectArray<Curb, CurbArray>(file_path));
category |= Category::CURB;
} else if (file_name == "whiteline.csv") {
white_line_pub.publish(createObjectArray<WhiteLine, WhiteLineArray>(file_path));
category |= Category::WHITE_LINE;
} else if (file_name == "stopline.csv") {
stop_line_pub.publish(createObjectArray<StopLine, StopLineArray>(file_path));
category |= Category::STOP_LINE;
} else if (file_name == "zebrazone.csv") {
zebra_zone_pub.publish(createObjectArray<ZebraZone, ZebraZoneArray>(file_path));
category |= Category::ZEBRA_ZONE;
} else if (file_name == "crosswalk.csv") {
cross_walk_pub.publish(createObjectArray<CrossWalk, CrossWalkArray>(file_path));
category |= Category::CROSS_WALK;
} else if (file_name == "road_surface_mark.csv") {
road_mark_pub.publish(createObjectArray<RoadMark, RoadMarkArray>(file_path));
category |= Category::ROAD_MARK;
} else if (file_name == "poledata.csv") {
road_pole_pub.publish(createObjectArray<RoadPole, RoadPoleArray>(file_path));
category |= Category::ROAD_POLE;
} else if (file_name == "roadsign.csv") {
road_sign_pub.publish(createObjectArray<RoadSign, RoadSignArray>(file_path));
category |= Category::ROAD_SIGN;
} else if (file_name == "signaldata.csv") {
signal_pub.publish(createObjectArray<Signal, SignalArray>(file_path));
category |= Category::SIGNAL;
} else if (file_name == "streetlight.csv") {
street_light_pub.publish(createObjectArray<StreetLight, StreetLightArray>(file_path));
category |= Category::STREET_LIGHT;
} else if (file_name == "utilitypole.csv") {
utility_pole_pub.publish(createObjectArray<UtilityPole, UtilityPoleArray>(file_path));
category |= Category::UTILITY_POLE;
} else if (file_name == "guardrail.csv") {
guard_rail_pub.publish(createObjectArray<GuardRail, GuardRailArray>(file_path));
category |= Category::GUARD_RAIL;
} else if (file_name == "sidewalk.csv") {
side_walk_pub.publish(createObjectArray<SideWalk, SideWalkArray>(file_path));
category |= Category::SIDE_WALK;
} else if (file_name == "driveon_portion.csv") {
drive_on_portion_pub.publish(createObjectArray<DriveOnPortion, DriveOnPortionArray>(file_path));
category |= Category::DRIVE_ON_PORTION;
} else if (file_name == "intersection.csv") {
cross_road_pub.publish(createObjectArray<CrossRoad, CrossRoadArray>(file_path));
category |= Category::CROSS_ROAD;
} else if (file_name == "sidestrip.csv") {
side_strip_pub.publish(createObjectArray<SideStrip, SideStripArray>(file_path));
category |= Category::SIDE_STRIP;
} else if (file_name == "curvemirror.csv") {
curve_mirror_pub.publish(createObjectArray<CurveMirror, CurveMirrorArray>(file_path));
category |= Category::CURVE_MIRROR;
} else if (file_name == "wall.csv") {
wall_pub.publish(createObjectArray<Wall, WallArray>(file_path));
category |= Category::WALL;
} else if (file_name == "fence.csv") {
fence_pub.publish(createObjectArray<Fence, FenceArray>(file_path));
category |= Category::FENCE;
} else if (file_name == "railroad_crossing.csv") {
rail_crossing_pub.publish(createObjectArray<RailCrossing, RailCrossingArray>(file_path));
category |= Category::RAIL_CROSSING;
} else
ROS_ERROR_STREAM("unknown csv file: " << file_path);
}

rviz可视化

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
VectorMap vmap;
vmap.subscribe(nh, category);

visualization_msgs::MarkerArray marker_array;
insertMarkerArray(marker_array, createRoadEdgeMarkerArray(vmap, Color::BLUE));
insertMarkerArray(marker_array, createGutterMarkerArray(vmap, Color::GRAY, Color::GRAY, Color::GRAY));
insertMarkerArray(marker_array, createCurbMarkerArray(vmap, Color::GRAY));
insertMarkerArray(marker_array, createWhiteLineMarkerArray(vmap, Color::WHITE, Color::YELLOW));
insertMarkerArray(marker_array, createStopLineMarkerArray(vmap, Color::RED));
insertMarkerArray(marker_array, createZebraZoneMarkerArray(vmap, Color::WHITE));
insertMarkerArray(marker_array, createCrossWalkMarkerArray(vmap, Color::WHITE));
insertMarkerArray(marker_array, createRoadMarkMarkerArray(vmap, Color::WHITE));
insertMarkerArray(marker_array, createRoadPoleMarkerArray(vmap, Color::GRAY));
insertMarkerArray(marker_array, createRoadSignMarkerArray(vmap, Color::GREEN, Color::GRAY));
insertMarkerArray(marker_array, createSignalMarkerArray(vmap, Color::RED, Color::BLUE, Color::YELLOW, Color::CYAN,
Color::GRAY));
insertMarkerArray(marker_array, createStreetLightMarkerArray(vmap, Color::YELLOW, Color::GRAY));
insertMarkerArray(marker_array, createUtilityPoleMarkerArray(vmap, Color::GRAY));
insertMarkerArray(marker_array, createGuardRailMarkerArray(vmap, Color::LIGHT_BLUE));
insertMarkerArray(marker_array, createSideWalkMarkerArray(vmap, Color::GRAY));
insertMarkerArray(marker_array, createDriveOnPortionMarkerArray(vmap, Color::LIGHT_CYAN));
insertMarkerArray(marker_array, createCrossRoadMarkerArray(vmap, Color::LIGHT_GREEN));
insertMarkerArray(marker_array, createSideStripMarkerArray(vmap, Color::GRAY));
insertMarkerArray(marker_array, createCurveMirrorMarkerArray(vmap, Color::MAGENTA, Color::GRAY));
insertMarkerArray(marker_array, createWallMarkerArray(vmap, Color::LIGHT_YELLOW));
insertMarkerArray(marker_array, createFenceMarkerArray(vmap, Color::LIGHT_RED));
insertMarkerArray(marker_array, createRailCrossingMarkerArray(vmap, Color::LIGHT_MAGENTA));
marker_array_pub.publish(marker_array); // 发布marker
-------------    本文结束  感谢您的阅读    -------------
胡想成 wechat
欢迎关注个人公众号!
您的支持,是我装逼的最大动力!

本文标题:自动驾驶实战系列(七)——高精地图制作流程详解与实践

文章作者:胡想成

发布时间:2020年05月14日 - 23:05

最后更新:2020年05月21日 - 14:05

原始链接:xchu.net/2020/05/14/40hdmap/

版权声明:本博客所有文章除特别声明外,均采用 CC BY-NC-SA 4.0许可协议,转载请注明出处!