详解Apllo简易地图绘制工具Step By Step
2023-12-01 # 实用教程 #Autonomous Driving #Apollo #HD Map #Python

关于

高精地图对于自动驾驶的重要性是不言而喻的，但是有关Apollo高精地图制作的资料真的少之又少。对此我想通过修改Apollo自带的绘制简易地图的工具，来生成官方demo一样的高精地图。不过先得理解下这套工具，也就有了本文。

绘制工具

Apollo的简易高精地图绘制工具在/apollo/modules/tools/map_gen目录下。

1
2
3
4
5
6
7
8
map_gen/　
├── add_signal.py                   //添加交通信号
├── BUILD                           //编译文件
├── extract_path.py                 //读取录制轨迹
├── map_gen.py                      //生成三车道地图
├── map_gen_single_lane.py          //生成单车道地图
├── map_gen_two_lanes_right_ext.py  //生成双车道地图
└── plot_path.py                    //显示录制轨迹

extract_path

主要读取提前录制好的bag包（可以是多个包），只提取里面的/apollo/localization/pose这个通道的消息（即定位数据），然后写到一个文件中。

1
2
3
4
5
6
7
8
9
10
11
12
13
filename = sys.argv[1]      //保存录制轨迹的文件路径
fbags = sys.argv[2:]        //使用列表切片获取从终端读入的多个包

with open(filename, 'w') as f:      //打开文件  
    for fbag in fbags:              //遍历多个包
        reader = RecordReader(fbag) //创建一个RecordReader()对象来读取每个包
        for msg in reader.read_messages():  //遍历包中的消息
            if msg.topic == "/apollo/localization/pose":    //如果是定位数据就把x和ｙ信息写入文件
                localization = localization_pb2.LocalizationEstimate()
                localization.ParseFromString(msg.message)
                x = localization.pose.position.x
                y = localization.pose.position.y
                f.write(str(x) + "," + str(y) + "\n")

plot_path

主要读取extract_path解析出来的轨迹信息，然后用matplot这个库来显示这条轨迹。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
f = open(sys.argv[1], 'r')      //轨迹文件
xs = []     //存放ｘ的列表
ys = []
for line in f:      //读取每一行
    line = line.replace("\n", '')   //替换掉每行末尾的换行符
    data = line.split(',')          //通过逗号提取ｘ跟ｙ
    x = float(data[0])
    y = float(data[1])
    xs.append(x)
    ys.append(y)
f.close()

//显示轨迹
fig = plt.figure()  
ax = plt.subplot2grid((1, 1), (0, 0))
ax.plot(xs, ys, "b-", lw=3, alpha=0.8)
ax.axis('equal')
plt.show()

在运行此代码时，可能会遇到如下报错。

plot_path.py:38: UserWarning: Matplotlib is currently using agg, which is a non-GUI backend, so cannot show the figure.

在docker环境下，通过apt安装tk库即可解决。

sudo apt-get install python3-tk

详细solution可参考此文。

map_gen_single_lane

map_gen、map_gen_single_lane跟map_gen_two_lanes_right_ext这三个代码很像，只是生成地图的车道数为三车道、单车道跟双车道这个区别，生成步骤基本一致，所以我以单车道来分析下。

一段段来看吧，首先是预处理部分，下面有我的注释。

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
// 读取轨迹点至points列表里
fpath = sys.argv[1]
f = open(fpath, 'r')
points = []
for line in f:
    line = line.replace("\n", '')
    data = line.split(',')
    x = float(data[0])
    y = float(data[1])
    points.append((x, y))

// 用shapely.LineString对象创建大概的path（线段）
// 关于LineString对象的具体内容可参考官方文档: https://shapely.readthedocs.io/en/stable/reference/shapely.LineString.html#shapely.LineString
path = LineString(points)
length = int(path.length)

extra_roi_extension = float(sys.argv[3])

// 打开存放地图的文件，以及初始化地图
// 有关python使用protobuf的相关内容请移步此处: https://developers.google.com/protocol-buffers/docs/pythontutorial#writing-a-message
fmap = open(sys.argv[2], 'w')
id = 0
map = map_pb2.Map()     // 构造地图对象
road = map.road.add()   //　add()方法添加初始的road信息
road.id.id = "1"
section = road.section.add()
section.id.id = "2"
lane = None

然后就是主程序，它是以100条path来构成一条lane的，所以一条lane长100米，且由central_curve、left_boundary和right_boundary这三部分组成。

处理流程有点长，但是其实代码重复的地方很多，所以我们就看下第一条path是咋处理的即可，其他就方便举一反三了。

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
for i in range(length - 1): // 遍历所有的path
    if i % 100 == 0:        // 100条path构成一条lane
        id += 1             
        if lane is not None:    //　如果已经有lane了，原来lane的下一条lane id为当前id值
            lane.successor_id.add().id = str(id)
         
        //　用create_lane()函数创建lane，主要把车道、中心线和左右边界能填充的内容都填充好，然后返回
        lane, central, left_boundary, right_boundary = create_lane(map, id)
        // RoadSection与当前lane　id绑定
        section.lane_id.add().id = str(id)

        // 初始化左右边界
        left_edge = section.boundary.outer_polygon.edge.add()
        left_edge.type = map_road_pb2.BoundaryEdge.LEFT_BOUNDARY
        left_edge_segment = left_edge.curve.segment.add()

        right_edge = section.boundary.outer_polygon.edge.add()
        right_edge.type = map_road_pb2.BoundaryEdge.RIGHT_BOUNDARY
        right_edge_segment = right_edge.curve.segment.add()

    // 初始化左右边界点和lane的中心点，并计算
    left_bound_point = left_boundary.line_segment.point.add()
    right_bound_point = right_boundary.line_segment.point.add()
    central_point = central.line_segment.point.add()

    right_edge_point = right_edge_segment.line_segment.point.add()
    left_edge_point = left_edge_segment.line_segment.point.add()

    // interpolate()会返回指定距离的点，即这里返回的p就是第一个点附近的点
    p = path.interpolate(i)
    p2 = path.interpolate(i + 0.5)  //　p2为跟第一个点相距0.5m的点
    distance = LANE_WIDTH / 2.0     //　LANE_WIDTH=3在上面定义
    // lp与rp分别为左右边界点，通过convert()函数计算出
    lp, rp = convert(p, p2, distance)

    // 计算出中心点与左右边界点后放到到lane结构中
    central_point.x = p.x
    central_point.y = p.y
    left_bound_point.y = lp[1]
    left_bound_point.x = lp[0]
    right_bound_point.y = rp[1]
    right_bound_point.x = rp[0]

    left_edge_point.y = lp[1]
    left_edge_point.x = lp[0]
    right_edge_point.y = rp[1]
    right_edge_point.x = rp[0]

    // left_sample跟right_sample记录lane的横纵向距离
    left_sample = lane.left_sample.add()
    left_sample.s = i % 100 + 1
    left_sample.width = LANE_WIDTH / 2.0

    right_sample = lane.right_sample.add()
    right_sample.s = i % 100 + 1
    right_sample.width = LANE_WIDTH / 2.0

光看代码分析计算过程会比较吃力，为此我特地画了个图方便大家理解。

catch_points

这里p为中心点，lp与rp分别为计算出来的左右边界点。其实这样看还蛮简单的，只用到了三角函数与勾股定理。

add_signal

这个程序不复杂就是读取两个文件，一个是上一步生成的地图文件，另一个是存放信号标识的文件，然后找到lane与signal中的stopline重叠的点，依此填充相应内容给map里signal字段下的overlap，最后写到一个新的地图文件里。

我们就看下主要的代码段吧。

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
// 从之前生成的map中读取所有的lane
lanes = {}
lanes_map = {}
for lane in map.lane:
    lane_points = []
    lanes_map[lane.id.id] = lane
    for segment in lane.central_curve.segment:
        for point in segment.line_segment.point:
            lane_points.append((point.x, point.y))
    lane_string = LineString(lane_points)
    lanes[lane.id.id] = lane_string

lines = {}
for stop_line in signal.stop_line: 

    //　从signal文件中读取所有stop_line的点
    stop_line_points = []
    for segment in stop_line.segment:
        for point in segment.line_segment.point:
            stop_line_points.append((point.x, point.y))
    stop_line_string = LineString(stop_line_points)

    // 遍历所有的lane
    for lane_id, lane_string in lanes.items():
        p = stop_line_string.intersection(lane_string)
        if type(p) == Point:    // 如果与stop line重合就为overlap赋值
            s = lane_string.project(p)
            overlap = map.overlap.add()
            overlap.id.id = str(lane_id) + "_" + str(signal.id.id)
            obj = overlap.object.add()
            obj.id.id = signal.id.id
            obj.signal_overlap_info.CopyFrom(
                map_overlap_pb2.SignalOverlapInfo())
            obj = overlap.object.add()
            obj.id.id = lane_id
            obj.lane_overlap_info.start_s = s
            obj.lane_overlap_info.end_s = s + 0.1
            obj.lane_overlap_info.is_merge = False

            signal.overlap_id.add().id = overlap.id.id
            lanes_map[lane_id].overlap_id.add().id = overlap.id.id
map.signal.add().CopyFrom(signal)

程序没啥多讲的，就是信号标识文件怎么来的很让人疑惑。

参考

  1. 地图数据结构看着蛮复杂的，如果有些字段有疑惑可以借助此文‘dig into apollo map’来理解。
  2. 简易地图的制作过程可参考此文‘apollo简易制图过程(二十)’
上一页
2023-12-01 13:50:35 # 实用教程 #Autonomous Driving #Apollo #HD Map #Python
下一页