2023年7月4日发(作者：)

Autowareplanning模块学习笔记（⼆）：路径规划（4）-节点lane_naviAutoware planning模块学习笔记（⼆）：路径规划（4）- 节点lane_navi写在前⾯：引⽤的时候请注明出处（I’m ZhengKX的博客：/xiaoxiao123jun），尊重他⼈的原创劳动成果，不打击原作者的创作激情，才能更好地促进我国科教进步，谢谢！继续为⼤家讲解Autoware planning模块，前⾯为⼤家分析了“勾选waypoint_loader”后所启动的三个节点waypoint_loader，waypoint_replanner和waypoint_marker_publisher。这篇博客继续分析后续操作。勾选Mission Planning->lane_planner->lane_navi我们依然查找src/autoware/utilities/runtime_manager/scripts/可知勾选lane_navi（防盗标记：zhengkunxian）后运⾏ROS包lane_planner内的lane_。lane_如下：也就是启动lane_navi和waypoint_marker_publisher两个节点，waypoint_marker_publisher这⼀节点在中已经分析完毕，不再赘述了。由lane_planner的可知节点lane_navi的源⽂件是nodes/lane_navi/lane_。节点lane_navi⾸先是其main函数，main函数在src/autoware/core_planning/lane_planner/nodes/lane_navi/lane_中。⾸先设置了⼀些参数，接着设置发布者和监听者。int main(int argc, char **argv){ ros::init(argc, argv, "lane_navi"); ros::NodeHandle n; int sub_vmap_queue_size; ("/lane_navi/sub_vmap_queue_size", sub_vmap_queue_size, 1);//由launch⽂件传⼊参数， //在没有获取到参数值的时候，可以设置默认值。 int sub_route_queue_size; ("/lane_navi/sub_route_queue_size", sub_route_queue_size, 1); int pub_waypoint_queue_size; ("/lane_navi/pub_waypoint_queue_size", pub_waypoint_queue_size, 1); bool pub_waypoint_latch; ("/lane_navi/pub_waypoint_latch", pub_waypoint_latch, true); ("/lane_navi/waypoint_max", waypoint_max, 10000); ("/lane_navi/search_radius", search_radius, 10); ("/lane_navi/velocity", velocity, 40); ("/lane_navi/frame_id", frame_id, "map"); ("/lane_navi/output_file", output_file, "/tmp/lane_"); //检验output_file是否合理 if (output_()) { ROS_ERROR_STREAM("output filename is empty"); return EXIT_FAILURE; } if (output_() == '/') { ROS_ERROR_STREAM(output_file << " is directory"); return EXIT_FAILURE; } waypoint_pub = ise("/lane_waypoints_array", pub_waypoint_queue_size, pub_waypoint_latch); ros::Subscriber route_sub = ibe("/route_cmd", sub_route_queue_size, create_waypoint); ros::Subscriber point_sub = ibe("/vector_map_info/point", sub_vmap_queue_size, cache_point); ros::Subscriber lane_sub = ibe("/vector_map_info/lane", sub_vmap_queue_size, cache_lane); ros::Subscriber node_sub = ibe("/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::VectorMapall_vmap;。void cache_point(const vector_map::PointArray& msg){ all_ = ; update_values();}void cache_lane(const vector_map::LaneArray& msg){ all_ = ; update_values();}void cache_node(const vector_map::NodeArray& msg){ all_ = ; update_values();}1.1 lane_planner::vmap::VectorMap函数定义于src/autoware/core_planning/lane_planner/include/lane_planner/lane_planner_。⽤于储存决策规划所需要的部分⽮量地图要素数据。其中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）位于src/autoware/messages/vector_map_msgs/msg/。记录所有的点云的经纬⾼blh和平⾯xy坐标 ，并⽤pid唯⼀编号，这⾥xy为utm坐标值（当然也可以是enu坐标）。# Ver 1.00int32 pidfloat64 bfloat64 lfloat64 hfloat64 bxfloat64 lyint32 refint32 mcode1int32 mcode2int32 mcode3（2）位于src/autoware/messages/vector_map_msgs/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定义了车辆可以⾏驶的道路区域。# jctuint8 NORMAL=0uint8 LEFT_BRANCHING=1uint8 RIGHT_BRANCHING=2uint8 LEFT_MERGING=3uint8 RIGHT_MERGING=4uint8 COMPOSITION=5# lanetypeuint8 STRAIGHT=0uint8 LEFT_TURN=1uint8 RIGHT_TURN=2# lanecfgfguint8 PASS=0uint8 FAIL=1# Ver 1.00int32 lnidint32 didint32 blidint32 flidint32 bnidint32 fnidint32 jctint32 blid2int32 blid3int32 blid4int32 flid2int32 flid3int32 flid4int32 clossidfloat64 spanint32 lcntint32 lno# Ver 1.20int32 lanetypeint32 limitvelint32 refvelint32 roadsecidint32 lanecfgfgint32 linkwaid（3）位于src/autoware/messages/vector_map_msgs/msg/。根据命名规则可知nid为Node的识别编号，参照其他元素可知这是Node的唯⼀标志。同样参照命名规则可知pid是Point的id，由此我们知道Node和Point之间的关系，即⼀个Node就是⼀个Point。# Ver 1.00int32 nidint32 pid（4）位于src/autoware/messages/vector_map_msgs/msg/。id是StopLine的唯⼀标志，StopLine是⽤Line来描述，其中lid指对应的Line的id。# Ver 1.00int32 idint32 lidint32 tlidint32 signidint32 # Ver 1.00int32 lidint32 bpidint32 fpidint32 blidint32 flid定义于src/autoware/messages/vector_map_msgs/msg/。Line表⽰线段，下图中的黄线，⽩线，路沿均以此来描述，⽤lid来唯⼀标志，bpid和fpid指的是连线的两个端点Point的id，blid和flid表⽰线段之间的关联关系，分别是上⼀条和下⼀条线段Line的lid，若blid=0的话需要设定起点，flid是其关联的下⼀条线的id。两个Point的连线即为Line。（5）位于src/autoware/messages/vector_map_msgs/msg/。DTLane为道路或车道中⼼线添加了线性元素和纵向横向坡度的数据，承担着中⼼线性数据的作⽤，并⽤did唯⼀编号。# Ver 1.00int32 didfloat64 distint32 pidfloat64 dirfloat64 aparafloat64 rfloat64 slopefloat64 cantfloat64 lwfloat64 rw1.2 update_valuesupdate_values函数被上⾯cache_point，cache_lane和cache_node这三个回调函数调⽤，⽤以在接收到更新的points，lanes和nodes数据时更新lane_vmap，当然要保证all_vmap中同时具有points，lanes和nodes数据才可以根据all_vmap去⽣成新的lane_vmap。接着如果接收到了路径规划的数据（cached_存在数据），则根据cached_route创建导航轨迹点。随后清除已被使⽤的cached_数据。void update_values(){ if (all_() || all_() || all_()) 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_()) {//tablet_socket_msgs::route_cmd cached_route; create_waypoint(cached_route); cached_(); cached__to_fit();//调⽤shrink_to_fit来要求vector将超出当前⼤⼩的多余内存退回给系统。 //然⽽这只是⼀个请求，标准库并不保证退还内存。 }}lane_planner::vmap::LNO_ALL定义在install/lane_planner/include/lane_planner/lane_planner_，其余还有LNO_CROSSING等。create_lane_vmap函数create_lane_vmap函数位于src/autoware/core_planning/lane_planner/lib/lane_planner/lane_planner_。create_lane_vmap函数将传⼊的all_vmap中的信息复制⾄lane_vmap。VectorMap create_lane_vmap(const VectorMap& vmap, int lno){ VectorMap lane_vmap; for (const vector_map::Lane& l : ) { if (lno != LNO_ALL && != lno) continue; lane__back(l);//将中的vector_map::Lane类型元素复制到lane_ for (const vector_map::Node& n : ) { if ( != && != ) continue; lane__back(n); for (const vector_map::Point& p : ) { if ( != ) continue; lane__back(p); } } for (const vector_map::StopLine& s : nes) { if ( != ) continue; lane__back(s); } for (const vector_map::DTLane& d : s) { if ( != ) continue; lane__back(d); } } return lane_vmap;}2. create_waypoint函数create_waypoint函数是对应于话题"/route_cmd"的回调函数，同时也被update_values函数在满⾜!cached_()==true的情况下调⽤。cached_route的定义是tablet_socket_msgs::route_cmd cached_route;。void create_waypoint(const tablet_socket_msgs::route_cmd& msg){ std_msgs::Header header; = ros::Time::now(); _id = frame_id; if (all_() || all_() || all_()) { cached_ = header; cached_ = ; 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_() < 2) return; std::vector 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（包括路径点，道路，道路 //根据只有路径点的导航地图coarse_vmap，从整个路⽹地图lane_vmap中搜寻信息，构建信息完备的导航地图fine_mostleft_vmap（包括路径点，道路，道路限速曲率等信息）。 if (fine_mostleft_() < 2) return; fine__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 (() < 2) continue; fine__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; = header; ent = 1; size_t last_index = () - 1; for (size_t i = 0; i < (); ++i) { double yaw; if (i == last_index) { geometry_msgs::Point p1 = lane_planner::vmap::create_geometry_msgs_point([i]); geometry_msgs::Point p2 = lane_planner::vmap::create_geometry_msgs_point([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([i]); //create_geometry_msgs_point函数根据Point数据构建geometry_msgs::Point消息。 geometry_msgs::Point p2 = lane_planner::vmap::create_geometry_msgs_point([i + 1]); yaw = atan2(p2.y - p1.y, p2.x - p1.x); } autoware_msgs::Waypoint w; = header; on = lane_planner::vmap::create_geometry_msgs_point([i]); ation = tf::createQuaternionMsgFromYaw(yaw); = header; .x = velocity / 3.6; // velocity默认为40km/h，这⾥将其转换为以m/s为单位。 _back(w); } lane__back(l); } waypoint_h(lane_waypoint);//在话题"/lane_waypoints_array"发布lane_waypoint for (size_t i = 0; i < fine_(); ++i) { std::stringstream ss; ss << "_" << i; std::vector v1 = split(output_file, '/'); std::vector v2 = split((), '.'); v2[0] = () + (); v1[() - 1] = join(v2, '.'); std::string path = join(v1, '/'); lane_planner::vmap::write_waypoints(fine_vmaps[i].points, velocity, path); } }}博主对下⾯的代码作⽤进⾏验证：可知下⾯代码块⽤于在output_file的后⾯加上后缀，以备后⾯将fine_vmaps中不同的导航地图中的路径点存在不同的⽂件内。std::stringstream ss;ss << "_" << i;std::vector v1 = split(output_file, '/');std::vector v2 = split((), '.');v2[0] = () + ();v1[() - 1] = join(v2, '.');std::string path = join(v1, '/');2.1 route_位于src/autoware/messages/tablet_socket_msgs/msg/route_，包含两个成员变量，header包含⼀些基本信息，point是路径点集合。Header headerWaypoint[] point2.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构建更完善的导航地图。VectorMap create_coarse_vmap_from_route(const tablet_socket_msgs::route_cmd& route){ geo_pos_conv geo; _plane(7);//设置UWB坐标原点，Autoware内置了19个经纬度坐标作为UWB坐标系的原点。 VectorMap coarse_vmap; for (const tablet_socket_msgs::Waypoint& w : ) { _to_xyz(, , 0);//将经纬度和⾼度转换为UWB坐标系下的xyz坐标 vector_map::Point p; = geo.x(); = geo.y(); coarse__back(p); } return coarse_vmap;}geo_pos_conv类geo_pos_conv类定义在src/autoware/common/gnss/include/gnss/geo_pos_，⽤于将经纬度转换为UWB坐标，内置的UWB坐标原点有多个，都设置在⼩⽇本的⼩岛上⾯，如果要⽤到咱们中国的话需要⾃⾏重新定义原点，并且相关的参数也要更改，⽐如地球半径等在不同经度是不同的。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 lonpublic: 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。但是我们的分析仍然是不管条件跳转，对所有步骤都进⾏解析。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_ = -1; if (lno == LNO_ALL) departure_point = find_nearest_point(lane_vmap, coarse_()); //departure_point为lane_vmap中与coarse_()距离最近的坐标点 else { for (int i = lno; i >= LNO_CROSSING; --i) {//LNO_CROSSING = 0 departure_point = find_departure_point(lane_vmap, i, coarse_, search_radius);//i=1；i=0 //这⾥的“出发点”指的是lane_vmap中满⾜a + d最⼩的Point x： //（1）a：Point x对应的Node作为起始点的Lane l（且编号等于传⼊的i）与直线之间的夹⾓; //（2）d：Point x与coarse_[0]的距离。 if (departure_ >= 0)//此时代表找到出发点，因为原始departure_point的pid是-1， //⽽此时departure_发⽣改变。 break; } } if (departure_ < 0) return null_vmap; vector_map::Point arrival_point; arrival_ = -1; if (lno == LNO_ALL) arrival_point = find_nearest_point(lane_vmap, coarse_()); //arrival_point为lane_vmap中与coarse_()距离最近的坐标点 else { for (int i = lno; i >= LNO_CROSSING; --i) { arrival_point = find_arrival_point(lane_vmap, i, coarse_, search_radius);//i=1;i=0（防盗标记：zhengkunxian） //这⾥的“到达点”（设为Point y）指的是lane_vmap中满⾜a + d最⼩的Point： //（1）a：直线（Point z：Point y所对应的Node作为末尾点的Lane l（且编号等于传⼊的i）的上⼀条Lane（设为Lane k）的起始点）与直线之间的夹⾓；（mark：I'm ZhengKX） //（2）d：Point y与coarse_[coarse_() - 1]的距离。 if (arrival_ >= 0) break; } } if (arrival_ < 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 ( < 0) return null_vmap; bool finish = false;

//开始构造fine_vmap for (int i = 0; i < waypoint_max; ++i) { fine__back(point); fine__back(point); //循环开始时的point为上⾯求得的departure_point；

//

查找lane对应的DTLane vector_map::DTLane dtlane; = -1; for (const vector_map::DTLane& d : lane_s) { if ( == ) { //循环开始时的lane为上⾯求得的departure_point对应的Node作为起始点的Lane，也就是车辆出发时所在的Lane。 dtlane = d; break; } } //fine_vmap内信息：（1）vector_map::DTLane：储存对应Lane的道路或车道中⼼线纵向横向坡度等数据。 fine__back(dtlane);

//

查找lane对应的StopLine vector_map::StopLine stopline; = -1; for (const vector_map::StopLine& s : lane_nes) { if ( == ) { //同样的，循环开始时的lane为上⾯求得的departure_point对应的Node作为起始点的Lane，也就是车辆出发时所在的Lane。 stopline = s; break; } } //fine_vmap内信息：（2）vector_map::StopLine：存储⼈⾏横道,交叉路⼝前的停⽌线信息。 fine__back(stopline); if (finish)//此处是跳出fine_vmap构造⼤循环的开关 break; //fine_vmap内信息：（3）vector_map::Line：车辆⾏驶的车道，包含了限速，车道类型（左转右转）等信息。 fine__back(lane); point = find_end_point(lane_vmap, lane);//在传⼊函数的⽮量地图lane_vmap中找到lane的末尾Node所对应的Point。 //此处实现了point在同⼀条Lane上的起始点和末尾点之间的切换，后⾯就会以当前lane的末尾点作为后⾯Lane的起始点寻找对应的Lane, //然后回到循环开始处根据这个新的Lane查找对应的信息存⼊fine_vmap。

if ( < 0) return null_vmap; if ( == arrival_ && == arrival_) {//到达“到达点”，停⽌循环 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_ < 0) return null_vmap; coarse_p1 = find_nearest_point(coarse_vmap, coarse_p1);//查找并返回传⼊函数的⽮量地图coarse_vmap中与lane_vmap中的coarse_p1距离最近的坐标点，并替代coarse_p1。 if (coarse_ < 0) return null_vmap; vector_map::Point coarse_p2; double distance = -1; for (const vector_map::Point& p : coarse_) { if (distance == -1) {//⼀直轮转直到coarse_p1将distance赋值为0，下⼀次循环便不会执⾏continue跳过此if结构后⾯的操作。（防盗标记：zhengkunxian） if ( == coarse_ && == coarse_) distance = 0; continue; } coarse_p2 = p; distance = hypot(coarse_ - coarse_, coarse_ - coarse_); distance = hypot(coarse_ - coarse_, coarse_ - coarse_); 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; = -1; for (int j = lno; j >= LNO_CROSSING; --j) { l = find_next_branching_lane(lane_vmap, j, lane, coarse_angle, search_radius); if ( >= 0) break; } lane = l; } } else {//如果当前lane没有分⽀，则直接调⽤find_next_lane函数找后续Lane lane = find_next_lane(lane_vmap, LNO_ALL, lane); } if ( < 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则代表未找到最近点。vector_map::Point find_nearest_point(const VectorMap& vmap, const vector_map::Point& point){ vector_map::Point nearest_point; nearest_ = -1;//若返回的nearest_<0则代表未找到最近点 double distance = DBL_MAX; for (const vector_map::Point& p : ) { double d = hypot( - , - );//计算两点之间的距离 if (d <= distance) { nearest_point = p; distance = d; } } return nearest_point;}2.3.2 find_departure_point函数vector_map::Point find_departure_point(const VectorMap& lane_vmap, int lno, const std::vector& 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_ < 0) return nearest_point; std::vector 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 ( < 0)//这种判断是否成功完成Lane寻找⽬标的⽅法跟上⾯哪些函数都是⼀样的，后⾯不再继续赘述。 continue; vector_map::Point p2 = find_end_point(lane_vmap, l);//在传⼊函数的⽮量地图lane_vmap中找到l的（mark：I'm ZhengKX） //末尾Node所对应的Point。 if ( < 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与直线之间的夹⾓ //要注意的是夹⾓a是lane_vmap中的点所构成的Lane与x轴的夹⾓ //coarse_angle是coarse_points（对应于传⼊find_departure_point函数中的create_fine_vmap函数中的coarse_）中头两个点构成的Lane与x轴的夹⾓ if (a > 180) a = fabs(a - 360); double d = hypot( - coarse_, - coarse_); double s = a + d; if (s <= score) {//前⾯的score = 180 + search_radius;中的180代表最⼤⾓度，search_radius则是最⼤距离 //也就是在lane_vmap中找到⼀个Point满⾜a + d最⼩： //（1）a：其对应的Node作为起始点的Lane（且编号等于传⼊的lno）与直线之间的夹⾓; //（2）d：与coarse_p1的距离。 nearest_point = p1; score = s; } } return nearest_point;}函数名的意思是“寻找出发点”，这⾥的“出发点”指的是lane_vmap中满⾜a + d最⼩的Point

x：（1）a：Point

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

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

x与coarse_points[0]的距离。如下图所⽰：（1）find_near_points函数寻找并返回传⼊函数的vmap中与point的距离在search_radius以内的所有路径点。std::vector find_near_points(const VectorMap& vmap, const vector_map::Point& point, double search_radius){ std::vector near_points; for (const vector_map::Point& p : ) { double d = hypot( - , - ); if (d <= search_radius)//距离在此范围内的⼀批路径点 near__back(p); } return near_points;}（2）compute_direction_angle函数原理很简单，根据p2和p1坐标计算出正切值，接着求反正切得到p2和p1构成的直线与世界坐标系x轴的夹⾓θ (π~−π），最后转换为“度”为单位。p2y−p1y180θ=arctan(p2x−p1x)×πdouble compute_direction_angle(const vector_map::Point& p1, const vector_map::Point& p2){ return (atan2( - , - ) * (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.1lane_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。vector_map::Lane find_lane(const VectorMap& vmap, int lno, const vector_map::Point& point){ vector_map::Lane error; = -1; for (const vector_map::Node& n : ) { if ( != )//找到传⼊函数的point所对应的Node continue; for (const vector_map::Lane& l : ) { if (lno != LNO_ALL && != lno)//在传⼊函数的vmap中找到车道编号和lno相符的Lane（mark：I'm ZhengKX） continue; if ( != )//找到传⼊函数的point所对应的Node作为起始Node的Lane continue; return l; } } return error;}（4）find_end_point函数find_end_point的作⽤是在传⼊函数的⽮量地图vmap中找到lane的末尾Node所对应的Point。vector_map::Point find_end_point(const VectorMap& vmap, const vector_map::Lane& lane){ vector_map::Point error; = -1; for (const vector_map::Node& n : ) { if ( != )//在传⼊函数的vmap中找到传⼊函数的lane的末尾Node所对应的Node continue; for (const vector_map::Point& p : ) { if ( != )//在传⼊函数的vmap中找到对应于上⾯找到的Node的Point continue; return p; } } return error;}2.3.3 find_arrival_point函数vector_map::Point find_arrival_point(const VectorMap& lane_vmap, int lno, const std::vector& coarse_points, double search_radius){ vector_map::Point coarse_p1 = coarse_points[coarse_() - 1];//coarse_points中的最后⼀个Point（防盗标记：zhengkunxian） vector_map::Point coarse_p2 = coarse_points[coarse_() - 2]; vector_map::Point nearest_point = find_nearest_point(lane_vmap, coarse_p1); //跟find_departure_point函数中是类似的，先找到lane_vmap中与coarse_p1距离最近的Point，作为保底返回结果 if (nearest_ < 0) return nearest_point; std::vector 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 ( < 0) continue; l = find_prev_lane(lane_vmap, lno, l);//在lane_vmap中找到Lane l的上⼀条Lane（车道编号和lno相符） if ( < 0) continue; vector_map::Point p2 = find_start_point(lane_vmap, l);//与前⾯的find_end_point函数作⽤相反， //在传⼊函数的⽮量地图lane_vmap中找到l的起始Node所对应的Point。 if ( < 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( - coarse_, - coarse_); double s = a + d; if (s <= score) { //在lane_vmap中找到⼀个Point p1满⾜a + d最⼩： //（1）a：直线（p2：p1所对应的Node作为末尾点的Lane（且编号等于传⼊的lno）的上⼀条Lane的起始点）与直线之间的夹⾓; //（2）d：与coarse_p1的距离。 nearest_point = p1; score = s; } } return nearest_point;}函数名的意思是“寻找到达点”，跟前⾯的“寻找出发点”实现过程是相似的。这⾥的“到达点”（设为Point

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

z：Point

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

l（且编号等于传⼊的lno）的上⼀条Lane（即Lane

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

y与coarse_points[coarse_() - 1]的距离。如下图所⽰：（1）find_prev_lane函数find_prev_lane函数的作⽤是找到传⼊函数的Lane lane的跟它相连的上⼀条Lane。从1.1 lane_planner::vmap::VectorMap函数中对的分析可知：（1）blid2-4： 指定要合并到当前车道的Lane ID。(0表⽰没有合并lane)；（2）flid2-4： 指定要分流出去的车道Lane ID。(0表⽰车道没有分⽀)；vector_map::Lane find_prev_lane(const VectorMap& vmap, int lno, const vector_map::Lane& lane){ vector_map::Lane error; = -1; if (is_merging_lane(lane)) {//判断lane是否处于合并模式 for (const vector_map::Lane& l : ) { if (lno != LNO_ALL && != lno)//在传⼊函数的vmap中找到车道编号和lno相符的Lane l continue; if ( != && != 2 && != 3 && != 4)//l是否是合并到Lane lane的上⼀条Lane continue; return l; } } else { for (const vector_map::Lane& l : ) { if ( != )//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函数中对的分析可知：jct： 指定当前车道分⽀/合并模式，（旧版本：0：正常，1：分⽀到左边，2：分⽀到右边，3：合并到左车道，4：合并到右车道 ），根据下⾯的代码可知当前Autoware V12.0版本中对于有了新的定义，3，4，5都代表道路合并，结合后⾯分析的is_branching_lane函数我们认为jct=5代表未知。bool is_merging_lane(const vector_map::Lane& lane){ return ( == 3 || == 4 || == 5);}（2）find_start_point函数find_start_point的作⽤是在传⼊函数的⽮量地图vmap中找到lane的起始Node所对应的Point。vector_map::Point find_start_point(const VectorMap& vmap, const vector_map::Lane& lane){ vector_map::Point error; = -1; for (const vector_map::Node& n : ) { if ( != )//在传⼊函数的vmap中找到传⼊函数的lane的起始Node所对应的Node continue; for (const vector_map::Point& p : ) { if ( != )//在传⼊函数的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中记录的数据即可。bool is_branching_lane(const vector_map::Lane& lane){ return ( == 1 || == 2 || == 5);}2.3.5 find_next_branching_lane函数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; = -1; vector_map::Point p1 = find_end_point(vmap, lane);//在vmap中找到lane的末尾Node对应的Point p1。 if ( < 0) return error; std::vector> candidates; for (const vector_map::Lane& l1 : ) { if (lno != LNO_ALL && != lno)//当lno=LNO_ALL或者=lno的时候不执⾏continue，也就是执⾏后⾯的代码。 continue; if ( == || == 2 || == 3 || == 4) { //从1.1 lane_planner::vmap::VectorMap函数中对的分析可知： //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 ( < 0) continue; vector_map::Point p2 = p; double d = hypot( - , - );//计算Lane l2的起始点和末尾点之间的距离，也就是Lane l2的路长。（I'm ZhengKX） while (d <= search_radius && != 0 && !is_branching_lane(l2)) { //⼀直循环直到找到距离传⼊函数中的Lane lane的末尾点刚刚超出搜索半径search_radius的Point //注意：此Point是lane的后续Lane的末尾点。 //或者迭代后的l2的后续Lane不存在（ = 0），或者迭代后的l2存在分⽀。 //此处while循环终⽌的条件有三个，但⽬的条件其实是d <= search_radius，另外两个条件⽤于问题避免， //因此最终得到的candidates还需要对其中的元素进⼀步筛选。 l2 = find_next_lane(vmap, LNO_ALL, l2);//不断⽤l2的下⼀条Lane迭代l2，往路⽹深处遍历 if ( < 0) break; p = find_end_point(vmap, l2); if ( < 0) break; p2 = p;//不断迭代p2 d = hypot( - , - ); } _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 (()) return error; vector_map::Lane branching_lane; double angle = 180; for (const std::tuple& 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’mZhengKX），记录此时被遍历到的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符合。vector_map::Lane find_next_lane(const VectorMap& vmap, int lno, const vector_map::Lane& lane){ vector_map::Lane error; = -1; if (is_branching_lane(lane)) {//判断Lane lane是否有分⽀。 for (const vector_map::Lane& l : ) { if (lno != LNO_ALL && != lno)//通⽤做法了，满⾜条件才执⾏后⾯的代码。 continue; if ( != && != 2 && != 3 && != 4)//Lane l为lane的分⽀Lane（利⽤记录在⽮量地图中的信息，⽮量地图对于决策⽽⾔作⽤重⼤）。（I'm ZhengKX） continue; return l; } } else { for (const vector_map::Lane& l : ) { if ( != )//若Lane lane没有分⽀的话，也就是lane仅有⼀条跟它相连的后续Lane，直接返回这个Lane即可，连判断道路编号都不需要了。 continue; return l; } } return error;}2.4 count_lane函数count_lane函数查找并返回传⼊函数的⽮量地图vmap中的最⼤车道数。int count_lane(const lane_planner::vmap::VectorMap& vmap){ int lcnt = -1; for (const vector_map::Lane& l : ) { if ( > lcnt)//是Lane l的车道数 lcnt = ; } return lcnt;//返回的是⽮量地图vmap中各个Lane中的最⼤车道数}2.5 create_geometry_msgs_point函数根据Point数据构建geometry_msgs::Point消息。geometry_msgs::Point create_geometry_msgs_point(const vector_map::Point& vp){ // reverse X-Y axis geometry_msgs::Point gp; gp.x = ; gp.y = ; gp.z = vp.h; return gp;}2.6 write_waypoints函数计算航向⾓yaw，调⽤write_waypoint函数将路径点数据写⼊⽂件path。void write_waypoints(const std::vector& points, double velocity, const std::string& path){ if (() < 2) return; size_t last_index = () - 1; for (size_t i = 0; i < (); ++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函数⽤于路径点的数据写⼊。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 << << "," << std::fixed << << "," << std::fixed << point.h << "," << std::fixed << yaw << std::endl; } else { std::ofstream ofs(path.c_str(), std::ios_base::app); ofs << std::fixed << << "," << std::fixed << << "," << std::fixed << point.h << "," << std::fixed << yaw << "," << std::fixed << velocity << std::endl; }}唉，决策⾥⾯需要⽮量地图的信息是我当初没有预料到的，道⾏不够，在⽮量地图⾥⾯各个元素中的变量含义上⾯卡了挺久，不过我卡了，⼤家看我的博客就不会被卡了。博主想到那⾥就写到哪，不可避免有些疏漏或者谬误之处，但是⽆伤⼤雅，给⼤家做⼊门参考⽤是够了的。