Autoware planning模块学习笔记（二）：路径规划（4）- 节点lane_navi


Autoware planning模块学习笔记（二）：路径规划（4）- 节点lane_navi

写在前面：引用的时候请注明出处（I’m ZhengKX的博客：https://blog.csdn.net/xiaoxiao123jun），尊重他人的原创劳动成果，不打击原作者的创作激情，才能更好地促进我国科教进步，谢谢！

继续为大家讲解Autoware planning模块，前面为大家分析了“勾选waypoint_loader”后所启动的三个节点waypoint_loader，waypoint_replanner和waypoint_marker_publisher。这篇博客继续分析后续操作。

勾选Mission Planning->lane_planner->lane_navi

在这里插入图片描述
我们依然查找src/autoware/utilities/runtime_manager/scripts/computing.yaml可知勾选lane_navi（防盗标记：zhengkunxian）后运行ROS包lane_planner内的lane_navi.launch。

在这里插入图片描述lane_navi.launch如下：
在这里插入图片描述也就是启动lane_navi和waypoint_marker_publisher两个节点，waypoint_marker_publisher这一节点在Autoware planning模块学习笔记（二）：路径规划（3）- 节点waypoint_marker_publisher中已经分析完毕，不再赘述了。

由lane_planner的CMakeLists.txt可知节点lane_navi的源文件是nodes/lane_navi/lane_navi.cpp。
在这里插入图片描述

节点lane_navi

首先是其main函数，main函数在src/autoware/core_planning/lane_planner/nodes/lane_navi/lane_navi.cpp中。首先设置了一些参数，接着设置发布者和监听者。

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
int main(int argc, char **argv)
{
    ros::init(argc, argv, "lane_navi");

    ros::NodeHandle n;

    int sub_vmap_queue_size;
    n.param<int>("/lane_navi/sub_vmap_queue_size", sub_vmap_queue_size, 1);//由launch文件传入参数，
    //在没有获取到参数值的时候，可以设置默认值。
    int sub_route_queue_size;
    n.param<int>("/lane_navi/sub_route_queue_size", sub_route_queue_size, 1);
    int pub_waypoint_queue_size;
    n.param<int>("/lane_navi/pub_waypoint_queue_size", pub_waypoint_queue_size, 1);
    bool pub_waypoint_latch;
    n.param<bool>("/lane_navi/pub_waypoint_latch", pub_waypoint_latch, true);

    n.param<int>("/lane_navi/waypoint_max", waypoint_max, 10000);
    n.param<double>("/lane_navi/search_radius", search_radius, 10);
    n.param<double>("/lane_navi/velocity", velocity, 40);
    n.param<std::string>("/lane_navi/frame_id", frame_id, "map");
    n.param<std::string>("/lane_navi/output_file", output_file, "/tmp/lane_waypoint.csv");

    //检验output_file是否合理
    if (output_file.empty()) {
        ROS_ERROR_STREAM("output filename is empty");
        return EXIT_FAILURE;
    }
    if (output_file.back() == '/') {
        ROS_ERROR_STREAM(output_file << " is directory");
        return EXIT_FAILURE;
    }

    waypoint_pub = n.advertise<autoware_msgs::LaneArray>("/lane_waypoints_array", pub_waypoint_queue_size,
                                 pub_waypoint_latch);

    ros::Subscriber route_sub = n.subscribe("/route_cmd", sub_route_queue_size, create_waypoint);
    ros::Subscriber point_sub = n.subscribe("/vector_map_info/point", sub_vmap_queue_size, cache_point);
    ros::Subscriber lane_sub = n.subscribe("/vector_map_info/lane", sub_vmap_queue_size, cache_lane);
    ros::Subscriber node_sub = n.subscribe("/vector_map_info/node", sub_vmap_queue_size, cache_node);

    ros::spin();

    return EXIT_SUCCESS;
}

1. cache_point，cache_lane和cache_node函数

cache_point，cache_lane和cache_node函数分别是对应于话题"/vector_map_info/point"，"/vector_map_info/lane"和"/vector_map_info/node"的回调函数。all_vmap的定义是lane_planner::vmap::VectorMap all_vmap;。

1
2
3
4
5
void cache_point(const vector_map::PointArray& msg)
{
    all_vmap.points = msg.data;
    update_values();
}
1
2
3
4
5
void cache_lane(const vector_map::LaneArray& msg)
{
    all_vmap.lanes = msg.data;
    update_values();
}
1
2
3
4
5
void cache_node(const vector_map::NodeArray& msg)
{
    all_vmap.nodes = msg.data;
    update_values();
}

1.1 lane_planner::vmap::VectorMap函数

定义于src/autoware/core_planning/lane_planner/include/lane_planner/lane_planner_vmap.hpp。用于储存决策规划所需要的部分矢量地图要素数据。
在这里插入图片描述其中vector_map定义于src/autoware/common/vector_map/include/vector_map/vector_map.h，里面包含了构成矢量地图所需的要素。Autoware将激光点云数据进行分类和下采样完成后，把所有的点按照其定义的类型重新组织，其中point代表点云中某点，Autoware对每个点进行唯一编号，包含经纬高blh和平面xy坐标。之后根据分类好的元素，按照line、vector、area分别组织数据。
在这里插入图片描述
用于储存决策规划所需要的部分矢量地图信息的VectorMap中包含了vector_map::Point，vector_map::Lane，vector_map::Node，vector_map::StopLine和vector_map::DTLane，它们的定义分别如下，具体含义参考自动驾驶实战系列(七)——高精地图制作流程实践（北航计算机学院的大佬呦～）：

（1）Point.msg

位于src/autoware/messages/vector_map_msgs/msg/Point.msg。记录所有的点云的经纬高blh和平面xy坐标 ，并用pid唯一编号，这里xy为utm坐标值（当然也可以是enu坐标）。

1
2
3
4
5
6
7
8
9
10
11
# Ver 1.00
int32 pid
float64 b
float64 l
float64 h
float64 bx
float64 ly
int32 ref
int32 mcode1
int32 mcode2
int32 mcode3

（2）Lane.msg

位于src/autoware/messages/vector_map_msgs/msg/Lane.msg。Lane用来定义车辆行驶的车道，并用lnid唯一编号，它由两个Node组成，bnid是起始Node的nid（识别编号），fnid是终点Node的nid。几个跟决策联系比较紧密的参数：（1）jct：指定当前车道分支/合并模式（ 0：正常，1：分支到左边，2：分支到右边，3：合并到左车道，4：合并到右车道 ）；（2）blid2-4：指定要合并到当前车道的Lane ID。(0表示没有合并lane)；（3）flid2-4：指定要分流出去的车道Lane ID。(当车道没有分支时，选择0)；（4）clossid：只使用在十字路口，输入clossid，否则值为0；（5）lcnt：车道数。(内部交叉是0)；（6）lno：本车道编号。从左边的车道开始编号（内部交叉是0）；（7）lanetype：车道类型。（ 0：直行，1：左转，2：右转 ）；（防盗标记：zhengkunxian）（8）limitvel ：设定的最大速度；（9）refvel：目标运动速度(驾驶期间估计速度)；（10）roadsecid：指定路段。(在十字路口是0，相邻车道通常具有相同的RoadSecID)；（11）lanecfgfg：是否车道允许变道。(0：允许，1：不允许)；（12）linkwaid：指定WayArea ID。WayArea定义了车辆可以行驶的道路区域。

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
# jct
uint8 NORMAL=0
uint8 LEFT_BRANCHING=1
uint8 RIGHT_BRANCHING=2
uint8 LEFT_MERGING=3
uint8 RIGHT_MERGING=4
uint8 COMPOSITION=5

# lanetype
uint8 STRAIGHT=0
uint8 LEFT_TURN=1
uint8 RIGHT_TURN=2

# lanecfgfg
uint8 PASS=0
uint8 FAIL=1

# Ver 1.00
int32 lnid
int32 did
int32 blid
int32 flid
int32 bnid
int32 fnid
int32 jct
int32 blid2
int32 blid3
int32 blid4
int32 flid2
int32 flid3
int32 flid4
int32 clossid
float64 span
int32 lcnt
int32 lno

# Ver 1.20
int32 lanetype
int32 limitvel
int32 refvel
int32 roadsecid
int32 lanecfgfg
int32 linkwaid

（3）Node.msg

位于src/autoware/messages/vector_map_msgs/msg/Node.msg。根据命名规则可知nid为Node的识别编号，参照其他元素可知这是Node的唯一标志。同样参照命名规则可知pid是Point的id，由此我们知道Node和Point之间的关系，即一个Node就是一个Point。

1
2
3
# Ver 1.00
int32 nid
int32 pid

（4）StopLine.msg

位于src/autoware/messages/vector_map_msgs/msg/StopLine.msg。id是StopLine的唯一标志，StopLine是用Line来描述，其中lid指对应的Line的id。

1
2
3
4
5
6
# Ver 1.00
int32 id
int32 lid
int32 tlid
int32 signid
int32 linkid

在这里插入图片描述

Line.msg

1
2
3
4
5
6
# Ver 1.00
int32 lid
int32 bpid
int32 fpid
int32 blid
int32 flid

定义于src/autoware/messages/vector_map_msgs/msg/Line.msg。Line表示线段，下图中的黄线，白线，路沿均以此来描述，用lid来唯一标志，bpid和fpid指的是连线的两个端点Point的id，blid和flid表示线段之间的关联关系，分别是上一条和下一条线段Line的lid，若blid=0的话需要设定起点，flid是其关联的下一条线的id。两个Point的连线即为Line。
在这里插入图片描述

（5）DTLane.msg

位于src/autoware/messages/vector_map_msgs/msg/DTLane.msg。DTLane为道路或车道中心线添加了线性元素和纵向横向坡度的数据，承担着中心线性数据的作用，并用did唯一编号。

1
2
3
4
5
6
7
8
9
10
11
# Ver 1.00
int32 did
float64 dist
int32 pid
float64 dir
float64 apara
float64 r
float64 slope
float64 cant
float64 lw
float64 rw

1.2 update_values

update_values函数被上面cache_point，cache_lane和cache_node这三个回调函数调用，用以在接收到更新的points，lanes和nodes数据时更新lane_vmap，当然要保证all_vmap中同时具有points，lanes和nodes数据才可以根据all_vmap去生成新的lane_vmap。接着如果接收到了路径规划的数据（cached_route.point存在数据），则根据cached_route创建导航轨迹点。随后清除已被使用的cached_route.point数据。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
void update_values()
{
    if (all_vmap.points.empty() || all_vmap.lanes.empty() || all_vmap.nodes.empty())
        return;

    //lane_vmap的定义是lane_planner::vmap::VectorMap lane_vmap;，跟all_vmap一样的数据类型。
    //lane_planner::vmap::LNO_ALL = -1
    lane_vmap = lane_planner::vmap::create_lane_vmap(all_vmap, lane_planner::vmap::LNO_ALL);

    if (!cached_route.point.empty()) {//tablet_socket_msgs::route_cmd cached_route;
        create_waypoint(cached_route);
        cached_route.point.clear();
        cached_route.point.shrink_to_fit();//调用shrink_to_fit来要求vector将超出当前大小的多余内存退回给系统。
        //然而这只是一个请求，标准库并不保证退还内存。
    }
}

lane_planner::vmap::LNO_ALL定义在install/lane_planner/include/lane_planner/lane_planner_vmap.hpp，其余还有LNO_CROSSING等。
在这里插入图片描述

create_lane_vmap函数

create_lane_vmap函数位于src/autoware/core_planning/lane_planner/lib/lane_planner/lane_planner_vmap.cpp。create_lane_vmap函数将传入的all_vmap中的信息复制至lane_vmap。

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
VectorMap create_lane_vmap(const VectorMap& vmap, int lno)
{
    VectorMap lane_vmap;
    for (const vector_map::Lane& l : vmap.lanes) {
        if (lno != LNO_ALL && l.lno != lno)
            continue;
        lane_vmap.lanes.push_back(l);//将vmap.lanes中的vector_map::Lane类型元素复制到lane_vmap.lanes

        for (const vector_map::Node& n : vmap.nodes) {
            if (n.nid != l.bnid && n.nid != l.fnid)
                continue;
            lane_vmap.nodes.push_back(n);

            for (const vector_map::Point& p : vmap.points) {
                if (p.pid != n.pid)
                    continue;
                lane_vmap.points.push_back(p);
            }
        }

        for (const vector_map::StopLine& s : vmap.stoplines) {
            if (s.linkid != l.lnid)
                continue;
            lane_vmap.stoplines.push_back(s);
        }

        for (const vector_map::DTLane& d : vmap.dtlanes) {
            if (d.did != l.did)
                continue;
            lane_vmap.dtlanes.push_back(d);
        }
    }

    return lane_vmap;
}

2. create_waypoint函数

create_waypoint函数是对应于话题"/route_cmd"的回调函数，同时也被update_values函数在满足!cached_route.point.empty()==true的情况下调用。cached_route的定义是tablet_socket_msgs::route_cmd cached_route;。

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
void create_waypoint(const tablet_socket_msgs::route_cmd& msg)
{
    std_msgs::Header header;
    header.stamp = ros::Time::now();
    header.frame_id = frame_id;

    if (all_vmap.points.empty() || all_vmap.lanes.empty() || all_vmap.nodes.empty()) {
        cached_route.header = header;
        cached_route.point = msg.point;
        return;
    }

    lane_planner::vmap::VectorMap coarse_vmap = lane_planner::vmap::create_coarse_vmap_from_route(msg);
    //coarse_vmap是根据传入create_coarse_vmap_from_route函数的route_cmd msg构建而得的，是只包含无人车预计要经过的Point数据的粗略地图。（I'm ZhengKX）
    //其被当作一个粗略的导航用以在信息更完备的针对整个路网的矢量地图lane_vmap中找到对应的路径点Point和道路Lane构建更完善的导航地图。
    if (coarse_vmap.points.size() < 2)
        return;

    std::vector<lane_planner::vmap::VectorMap> fine_vmaps;
    lane_planner::vmap::VectorMap fine_mostleft_vmap =
        lane_planner::vmap::create_fine_vmap(lane_vmap, lane_planner::vmap::LNO_MOSTLEFT, coarse_vmap,
                             search_radius, waypoint_max);
    //根据只有路径点的导航地图coarse_vmap，从整个路网地图lane_vmap中搜寻信息，构建信息完备的导航地图fine_mostleft_vmap（包括路径点，道路，道路限速曲率等信息）。
    if (fine_mostleft_vmap.points.size() < 2)
        return;
    fine_vmaps.push_back(fine_mostleft_vmap);

    int lcnt = count_lane(fine_mostleft_vmap);//count_lane函数查找并返回传入函数的矢量地图fine_mostleft_vmap中的最大车道数。
    //下面的for循环补充完善fine_vmaps，往里面添加以其他车道编号为基准寻找后续Lane的导航地图。
    for (int i = lane_planner::vmap::LNO_MOSTLEFT + 1; i <= lcnt; ++i) {
        lane_planner::vmap::VectorMap v =
            lane_planner::vmap::create_fine_vmap(lane_vmap, i, coarse_vmap, search_radius, waypoint_max);//（防盗标记：zhengkunxian）
        if (v.points.size() < 2)
            continue;
        fine_vmaps.push_back(v);
    }

    //下面从fine_vmaps中读取信息构建LaneArray lane_waypoint
    autoware_msgs::LaneArray lane_waypoint;
    for (const lane_planner::vmap::VectorMap& v : fine_vmaps) {
        autoware_msgs::Lane l;
        l.header = header;
        l.increment = 1;

        size_t last_index = v.points.size() - 1;
        for (size_t i = 0; i < v.points.size(); ++i) {
            double yaw;
            if (i == last_index) {
                geometry_msgs::Point p1 =
                    lane_planner::vmap::create_geometry_msgs_point(v.points[i]);
                geometry_msgs::Point p2 =
                    lane_planner::vmap::create_geometry_msgs_point(v.points[i - 1]);
                yaw = atan2(p2.y - p1.y, p2.x - p1.x);
                yaw -= M_PI;
            } else {
                geometry_msgs::Point p1 =
                    lane_planner::vmap::create_geometry_msgs_point(v.points[i]);
                    //create_geometry_msgs_point函数根据Point数据构建geometry_msgs::Point消息。
                geometry_msgs::Point p2 =
                    lane_planner::vmap::create_geometry_msgs_point(v.points[i + 1]);
                yaw = atan2(p2.y - p1.y, p2.x - p1.x);
            }

            autoware_msgs::Waypoint w;
            w.pose.header = header;
            w.pose.pose.position = lane_planner::vmap::create_geometry_msgs_point(v.points[i]);
            w.pose.pose.orientation = tf::createQuaternionMsgFromYaw(yaw);
            w.twist.header = header;
            w.twist.twist.linear.x = velocity / 3.6; // velocity默认为40km/h，这里将其转换为以m/s为单位。
            l.waypoints.push_back(w);
        }
        lane_waypoint.lanes.push_back(l);
    }
    waypoint_pub.publish(lane_waypoint);//在话题"/lane_waypoints_array"发布lane_waypoint

    for (size_t i = 0; i < fine_vmaps.size(); ++i) {
        std::stringstream ss;
        ss << "_" << i;

        std::vector<std::string> v1 = split(output_file, '/');
        std::vector<std::string> v2 = split(v1.back(), '.');
        v2[0] = v2.front() + ss.str();
        v1[v1.size() - 1] = join(v2, '.');
        std::string path = join(v1, '/');

        lane_planner::vmap::write_waypoints(fine_vmaps[i].points, velocity, path);
    }
}

博主对下面的代码作用进行验证：
可知下面代码块用于在output_file的后面加上后缀，以备后面将fine_vmaps中不同的导航地图中的路径点存在不同的文件内。

1
2
3
4
5
6
7
8
std::stringstream ss;
ss << "_" << i;

std::vector<std::string> v1 = split(output_file, '/');
std::vector<std::string> v2 = split(v1.back(), '.');
v2[0] = v2.front() + ss.str();
v1[v1.size() - 1] = join(v2, '.');
std::string path = join(v1, '/');

在这里插入图片描述

2.1 route_cmd.msg

位于src/autoware/messages/tablet_socket_msgs/msg/route_cmd.msg，包含两个成员变量，header包含一些基本信息，point是路径点集合。

1
2
Header header
Waypoint[] point

2.2 create_coarse_vmap_from_route函数

create_coarse_vmap_from_route根据传入的tablet_socket_msgs::route_cmd数据构建只包含无人车预计要经过的Point数据的粗略地图coarse_vmap，它跟lane_vmap和all_vmap是一样的数据类型，都是lane_planner::vmap::VectorMap。

coarse_vmap被当作一个粗略的行驶导航用以在信息更完备的针对整个路网的矢量地图lane_vmap中找到对应的路径点Point和道路Lane构建更完善的导航地图。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
VectorMap create_coarse_vmap_from_route(const tablet_socket_msgs::route_cmd& route)
{
    geo_pos_conv geo;
    geo.set_plane(7);//设置UWB坐标原点，Autoware内置了19个经纬度坐标作为UWB坐标系的原点。
    VectorMap coarse_vmap;
    for (const tablet_socket_msgs::Waypoint& w : route.point) {
        geo.llh_to_xyz(w.lat, w.lon, 0);//将经纬度和高度转换为UWB坐标系下的xyz坐标

        vector_map::Point p;
        p.bx = geo.x();
        p.ly = geo.y();
        coarse_vmap.points.push_back(p);
    }

    return coarse_vmap;
}

geo_pos_conv类

geo_pos_conv类定义在src/autoware/common/gnss/include/gnss/geo_pos_conv.hpp，用于将经纬度转换为UWB坐标，内置的UWB坐标原点有多个，都设置在小日本的小岛上面，如果要用到咱们中国的话需要自行重新定义原点，并且相关的参数也要更改，比如地球半径等在不同经度是不同的。

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
class geo_pos_conv {
private:
    double m_x;  //m
    double m_y;  //m
    double m_z;  //m

    double m_lat;  //latitude
    double m_lon; //longitude
    double m_h;

    double m_PLato;        //plane lat
    double m_PLo;          //plane lon

public:
    geo_pos_conv();
    double x() const;
    double y() const;
    double z() const;

    void set_plane(double lat,   double lon);
    void set_plane(int num);
    void set_xyz(double cx,   double cy,   double cz);

    //set llh in radians
    void set_llh(double lat, double lon, double h);

    //set llh in nmea degrees
    void set_llh_nmea_degrees(double latd,double lond, double h);

        void llh_to_xyz(double lat, double lon, double ele);

    void conv_llh2xyz(void);
    void conv_xyz2llh(void);
};

2.3 create_fine_vmap函数

create_waypoint函数中对create_fine_vmap函数的调用是lane_planner::vmap::VectorMap fine_mostleft_vmap = lane_planner::vmap::create_fine_vmap(lane_vmap, lane_planner::vmap::LNO_MOSTLEFT, coarse_vmap, search_radius, waypoint_max);，create_fine_vmap函数的作用是根据只有路径点的粗略导航地图coarse_vmap，从整个路网地图lane_vmap中搜寻信息，构建信息完备的导航地图fine_mostleft_vmap（包括路径点，道路，道路限速曲率等信息）。由调用可知传入create_fine_vmap函数的lno是lane_planner::vmap::LNO_MOSTLEFT=1。但是我们的分析仍然是不管条件跳转，对所有步骤都进行解析。

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
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
VectorMap create_fine_vmap(const VectorMap& lane_vmap, int lno, const VectorMap& coarse_vmap, double search_radius,
               int waypoint_max)
{
    VectorMap fine_vmap;
    VectorMap null_vmap;

    vector_map::Point departure_point;
    departure_point.pid = -1;
    if (lno == LNO_ALL)
        departure_point = find_nearest_point(lane_vmap, coarse_vmap.points.front());
        //departure_point为lane_vmap中与coarse_vmap.points.front()距离最近的坐标点
    else {
        for (int i = lno; i >= LNO_CROSSING; --i) {//LNO_CROSSING = 0
            departure_point = find_departure_point(lane_vmap, i, coarse_vmap.points, search_radius);//i=1；i=0
            //这里的“出发点”指的是lane_vmap中满足a + d最小的Point x：
            //（1）a：Point x对应的Node作为起始点的Lane l（且编号等于传入的i）与直线<coarse_vmap.points[0],coarse_vmap.points[1]>之间的夹角;
            //（2）d：Point x与coarse_vmap.points[0]的距离。
            if (departure_point.pid >= 0)//此时代表找到出发点，因为原始departure_point的pid是-1，
            //而此时departure_point.pid发生改变。
                break;
        }
    }
    if (departure_point.pid < 0)
        return null_vmap;

    vector_map::Point arrival_point;
    arrival_point.pid = -1;
    if (lno == LNO_ALL)
        arrival_point = find_nearest_point(lane_vmap, coarse_vmap.points.back());
        //arrival_point为lane_vmap中与coarse_vmap.points.back()距离最近的坐标点
    else {
        for (int i = lno; i >= LNO_CROSSING; --i) {
            arrival_point = find_arrival_point(lane_vmap, i, coarse_vmap.points, search_radius);//i=1;i=0（防盗标记：zhengkunxian）
            //这里的“到达点”（设为Point y）指的是lane_vmap中满足a + d最小的Point：
            //（1）a：直线<z,y>（Point z：Point y所对应的Node作为末尾点的Lane l（且编号等于传入的i）的上一条Lane（设为Lane k）的起始点）与直线<coarse_vmap.points[coarse_vmap.points.size() - 1],coarse_vmap.points[coarse_vmap.points.size() - 2]>之间的夹角；（mark：I'm ZhengKX）
            //（2）d：Point y与coarse_vmap.points[coarse_vmap.points.size() - 1]的距离。
            if (arrival_point.pid >= 0)
                break;
        }
    }
    if (arrival_point.pid < 0)//多一道保险
        return null_vmap;

    vector_map::Point point = departure_point;
    vector_map::Lane lane = find_lane(lane_vmap, LNO_ALL, point);
    //find_lane(const VectorMap& vmap, int lno, const vector_map::Point& point)：
    //在传入find_lane函数的矢量地图vmap中找到车道编号为lno(当lno不等于LNO_ALL=-1时)且起始Node为point所对应的Node的Lane。（防盗标记：zhengkunxian）
    //因为这里传入的lno=LNO_ALL，因此找到的Lane只需要满足起始Node为传入函数的point所对应的Node。
    //传入的point为出发点departure_point，因此得到的lane为车辆出发时所在的Lane。
    if (lane.lnid < 0)
        return null_vmap;

    bool finish = false;
   
    //开始构造fine_vmap
    for (int i = 0; i < waypoint_max; ++i) {
        fine_vmap.points.push_back(point);
        //循环开始时的point为上面求得的departure_point；
       
        // 查找lane对应的DTLane
        vector_map::DTLane dtlane;
        dtlane.did = -1;
        for (const vector_map::DTLane& d : lane_vmap.dtlanes) {
            if (d.did == lane.did) {
            //循环开始时的lane为上面求得的departure_point对应的Node作为起始点的Lane，也就是车辆出发时所在的Lane。
                dtlane = d;
                break;
            }
        }
        //fine_vmap内信息：（1）vector_map::DTLane：储存对应Lane的道路或车道中心线纵向横向坡度等数据。
        fine_vmap.dtlanes.push_back(dtlane);
       
        // 查找lane对应的StopLine
        vector_map::StopLine stopline;
        stopline.id = -1;
        for (const vector_map::StopLine& s : lane_vmap.stoplines) {
            if (s.linkid == lane.lnid) {
            //同样的，循环开始时的lane为上面求得的departure_point对应的Node作为起始点的Lane，也就是车辆出发时所在的Lane。
                stopline = s;
                break;
            }
        }
        //fine_vmap内信息：（2）vector_map::StopLine：存储人行横道,交叉路口前的停止线信息。
        fine_vmap.stoplines.push_back(stopline);

        if (finish)//此处是跳出fine_vmap构造大循环的开关
            break;

        //fine_vmap内信息：（3）vector_map::Line：车辆行驶的车道，包含了限速，车道类型（左转右转）等信息。
        fine_vmap.lanes.push_back(lane);

        point = find_end_point(lane_vmap, lane);//在传入函数的矢量地图lane_vmap中找到lane的末尾Node所对应的Point。
        //此处实现了point在同一条Lane上的起始点和末尾点之间的切换，后面就会以当前lane的末尾点作为后面Lane的起始点寻找对应的Lane,
        //然后回到循环开始处根据这个新的Lane查找对应的信息存入fine_vmap。
       
        if (point.pid < 0)
            return null_vmap;
        if (point.bx == arrival_point.bx && point.ly == arrival_point.ly) {//到达“到达点”，停止循环
            finish = true;
            continue;
        }

        if (is_branching_lane(lane)) {//判断传入函数的lane是否会分成多个Lane分支
            vector_map::Point coarse_p1 = find_end_point(lane_vmap, lane);//在传入函数的矢量地图lane_vmap中找到lane的末尾Node所对应的Point。
            if (coarse_p1.pid < 0)
                return null_vmap;

            coarse_p1 = find_nearest_point(coarse_vmap, coarse_p1);//查找并返回传入函数的矢量地图coarse_vmap中与lane_vmap中的coarse_p1距离最近的坐标点，并替代coarse_p1。
            if (coarse_p1.pid < 0)
                return null_vmap;

            vector_map::Point coarse_p2;
            double distance = -1;
            for (const vector_map::Point& p : coarse_vmap.points) {
                if (distance == -1) {//一直轮转直到coarse_p1将distance赋值为0，下一次循环便不会执行continue跳过此if结构后面的操作。（防盗标记：zhengkunxian）
                    if (p.bx == coarse_p1.bx && p.ly == coarse_p1.ly)
                        distance = 0;
                    continue;
                }
                coarse_p2 = p;
                distance = hypot(coarse_p2.bx - coarse_p1.bx, coarse_p2.ly - coarse_p1.ly);
                if (distance > search_radius)//找到在搜索范围边缘处的Point
                    break;
            }
            if (distance <= 0)
                return null_vmap;

            double coarse_angle = compute_direction_angle(coarse_p1, coarse_p2);
            //得到coarse_p1和coarse_p2构成的直线与世界坐标系x轴的夹角（-180～180度）。

            //find_next_branching_lane的作用：
            //前提：在粗略地图coarse_vmap中确定coarse_p1（与矢量地图lane_vmap中当前Lane的末尾节点位置最接近的Point）（I'm ZhengKX）
            //和coarse_p2（与coarse_p1之间的距离刚刚超过搜索范围search_radius），
            //继而得到这两个点与世界坐标系x轴的夹角coarse_angle。
           
            //函数流程：在精度更高的矢量地图lane_vmap中寻找当前Lane（设为l）的所有分支Lane，
            //以不同的分支Lane作为起点分别遍历它们的后续Lane，直到Lane
            //（1）开始分支，（2）走到头，（3）末尾节点与Lane l的末尾节点（设为x）距离刚刚超过search_radius，（I'm ZhengKX）
            //记录此时被遍历到的Lane的末尾节点。
            //计算所有被记录的末尾节点与Point x形成的直线与世界坐标系x轴的夹角，
            //选择最接近coarse_angle的对应末尾节点所处的分支Lane作为当前Lane l的后续Lane。
           
            //通俗而言，find_next_branching_lane函数根据粗略地图计算出来的夹角作为一个粗略的标准在更高精度的矢量地图中以更完善的标准寻找下一条Lane。
            if (lno == LNO_ALL) {
                lane = find_next_branching_lane(lane_vmap, LNO_ALL, lane, coarse_angle, search_radius);
            } else {
                vector_map::Lane l;
                l.lnid = -1;
                for (int j = lno; j >= LNO_CROSSING; --j) {
                    l = find_next_branching_lane(lane_vmap, j, lane, coarse_angle, search_radius);
                    if (l.lnid >= 0)
                        break;
                }
                lane = l;
            }
        } else {//如果当前lane没有分支，则直接调用find_next_lane函数找后续Lane
            lane = find_next_lane(lane_vmap, LNO_ALL, lane);
        }
        if (lane.lnid < 0)
            return null_vmap;
    }

    if (!finish) {
        ROS_ERROR_STREAM("lane is too long");
        return null_vmap;
    }

    return fine_vmap;
}

create_fine_vmap函数的主体执行流程图如下所示：
在这里插入图片描述

2.3.1 find_nearest_point函数

查找并返回传入函数的矢量地图vmap中与point距离最近的坐标点，若返回的vector_map::Point的pid<0则代表未找到最近点。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
vector_map::Point find_nearest_point(const VectorMap& vmap, const vector_map::Point& point)
{
    vector_map::Point nearest_point;
    nearest_point.pid = -1;//若返回的nearest_point.pid<0则代表未找到最近点

    double distance = DBL_MAX;
    for (const vector_map::Point& p : vmap.points) {
        double d = hypot(p.bx - point.bx, p.ly - point.ly);//计算两点之间的距离
        if (d <= distance) {
            nearest_point = p;
            distance = d;
        }
    }

    return nearest_point;
}

2.3.2 find_departure_point函数

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
vector_map::Point find_departure_point(const VectorMap& lane_vmap, int lno,
                       const std::vector<vector_map::Point>& coarse_points,
                       double search_radius)
{
    vector_map::Point coarse_p1 = coarse_points[0];
    vector_map::Point coarse_p2 = coarse_points[1];

    vector_map::Point nearest_point = find_nearest_point(lane_vmap, coarse_p1);
    //找到lane_vmap中距离coarse_p1最近的Point，作为保底返回结果
    if (nearest_point.pid < 0)
        return nearest_point;

    std::vector<vector_map::Point> near_points = find_near_points(lane_vmap, coarse_p1, search_radius);
    //find_near_points函数与find_nearest_point函数相较就是将条件判断从距离最小改为距离在某一范围以内
    //找到lane_vmap中与coarse_p1的距离在search_radius范围内的一群Point
    double coarse_angle = compute_direction_angle(coarse_p1, coarse_p2);
    //计算coarse_p2和coarse_p1构成的直线与当前世界坐标系下x轴正方向间的夹角（180~-180度）
    double score = 180 + search_radius;
    for (const vector_map::Point& p1 : near_points) {
        vector_map::Lane l = find_lane(lane_vmap, lno, p1);
        //find_lane的作用是在传入函数的矢量地图lane_vmap中找到车道编号为lno(当lno不等于LNO_ALL=-1时)且起始Node为Point p1所对应的Node的Lane。
        if (l.lnid < 0)//这种判断是否成功完成Lane寻找目标的方法跟上面哪些函数都是一样的，后面不再继续赘述。
            continue;

        vector_map::Point p2 = find_end_point(lane_vmap, l);//在传入函数的矢量地图lane_vmap中找到l的（mark：I'm ZhengKX）
        //末尾Node所对应的Point。
        if (p2.pid < 0)
            continue;

        double a = compute_direction_angle(p1, p2);//p1和p2对应的Node构成了Lane l，因此这里计算的是Lane l与世界坐标系x轴之间的夹角（防盗标记：zhengkunxian）
        //因为p1是near_points中的Point，也就是Lane l是与coarse_p1的距离在search_radius范围内的其他Point对应的Node作为起始点的Lane
        a = fabs(a - coarse_angle);//Lane l与直线<coarse_p1,coarse_p2>之间的夹角
        //要注意的是夹角a是lane_vmap中的点所构成的Lane与x轴的夹角
        //coarse_angle是coarse_points（对应于传入find_departure_point函数中的create_fine_vmap函数中的coarse_vmap.points）中头两个点构成的Lane与x轴的夹角
        if (a > 180)
            a = fabs(a - 360);
        double d = hypot(p1.bx - coarse_p1.bx, p1.ly - coarse_p1.ly);
        double s = a + d;
        if (s <= score) {//前面的score = 180 + search_radius;中的180代表最大角度，search_radius则是最大距离
        //也就是在lane_vmap中找到一个Point满足a + d最小：
        //（1）a：其对应的Node作为起始点的Lane（且编号等于传入的lno）与直线<coarse_p1,coarse_p2>之间的夹角;
        //（2）d：与coarse_p1的距离。
            nearest_point = p1;
            score = s;
        }
    }

    return nearest_point;
}

函数名的意思是“寻找出发点”，这里的“出发点”指的是lane_vmap中满足a + d最小的Point

xx

x：
（1）a：Point

xx

x对应的Node作为起始点的Lane

ll

l（且编号等于传入的lno）与直线之间的夹角；
（2）d：Point

xx

x与coarse_points[0]的距离。
如下图所示：
在这里插入图片描述

（1）find_near_points函数

寻找并返回传入函数的vmap中与point的距离在search_radius以内的所有路径点。

1
2
3
4
5
6
7
8
9
10
11
12
std::vector<vector_map::Point> find_near_points(const VectorMap& vmap, const vector_map::Point& point,
                        double search_radius)
{
    std::vector<vector_map::Point> near_points;
    for (const vector_map::Point& p : vmap.points) {
        double d = hypot(p.bx - point.bx, p.ly - point.ly);
        if (d <= search_radius)//距离在此范围内的一批路径点
            near_points.push_back(p);
    }

    return near_points;
}

（2）compute_direction_angle函数

原理很简单，根据p2和p1坐标计算出正切值，接着求反正切得到p2和p1构成的直线与世界坐标系x轴的夹角

θ\theta

θ (

π\pi

π~

?π-\pi

?π），最后转换为“度”为单位。

θ=arctan(p2y?p1yp2x?p1x)×180π
\theta= arctan(\frac {p2_y-p1_y} {p2_x-p1_x})\times\frac {180} {\pi}

θ=arctan(p2x??p1x?p2y??p1y??)×π180?

1
2
3
4
double compute_direction_angle(const vector_map::Point& p1, const vector_map::Point& p2)
{
    return (atan2(p2.ly - p1.ly, p2.bx - p1.bx) * (180 / M_PI)); // -180～180 度
}

（3）find_lane函数

find_lane的作用是在传入函数的矢量地图vmap中找到车道编号为lno (当lno不等于LNO_ALL=-1时) 且起始Node为point所对应的Node的Lane。
find_lane函数的解析需要对Autoware矢量地图的数据组织形式有一定的了解，（防盗标记：zhengkunxian）可以看前面的1.1 lane_planner::vmap::VectorMap中对VectorMap中所包含的vector_map::Point，vector_map::Lane，vector_map::Node，vector_map::StopLine和vector_map::DTLane的解析，大致了解它们之间的关系，可以辅助理解find_lane函数的实现过程。
传入find_lane函数的lno=1或者0。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
vector_map::Lane find_lane(const VectorMap& vmap, int lno, const vector_map::Point& point)
{
    vector_map::Lane error;
    error.lnid = -1;

    for (const vector_map::Node& n : vmap.nodes) {
        if (n.pid != point.pid)//找到传入函数的point所对应的Node
            continue;
        for (const vector_map::Lane& l : vmap.lanes) {
            if (lno != LNO_ALL && l.lno != lno)//在传入函数的vmap中找到车道编号和lno相符的Lane（mark：I'm ZhengKX）
                continue;
            if (l.bnid != n.nid)//找到传入函数的point所对应的Node作为起始Node的Lane
                continue;
            return l;
        }
    }

    return error;
}

（4）find_end_point函数

find_end_point的作用是在传入函数的矢量地图vmap中找到lane的末尾Node所对应的Point。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
vector_map::Point find_end_point(const VectorMap& vmap, const vector_map::Lane& lane)
{
    vector_map::Point error;
    error.pid = -1;

    for (const vector_map::Node& n : vmap.nodes) {
        if (n.nid != lane.fnid)//在传入函数的vmap中找到传入函数的lane的末尾Node所对应的Node
            continue;
        for (const vector_map::Point& p : vmap.points) {
            if (p.pid != n.pid)//在传入函数的vmap中找到对应于上面找到的Node的Point
                continue;
            return p;
        }
    }

    return error;
}

2.3.3 find_arrival_point函数

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
vector_map::Point find_arrival_point(const VectorMap& lane_vmap, int lno,
                     const std::vector<vector_map::Point>& coarse_points,
                     double search_radius)
{
    vector_map::Point coarse_p1 = coarse_points[coarse_points.size() - 1];//coarse_points中的最后一个Point（防盗标记：zhengkunxian）
    vector_map::Point coarse_p2 = coarse_points[coarse_points.size() - 2];

    vector_map::Point nearest_point = find_nearest_point(lane_vmap, coarse_p1);
    //跟find_departure_point函数中是类似的，先找到lane_vmap中与coarse_p1距离最近的Point，作为保底返回结果
    if (nearest_point.pid < 0)
        return nearest_point;

    std::vector<vector_map::Point> near_points = find_near_points(lane_vmap, coarse_p1, search_radius);
    double coarse_angle = compute_direction_angle(coarse_p1, coarse_p2);
    double score = 180 + search_radius;
    for (const vector_map::Point& p1 : near_points) {
        vector_map::Lane l = find_lane(lane_vmap, lno, p1);//前面已经分析find_lane函数的作用
        if (l.lnid < 0)
            continue;

        l = find_prev_lane(lane_vmap, lno, l);//在lane_vmap中找到Lane l的上一条Lane（车道编号和lno相符）
        if (l.lnid < 0)
            continue;

        vector_map::Point p2 = find_start_point(lane_vmap, l);//与前面的find_end_point函数作用相反，
        //在传入函数的矢量地图lane_vmap中找到l的起始Node所对应的Point。
        if (p2.pid < 0)
            continue;

        double a = compute_direction_angle(p1, p2);//p1(某一Lane x的末尾Point)和p2(Lane x的上一条Lane y的起始Point)组成的直线与世界坐标系x轴之间的夹角（防盗标记：zhengkunxian）
        a = fabs(a - coarse_angle);
        if (a > 180)
            a = fabs(a - 360);
        double d = hypot(p1.bx - coarse_p1.bx, p1.ly - coarse_p1.ly);
        double s = a + d;
        if (s <= score) {
        //在lane_vmap中找到一个Point p1满足a + d最小：
        //（1）a：直线<p1,p2>（p2：p1所对应的Node作为末尾点的Lane（且编号等于传入的lno）的上一条Lane的起始点）与直线<coarse_p1,coarse_p2>之间的夹角;
        //（2）d：与coarse_p1的距离。
            nearest_point = p1;
            score = s;
        }
    }

    return nearest_point;
}

函数名的意思是“寻找到达点”，跟前面的“寻找出发点”实现过程是相似的。这里的“到达点”（设为Point

yy

y）指的是lane_vmap中满足a + d最小的Point：
（1）a：直线

<z,y>

（Point

zz

z：Point

yy

y所对应的Node作为末尾点的Lane

ll

l（且编号等于传入的lno）的上一条Lane（即Lane

kk

k）的起始点）与直线之间的夹角；
（2）d：Point

yy

y与coarse_points[coarse_points.size() - 1]的距离。
如下图所示：
在这里插入图片描述

（1）find_prev_lane函数

find_prev_lane函数的作用是找到传入函数的Lane lane的跟它相连的上一条Lane。
从1.1 lane_planner::vmap::VectorMap函数中对Lane.msg的分析可知：
（1）blid2-4： 指定要合并到当前车道的Lane ID。(0表示没有合并lane)；
（2）flid2-4： 指定要分流出去的车道Lane ID。(0表示车道没有分支)；

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
vector_map::Lane find_prev_lane(const VectorMap& vmap, int lno, const vector_map::Lane& lane)
{
    vector_map::Lane error;
    error.lnid = -1;

    if (is_merging_lane(lane)) {//判断lane是否处于合并模式
        for (const vector_map::Lane& l : vmap.lanes) {
            if (lno != LNO_ALL && l.lno != lno)//在传入函数的vmap中找到车道编号和lno相符的Lane l
                continue;
            if (l.lnid != lane.blid && l.lnid != lane.blid2 && l.lnid != lane.blid3 &&
                l.lnid != lane.blid4)//l是否是合并到Lane lane的上一条Lane
                continue;
            return l;
        }
    } else {
        for (const vector_map::Lane& l : vmap.lanes) {
            if (l.lnid != lane.blid)//lane不是多个Lane合并而来的时候，l是否是lane的上一条Lane
                continue;
            return l;
        }
    }

    return error;
}

is_merging_lane函数

is_merging_lane函数的作用是判断传入函数的Lane lane是否是多个Lane合并而来的，实现过程很简单，只要查询vector_map中记录的数据即可。
从1.1 lane_planner::vmap::VectorMap函数中对Lane.msg的分析可知：
jct： 指定当前车道分支/合并模式，（旧版本：0：正常，1：分支到左边，2：分支到右边，3：合并到左车道，4：合并到右车道 ），根据下面的代码可知当前Autoware V12.0版本中对于Lane.jct有了新的定义，3，4，5都代表道路合并，结合后面分析的is_branching_lane函数我们认为jct=5代表未知。

1
2
3
4
bool is_merging_lane(const vector_map::Lane& lane)
{
    return (lane.jct == 3 || lane.jct == 4 || lane.jct == 5);
}

（2）find_start_point函数

find_start_point的作用是在传入函数的矢量地图vmap中找到lane的起始Node所对应的Point。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
vector_map::Point find_start_point(const VectorMap& vmap, const vector_map::Lane& lane)
{
    vector_map::Point error;
    error.pid = -1;

    for (const vector_map::Node& n : vmap.nodes) {
        if (n.nid != lane.bnid)//在传入函数的vmap中找到传入函数的lane的起始Node所对应的Node
            continue;
        for (const vector_map::Point& p : vmap.points) {
            if (p.pid != n.pid)//在传入函数的vmap中找到对应于上面找到的Node的Point
                continue;
            return p;
        }
    }

    return error;
}

2.3.4 is_branching_lane函数

is_branching_lane函数的作用是判断传入函数的Lane lane是否会分成多个Lane分支，实现过程跟is_merging_lane函数一样，都只要查询vector_map中记录的数据即可。

1
2
3
4
bool is_branching_lane(const vector_map::Lane& lane)
{
    return (lane.jct == 1 || lane.jct == 2 || lane.jct == 5);
}

2.3.5 find_next_branching_lane函数

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
vector_map::Lane find_next_branching_lane(const VectorMap& vmap, int lno, const vector_map::Lane& lane,
                      double coarse_angle, double search_radius)
{
    vector_map::Lane error;
    error.lnid = -1;

    vector_map::Point p1 = find_end_point(vmap, lane);//在vmap中找到lane的末尾Node对应的Point p1。
    if (p1.pid < 0)
        return error;

    std::vector<std::tuple<vector_map::Point, vector_map::Lane>> candidates;
    for (const vector_map::Lane& l1 : vmap.lanes) {
        if (lno != LNO_ALL && l1.lno != lno)//当lno=LNO_ALL或者l1.lno=lno的时候不执行continue，也就是执行后面的代码。
            continue;
        if (l1.lnid == lane.flid || l1.lnid == lane.flid2 || l1.lnid == lane.flid3 || l1.lnid == lane.flid4) {
        //从1.1 lane_planner::vmap::VectorMap函数中对Lane.msg的分析可知：
        //flid2-4：指定要分流出去的车道Lane ID。(0表示车道没有分支)。
        //这里if结构判断l1是否是传入函数的Lane lane的分支Lane。
            vector_map::Lane l2 = l1;
            vector_map::Point p = find_end_point(vmap, l2);//在vmap中找到Lane l2的末尾Node对应的Point p。
            if (p.pid < 0)
                continue;
            vector_map::Point p2 = p;
            double d = hypot(p2.bx - p1.bx, p2.ly - p1.ly);//计算Lane l2的起始点和末尾点之间的距离，也就是Lane l2的路长。（I'm ZhengKX）
            while (d <= search_radius && l2.flid != 0 && !is_branching_lane(l2)) {
            //一直循环直到找到距离传入函数中的Lane lane的末尾点刚刚超出搜索半径search_radius的Point
            //注意：此Point是lane的后续Lane的末尾点。
            //或者迭代后的l2的后续Lane不存在（l2.flid = 0），或者迭代后的l2存在分支。
            //此处while循环终止的条件有三个，但目的条件其实是d <= search_radius，另外两个条件用于问题避免，
            //因此最终得到的candidates还需要对其中的元素进一步筛选。
                l2 = find_next_lane(vmap, LNO_ALL, l2);//不断用l2的下一条Lane迭代l2，往路网深处遍历
                if (l2.lnid < 0)
                    break;
                p = find_end_point(vmap, l2);
                if (p.pid < 0)
                    break;
                p2 = p;//不断迭代p2
                d = hypot(p2.bx - p1.bx, p2.ly - p1.ly);
            }
            candidates.push_back(std::make_tuple(p2, l1));
            //由判断条件可知：
            //l1：为传入函数中的Lane lane的后续Lane（flid，flid2，flid3，flid4）；
            //p2：有三种情况：
            //（1）为距离l1的起始点（lane的末尾点）的距离刚刚大于search_radius的Point；
            //（2）距离l1的起始点（lane的末尾点）的距离小于search_radius的Point，但所处的Lane没有后续Lane；（I'm ZhengKX）
            //（3）距离l1的起始点（lane的末尾点）的距离小于search_radius的Point，但所处的Lane存在分支。
        }
    }

    if (candidates.empty())
        return error;

    vector_map::Lane branching_lane;
    double angle = 180;
    for (const std::tuple<vector_map::Point, vector_map::Lane>& c : candidates) {
        vector_map::Point p2 = std::get<0>(c);
        double a = compute_direction_angle(p1, p2);
        a = fabs(a - coarse_angle);//随着循环令compute_direction_angle(p1, p2)不断逼近coarse_angle
        if (a > 180)
            a = fabs(a - 360);
        if (a <= angle) {
            branching_lane = std::get<1>(c);
            angle = a;
        }
    }

    return branching_lane;
}

find_next_branching_lane函数在2.3 create_fine_vmap函数中被调用，通过create_fine_vmap函数我们可以获得一些辅助信息帮助理解函数的作用：

前提：在粗略地图coarse_vmap中确定coarse_p1（与矢量地图lane_vmap中当前Lane的末尾节点位置最接近的Point）和coarse_p2（与coarse_p1之间的距离刚刚超过搜索范围search_radius），继而得到这两个点与世界坐标系x轴的夹角coarse_angle。

函数流程：在精度更高的矢量地图lane_vmap中寻找当前Lane（设为l）的所有分支Lane，以不同的分支Lane作为起点分别遍历它们的后续Lane，直到Lane（1）开始分支，（2）走到头，（3）末尾节点与Lane l的末尾节点（设为x）距离刚刚超过search_radius（I’m ZhengKX），记录此时被遍历到的Lane的末尾节点。计算所有被记录的末尾节点与Point x形成的直线与世界坐标系x轴的夹角，选择最接近coarse_angle的对应末尾节点所处的分支Lane作为当前Lane l的后续Lane。

通俗而言，find_next_branching_lane函数根据粗略地图计算出来的夹角作为一个粗略的标准在更高精度的矢量地图中以更完善的标准寻找下一条Lane。

上面描述的find_next_branching_lane函数的流程如下图所示：

在这里插入图片描述

find_next_lane函数

根据传入函数的Lane lane的后续Lane是否有多条（也就是lane是否有分支）返回对应的后续Lane：
（1）有分支：返回道路编号等于传入函数中的lno，且lnid（Lane的唯一标识）为矢量地图中记录的lane的后续Lane的lnid的Lane；
（2）无分支：直接返回lane的后续Lane，不必判断道路编号是否跟lno符合。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
vector_map::Lane find_next_lane(const VectorMap& vmap, int lno, const vector_map::Lane& lane)
{
    vector_map::Lane error;
    error.lnid = -1;

    if (is_branching_lane(lane)) {//判断Lane lane是否有分支。
        for (const vector_map::Lane& l : vmap.lanes) {
            if (lno != LNO_ALL && l.lno != lno)//通用做法了，满足条件才执行后面的代码。
                continue;
            if (l.lnid != lane.flid && l.lnid != lane.flid2 && l.lnid != lane.flid3 &&
                l.lnid != lane.flid4)//Lane l为lane的分支Lane（利用记录在矢量地图中的信息，矢量地图对于决策而言作用重大）。（I'm ZhengKX）
                continue;
            return l;
        }
    } else {
        for (const vector_map::Lane& l : vmap.lanes) {
            if (l.lnid != lane.flid)//若Lane lane没有分支的话，也就是lane仅有一条跟它相连的后续Lane，直接返回这个Lane即可，连判断道路编号都不需要了。
                continue;
            return l;
        }
    }

    return error;
}

2.4 count_lane函数

count_lane函数查找并返回传入函数的矢量地图vmap中的最大车道数。

1
2
3
4
5
6
7
8
9
10
11
int count_lane(const lane_planner::vmap::VectorMap& vmap)
{
    int lcnt = -1;

    for (const vector_map::Lane& l : vmap.lanes) {
        if (l.lcnt > lcnt)//l.lcnt是Lane l的车道数
            lcnt = l.lcnt;
    }

    return lcnt;//返回的是矢量地图vmap中各个Lane中的最大车道数
}

2.5 create_geometry_msgs_point函数

根据Point数据构建geometry_msgs::Point消息。

1
2
3
4
5
6
7
8
9
10
geometry_msgs::Point create_geometry_msgs_point(const vector_map::Point& vp)
{
    // reverse X-Y axis
    geometry_msgs::Point gp;
    gp.x = vp.ly;
    gp.y = vp.bx;
    gp.z = vp.h;

    return gp;
}

2.6 write_waypoints函数

计算航向角yaw，调用write_waypoint函数将路径点数据写入文件path。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
void write_waypoints(const std::vector<vector_map::Point>& points, double velocity, const std::string& path)
{
    if (points.size() < 2)
        return;

    size_t last_index = points.size() - 1;
    for (size_t i = 0; i < points.size(); ++i) {
        double yaw;
        if (i == last_index) {
            geometry_msgs::Point p1 = create_geometry_msgs_point(points[i]);
            geometry_msgs::Point p2 = create_geometry_msgs_point(points[i - 1]);
            yaw = atan2(p2.y - p1.y, p2.x - p1.x);
            yaw -= M_PI;
        } else {
            geometry_msgs::Point p1 = create_geometry_msgs_point(points[i]);
            geometry_msgs::Point p2 = create_geometry_msgs_point(points[i + 1]);
            yaw = atan2(p2.y - p1.y, p2.x - p1.x);
        }

        write_waypoint(points[i], yaw, velocity, path, (i == 0));
    }
}

write_waypoint函数

用于路径点的数据写入。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
void write_waypoint(const vector_map::Point& point, double yaw, double velocity, const std::string& path,
            bool first)
{
    // reverse X-Y axis
    if (first) {
        std::ofstream ofs(path.c_str());
        ofs << std::fixed << point.ly << ","
            << std::fixed << point.bx << ","
            << std::fixed << point.h << ","
            << std::fixed << yaw << std::endl;
    } else {
        std::ofstream ofs(path.c_str(), std::ios_base::app);
        ofs << std::fixed << point.ly << ","
            << std::fixed << point.bx << ","
            << std::fixed << point.h << ","
            << std::fixed << yaw << ","
            << std::fixed << velocity << std::endl;
    }
}

唉，决策里面需要矢量地图的信息是我当初没有预料到的，道行不够，在矢量地图里面各个元素中的变量含义上面卡了挺久，不过我卡了，大家看我的博客就不会被卡了。

博主想到那里就写到哪，不可避免有些疏漏或者谬误之处，但是无伤大雅，给大家做入门参考用是够了的。