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，真是轻松不少。