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中。首先設置了一些參數，接着設置發佈者和監聽者。

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;。

void cache_point(const vector_map::PointArray& msg)
{
	all_vmap.points = msg.data;
	update_values();
}

void cache_lane(const vector_map::LaneArray& msg)
{
	all_vmap.lanes = msg.data;
	update_values();
}

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座標）。

# 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定義了車輛可以行駛的道路區域。

# 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。

# 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。

# Ver 1.00
int32 id
int32 lid
int32 tlid
int32 signid
int32 linkid

Line.msg

# 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唯一編號。

# 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數據。

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。

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;。

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中不同的導航地圖中的路徑點存在不同的文件內。

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是路徑點集合。

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構建更完善的導航地圖。

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座標原點有多個，都設置在小日本的小島上面，如果要用到咱們中國的話需要自行重新定義原點，並且相關的參數也要更改，比如地球半徑等在不同經度是不同的。

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。但是我們的分析仍然是不管條件跳轉，對所有步驟都進行解析。

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則代表未找到最近點。

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函數

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 $x$：
（1）a：Point $x$對應的Node作爲起始點的Lane $l$（且編號等於傳入的lno）與直線<coarse_points[0],coarse_points[1]>之間的夾角；
（2）d：Point $x$與coarse_points[0]的距離。
如下圖所示：

（1）find_near_points函數

尋找並返回傳入函數的vmap中與point的距離在search_radius以內的所有路徑點。

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$），最後轉換爲“度”爲單位。

$\theta= arctan(\frac {p2_y-p1_y} {p2_x-p1_x})\times\frac {180} {\pi}$

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。

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。

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函數

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 $y$）指的是lane_vmap中滿足a + d最小的Point：
（1）a：直線$<z,y>$（Point $z$：Point $y$所對應的Node作爲末尾點的Lane $l$（且編號等於傳入的lno）的上一條Lane（即Lane $k$）的起始點）與直線<coarse_points[coarse_points.size() - 1],coarse_points[coarse_points.size() - 2]>之間的夾角；
（2）d：Point $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表示車道沒有分支)；

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代表未知。

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。

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中記錄的數據即可。

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函數

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符合。

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中的最大車道數。

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消息。

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。

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函數

用於路徑點的數據寫入。

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;
	}
}

唉，決策裏面需要矢量地圖的信息是我當初沒有預料到的，道行不夠，在矢量地圖裏面各個元素中的變量含義上面卡了挺久，不過我卡了，大家看我的博客就不會被卡了。

博主想到那裏就寫到哪，不可避免有些疏漏或者謬誤之處，但是無傷大雅，給大家做入門參考用是夠了的。