Autoware planning模塊學習筆記（二）：路徑規劃（6）- 節點lane_stop

Autoware planning模塊學習筆記（二）：路徑規劃（6）- 節點lane_stop

寫在前面：引用的時候請註明出處（I’m ZhengKX的博客：https://blog.csdn.net/xiaoxiao123jun），尊重他人的原創勞動成果，不打擊原作者的創作激情，才能更好地促進我國科教進步，謝謝！

繼續爲大家講解Autoware planning模塊，前面爲大家分析了“勾選Mission Planning->lane_planner->lane_rule”後所啓動的節點lane_rule。這篇博客繼續分析後續操作。

再勾選Mission Planning->lane_planner->lane_stop

我們依然查找src/autoware/utilities/runtime_manager/scripts/computing.yaml可知勾選lane_stop（防盜標記：zhengkunxian）後運行ROS包lane_planner內的ROS節點lane_stop。

由lane_planner的CMakeLists.txt可知節點lane_stop的源文件是nodes/lane_stop/lane_stop.cpp。

節點lane_stop

這個節點的內容相較於其他節點而言就少了很多，博主分析起來也輕鬆也許多。首先是其main函數，main函數在src/autoware/core_planning/lane_planner/nodes/lane_stop/lane_stop.cpp中。跟大多數節點一樣， main函數裏面只做了三件事，首先設置了一些參數，接着設置發佈者和監聽者。

int main(int argc, char **argv)
{
	ros::init(argc, argv, "lane_stop");

	ros::NodeHandle n;

	int sub_light_queue_size;
	//設置參數默認值
	n.param<int>("/lane_stop/sub_light_queue_size", sub_light_queue_size, 1);
	int sub_waypoint_queue_size;
	n.param<int>("/lane_stop/sub_waypoint_queue_size", sub_waypoint_queue_size, 1);
	int sub_config_queue_size;
	n.param<int>("/lane_rule/sub_config_queue_size", sub_config_queue_size, 1);
	int pub_waypoint_queue_size;
	n.param<int>("/lane_stop/pub_waypoint_queue_size", pub_waypoint_queue_size, 1);
	bool pub_waypoint_latch;
	n.param<bool>("/lane_stop/pub_waypoint_latch", pub_waypoint_latch, true);

	//設置發佈者
	traffic_pub = n.advertise<autoware_msgs::LaneArray>("/traffic_waypoints_array", pub_waypoint_queue_size,
								pub_waypoint_latch);
	//設置訂閱者
	ros::Subscriber light_sub = n.subscribe("/light_color", sub_light_queue_size, receive_auto_detection);
	ros::Subscriber light_managed_sub = n.subscribe("/light_color_managed", sub_light_queue_size,
							receive_manual_detection);
	ros::Subscriber red_sub = n.subscribe("/red_waypoints_array", sub_waypoint_queue_size, cache_red_lane);
	ros::Subscriber green_sub = n.subscribe("/green_waypoints_array", sub_waypoint_queue_size, cache_green_lane);
	ros::Subscriber config_sub = n.subscribe("/config/lane_stop", sub_config_queue_size, config_parameter);

	ros::spin();

	return EXIT_SUCCESS;
}

1. config_parameter函數

config_parameter函數是話題“/config/lane_stop”的回調函數，用於配置參數config_manual_detection。

void config_parameter(const autoware_config_msgs::ConfigLaneStop& msg)
{
	config_manual_detection = msg.manual_detection;
}

2. cache_red_lane和cache_green_lane函數

cache_red_lane和cache_green_lane函數分別是話題“/red_waypoints_array”和“/green_waypoints_array”的回調函數，根據監聽得到的消息更新current_red_lane和current_green_lane。話題“/red_waypoints_array”和“/green_waypoints_array”是節點lane_rule發佈的，話題“/red_waypoints_array”內的導航路徑相較於話題“/green_waypoints_array”內的導航路徑對接近/離開停車線的車速進行了修正，實現減速接近/加速離開。

void cache_red_lane(const autoware_msgs::LaneArray& msg)
{
	current_red_lane = msg;
}

void cache_green_lane(const autoware_msgs::LaneArray& msg)
{
	current_green_lane = msg;
}

3. receive_auto_detection和receive_manual_detection函數

receive_auto_detection和receive_manual_detection函數分別是話題“/light_color”和“/light_color_managed”的回調函數，原意是根據config_manual_detection調用對應的函數，然而這裏調用的都是select_current_lane函數，其實並沒有起到區分的作用。

void receive_auto_detection(const autoware_msgs::TrafficLight& msg)
{
	if (!config_manual_detection)
		select_current_lane(msg);
}

void receive_manual_detection(const autoware_msgs::TrafficLight& msg)
{
	if (config_manual_detection)
		select_current_lane(msg);
}

select_current_lane函數

select_current_lane函數被receive_auto_detection和receive_manual_detection函數調用，根據信號燈的相位確定是選擇current_red_lane還是current_green_lane，並將選擇的路徑發佈到話題"/traffic_waypoints_array"，實際上話題"/traffic_waypoints_array"同時也被節點lane_rule發佈。

void select_current_lane(const autoware_msgs::TrafficLight& msg)
{
	const autoware_msgs::LaneArray *current;
	switch (msg.traffic_light) {//根據信號燈狀態選擇對應的導航路徑
	case lane_planner::vmap::TRAFFIC_LIGHT_RED:
		current = &current_red_lane;
		break;
	case lane_planner::vmap::TRAFFIC_LIGHT_GREEN:
		current = &current_green_lane;
		break;
	case lane_planner::vmap::TRAFFIC_LIGHT_UNKNOWN:
		current = previous_lane; //如果信號燈狀態未知，則保持原狀態。
		break;
	default:
		ROS_ERROR_STREAM("undefined traffic light");
		return;
	}

	if (current->lanes.empty()) {
		ROS_ERROR_STREAM("empty lanes");
		return;
	}

	traffic_pub.publish(*current);//發佈導航路徑到話題"/traffic_waypoints_array"

	previous_lane = current;
}

節點lane_stop的內容確實很簡單，就是根據信號燈相位選擇之前節點lane_rule發佈的紅燈/綠燈時導航路徑，並將其發佈到話題"/traffic_waypoints_array"，能起到更新替換前面節點lane_rule發佈到話題"/traffic_waypoints_array"中的導航路徑消息的作用，相較於之前分析的其他節點，尤其是lane_navi，真是輕鬆不少。