Autoware planning模块学习笔记（二）：路径规划（5）- 节点lane_rule
Autoware planning模块学习笔记（二）：路径规划（5）- 节点lane_rule
写在前面：引用的时候请注明出处（I’m ZhengKX的博客：https://blog.csdn.net/xiaoxiao123jun），尊重他人的原创劳动成果，不打击原作者的创作激情，才能更好地促进我国科教进步，谢谢！
继续为大家讲解Autoware planning模块，前面为大家分析了“勾选Mission Planning->lane_planner->lane_navi”后所启动的节点lane_navi。这篇博客继续分析后续操作。
勾选Mission Planning->lane_planner->lane_rule
我们依然查找src/autoware/utilities/runtime_manager/scripts/computing.yaml可知勾选lane_rule（防盗标记：zhengkunxian）后运行ROS包lane_planner内的ROS节点lane_rule。
由lane_planner的CMakeLists.txt可知节点lane_rule的源文件是nodes/lane_rule/lane_rule.cpp。
节点lane_rule
首先是其main函数，main函数在src/autoware/core_planning/lane_planner/nodes/lane_rule/lane_rule.cpp中。虽然内容看起来比较多，实际上只做了三件事，首先设置了一些参数，接着设置发布者和监听者。
int main(int argc, char** argv)
{
ros::init(argc, argv, "lane_rule");
ros::NodeHandle n;
int sub_vmap_queue_size;
//--------------------------------设置参数默认值-----------------------------------------
n.param<int>("/lane_rule/sub_vmap_queue_size", sub_vmap_queue_size, 1);
int sub_waypoint_queue_size;
n.param<int>("/lane_rule/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_rule/pub_waypoint_queue_size", pub_waypoint_queue_size, 1);
bool pub_waypoint_latch;
n.param<bool>("/lane_rule/pub_waypoint_latch", pub_waypoint_latch, true);
n.param<int>("/lane_rule/waypoint_max", waypoint_max, 10000);
n.param<double>("/lane_rule/search_radius", search_radius, 10);
n.param<double>("/lane_rule/curve_weight", curve_weight, 0.6);
n.param<double>("/lane_rule/crossroad_weight", crossroad_weight, 0.9);
n.param<double>("/lane_rule/clothoid_weight", clothoid_weight, 0.215);
n.param<std::string>("/lane_rule/frame_id", frame_id, "map");
//--------------------------------设置发布者-----------------------------------------
traffic_pub =
n.advertise<autoware_msgs::LaneArray>("/traffic_waypoints_array", pub_waypoint_queue_size, pub_waypoint_latch);
red_pub = n.advertise<autoware_msgs::LaneArray>("/red_waypoints_array", pub_waypoint_queue_size, pub_waypoint_latch);
green_pub =
n.advertise<autoware_msgs::LaneArray>("/green_waypoints_array", pub_waypoint_queue_size, pub_waypoint_latch);
//--------------------------------设置订阅者-----------------------------------------
ros::Subscriber waypoint_sub = n.subscribe("/lane_waypoints_array", sub_waypoint_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::Subscriber stopline_sub = n.subscribe("/vector_map_info/stop_line", sub_vmap_queue_size, cache_stopline);
ros::Subscriber dtlane_sub = n.subscribe("/vector_map_info/dtlane", sub_vmap_queue_size, cache_dtlane);//防盗标记：zhengkunxian
ros::Subscriber config_sub = n.subscribe("/config/lane_rule", sub_config_queue_size, config_parameter);
ros::spin();
return EXIT_SUCCESS;
}
1. cache_point，cache_lane，cache_node，cache_stopline和cache_dtlane函数
在前面的Autoware planning模块学习笔记（二）：路径规划（4）- 节点lane_navi中已经分析了cache_point，cache_lane和cache_node函数，节点lane_rule比相较于节点lane_navi对all_vmap的更新更加全面。这些函数同样是在更新all_vmap中的数据后，调用update_values函数，但是节点lane_rule的update_values函数跟节点lane_navi的update_values函数有些不一样。
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();
}
void cache_stopline(const vector_map::StopLineArray& msg)
{
all_vmap.stoplines = msg.data;
update_values();
}
void cache_dtlane(const vector_map::DTLaneArray& msg)
{
all_vmap.dtlanes = msg.data;
update_values();
}
update_values函数
对lane_vmap，curve_radius_min，crossroad_radius_min，clothoid_radius_min进行更新，最后调用create_waypoint函数更新期望轨迹点。
void update_values()
{
if (all_vmap.points.empty() || all_vmap.lanes.empty() || all_vmap.nodes.empty() || all_vmap.stoplines.empty() ||
all_vmap.dtlanes.empty())//判断条件相较于节点lane_navi多了对all_vmap.stoplines和all_vmap.dtlanes的判断
return;
lane_vmap = lane_planner::vmap::create_lane_vmap(all_vmap, lane_planner::vmap::LNO_ALL);//调用的create_lane_vmap函数与节点lane_navi是一致的，不再赘述。
//----------------------后面的部分就都与节点lane_navi的update_values函数不一样了----------------------------
curve_radius_min = lane_planner::vmap::RADIUS_MAX;
crossroad_radius_min = lane_planner::vmap::RADIUS_MAX;
clothoid_radius_min = lane_planner::vmap::RADIUS_MAX;
//下面的for循环遍历lane_vmap.dtlanes对curve_radius_min，crossroad_radius_min和clothoid_radius_min进行赋值。（防盗标记：zhengkunxian）
for (const vector_map::DTLane& d : lane_vmap.dtlanes)
{
double radius_min = fabs(d.r);
if (lane_planner::vmap::is_curve_dtlane(d))
{
if (lane_planner::vmap::is_crossroad_dtlane(d))
{
if (radius_min < crossroad_radius_min)
crossroad_radius_min = radius_min;
}
else
{
if (radius_min < curve_radius_min)
curve_radius_min = radius_min;
}
}
else if (lane_planner::vmap::is_clothoid_dtlane(d))
{
if (radius_min < clothoid_radius_min)
clothoid_radius_min = radius_min;
}
}
if (!cached_waypoint.lanes.empty())
{
autoware_msgs::LaneArray update_waypoint = cached_waypoint;
create_waypoint(update_waypoint);
}
}
is_curve_dtlane，is_crossroad_dtlane和is_clothoid_dtlane函数
bool is_curve_dtlane(const vector_map::DTLane& dtlane)//是弯道？
{
return (dtlane.apara == 0 && dtlane.r != RADIUS_MAX);
}
bool is_crossroad_dtlane(const vector_map::DTLane& dtlane)//是十字路口？
{
//以半径10m或者更小的半径沿十字路口行驶
return (fabs(dtlane.r) <= 10);
}
bool is_clothoid_dtlane(const vector_map::DTLane& dtlane)//道路回旋？
{
return (dtlane.apara != 0);
}
2. config_parameter函数
作用正如其名，配置参数。最后调用create_waypoint函数更新期望轨迹点。
void config_parameter(const autoware_config_msgs::ConfigLaneRule& msg)
{
//下面根据人际交互面板上的参数设置对变量进行赋值
config_acceleration = msg.acceleration;
config_stopline_search_radius = msg.stopline_search_radius;
config_number_of_zeros_ahead = msg.number_of_zeros_ahead;
config_number_of_zeros_behind = msg.number_of_zeros_behind;
config_number_of_smoothing_count = msg.number_of_smoothing_count;
//与update_values函数中对create_waypoint函数的调用方式是一样的，之所以新建update_waypoint，是为了避免函数内部操作改变（防盗标记：zhengkunxian）
//cached_waypoint的值，造成意外干扰。
if (!cached_waypoint.lanes.empty())
{
autoware_msgs::LaneArray update_waypoint = cached_waypoint;
create_waypoint(update_waypoint);
}
}
3. create_waypoint函数
节点lane_rule的create_waypoint函数接收的数据类型是autoware_msgs::LaneArray，而节点lane_navi的create_waypoint函数接收的数据类型是tablet_socket_msgs::route_cmd。节点lane_rule的create_waypoint函数是话题“/lane_waypoints_array”的回调函数，而话题“/lane_waypoints_array”的发布者正是节点lane_navi（防盗标记：zhengkunxian），实际上节点lane_navi根据路由指令在矢量地图内搜索到达目的地的路径并在话题“/lane_waypoints_array”上进行发布，被节点lane_rule监听后对规划路径进一步修正。
void create_waypoint(const autoware_msgs::LaneArray& msg)
{
std_msgs::Header header;
header.stamp = ros::Time::now();
header.frame_id = frame_id;
cached_waypoint.lanes.clear();
cached_waypoint.lanes.shrink_to_fit();
cached_waypoint.id = msg.id;
for (const autoware_msgs::Lane& l : msg.lanes)
cached_waypoint.lanes.push_back(create_new_lane(l, header));//create_new_lane函数更新传入函数的Lane l的所有元素的header。
if (all_vmap.points.empty() || all_vmap.lanes.empty() || all_vmap.nodes.empty() || all_vmap.stoplines.empty() ||
all_vmap.dtlanes.empty())//如果all_vmap（在节点lane_navi中也有此变量，通读对节点lane_navi的解析可知其作用，（防盗标记：zhengkunxian）
//简单而言all_vmap是各种数据最完备的矢量地图）有元素为空，则直接发布消息到话题"/traffic_waypoints_array"。
{
traffic_pub.publish(cached_waypoint);
return;
}
autoware_msgs::LaneArray traffic_waypoint;
autoware_msgs::LaneArray red_waypoint;
autoware_msgs::LaneArray green_waypoint;
traffic_waypoint.id = red_waypoint.id = green_waypoint.id = msg.id;
for (size_t i = 0; i < msg.lanes.size(); ++i)
{
autoware_msgs::Lane lane = create_new_lane(msg.lanes[i], header);
lane_planner::vmap::VectorMap coarse_vmap = lane_planner::vmap::create_coarse_vmap_from_lane(lane);//创建只有轨迹点的粗略导航地图。
if (coarse_vmap.points.size() < 2)
{
traffic_waypoint.lanes.push_back(lane);
continue;
}
lane_planner::vmap::VectorMap fine_vmap = lane_planner::vmap::create_fine_vmap(
lane_vmap, lane_planner::vmap::LNO_ALL, coarse_vmap, search_radius, waypoint_max);
//create_fine_vmap在分析节点lane_navi的时候已经详细分析了，不再赘述。
if (fine_vmap.points.size() < 2 || !is_fine_vmap(fine_vmap, lane))
//is_fine_vmap函数根据lane中的数据对比fine_vmap中储存的数据，判断fine_vmap是否正确生成
{
traffic_waypoint.lanes.push_back(lane);
green_waypoint.lanes.push_back(lane);
lane = apply_stopline_acceleration(lane, config_acceleration, config_stopline_search_radius,
config_number_of_zeros_ahead, config_number_of_zeros_behind);
//apply_stopline_acceleration函数对传入函数的lane里面接近停止点和离开停止点的轨迹点速度进行修正。
red_waypoint.lanes.push_back(lane);
continue;
}
for (size_t j = 0; j < lane.waypoints.size(); ++j)
{
lane.waypoints[j].twist.twist.linear.x *= create_reduction(fine_vmap, j);//根据当前路径点所处Lane是否是弯道或者回旋道等情况降低车速
if (fine_vmap.dtlanes[j].did >= 0)
{
lane.waypoints[j].dtlane = lane_planner::vmap::create_waypoint_follower_dtlane(fine_vmap.dtlanes[j]);
//create_waypoint_follower_dtlane函数更新lane.waypoints[j].dtlane，
//拷贝传入函数的fine_vmap.dtlanes[j]中除了did和pid外的属性数据，did和pid为默认值。
}
}
/* velocity smoothing */
for (int k = 0; k < config_number_of_smoothing_count; ++k)
{
autoware_msgs::Lane temp_lane = lane;
if (lane.waypoints.size() >= 3)//检查路径点数目
{
for (size_t j = 1; j < lane.waypoints.size() - 1; ++j)//遍历lane中的路径点
{
if (lane.waypoints.at(j).twist.twist.linear.x != 0)
{
lane.waypoints[j].twist.twist.linear.x =
(temp_lane.waypoints.at(j - 1).twist.twist.linear.x + temp_lane.waypoints.at(j).twist.twist.linear.x +
temp_lane.waypoints.at(j + 1).twist.twist.linear.x) /
3;//顺滑速度的方式很简单，以下标为j的路径点为中心将三个连续点速度取均值作为下标为j的路径点的速度
}
}
}
}
lane = apply_crossroad_acceleration(lane, config_acceleration);//apply_crossroad_acceleration函数对传入函数的lane里面接近交叉口起始点和离开交叉口末尾点的轨迹点速度进行修正。
traffic_waypoint.lanes.push_back(lane);
green_waypoint.lanes.push_back(lane);
lane = apply_stopline_acceleration(lane, config_acceleration, fine_vmap, config_number_of_zeros_ahead,
config_number_of_zeros_behind);
//此处apply_stopline_acceleration函数与前面的apply_stopline_acceleration函数不是一个函数，函数的参数列表是不一样的。（防盗标记：zhengkunxian）
//前面的apply_stopline_acceleration函数因为缺少导航矢量地图fine_vmap需要在全局路网地图lane_vmap中寻找停止点，
//而此处的apply_stopline_acceleration函数直接借助fine_vmap即可找到行车路径上的停止点。
red_waypoint.lanes.push_back(lane);
}
//-------------------发布消息至话题-------------------
traffic_pub.publish(traffic_waypoint);
red_pub.publish(red_waypoint);
//red_waypoint是经过apply_stopline_acceleration函数处理过的，是因为要应用于红灯时在停车线减速停车的场景。
green_pub.publish(green_waypoint);
}
create_waypoint函数根据交规对行车轨迹点修正的主要流程如下图所示，实际上修正的只有车速：
3.1 create_new_lane函数
更新传入函数的Lane lane的所有元素的帧头header。
autoware_msgs::Lane create_new_lane(const autoware_msgs::Lane& lane, const std_msgs::Header& header)
{
autoware_msgs::Lane l = lane;
l.header = header;
for (autoware_msgs::Waypoint& w : l.waypoints)
{
w.pose.header = header;
w.twist.header = header;
}
return l;
}
3.2 create_coarse_vmap_from_lane函数
作用与节点lane_navi内的create_coarse_vmap_from_route函数一样，只不过因为输入的数据类型不同，内部实现过程有所区别。
VectorMap create_coarse_vmap_from_lane(const autoware_msgs::Lane& lane)
{
VectorMap coarse_vmap;
for (const autoware_msgs::Waypoint& w : lane.waypoints)
coarse_vmap.points.push_back(create_vector_map_point(w.pose.pose.position));
return coarse_vmap;
}
create_vector_map_point函数
将以geometry_msgs::Point类型储存的轨迹点gp中转换为vector_map::Point类型，注意的是x-y坐标是反转的。
vector_map::Point create_vector_map_point(const geometry_msgs::Point& gp)
{
//反向X-Y轴
vector_map::Point vp;
vp.bx = gp.y;
vp.ly = gp.x;
vp.h = gp.z;
return vp;
}
3.3 is_fine_vmap函数
根据VectorMap中的路径点数量和每个路径点与lane中对应路径点的距离判断fine_vmap是否正确生成。
bool is_fine_vmap(const lane_planner::vmap::VectorMap& fine_vmap, const autoware_msgs::Lane& lane)
{
if (fine_vmap.points.size() != lane.waypoints.size())
return false;
for (size_t i = 0; i < fine_vmap.points.size(); ++i)
{
vector_map::Point point = lane_planner::vmap::create_vector_map_point(lane.waypoints[i].pose.pose.position);
//要先用create_vector_map_point函数进行转换，因为矢量地图内轨迹点的坐标与geometry_msgs::Point类型的轨迹点的x-y坐标是反向的。（mark：I'm ZhengKX）
double distance = hypot(fine_vmap.points[i].bx - point.bx, fine_vmap.points[i].ly - point.ly);
if (distance > 0.1)//对应的路径点应当是在同一位置的。
return false;
}
return true;
}
3.4 apply_stopline_acceleration(const autoware_msgs::Lane& lane, double acceleration,double stopline_search_radius, size_t ahead_cnt, size_t behind_cnt)函数
对车速进行修正，以停车点为中心修正附近路径点车速为0，接着运用匀减速/匀加速动力学方程修正匀减速接近/匀加速离开停车点过程中的车速。需要注意的是，这个apply_stopline_acceleration函数需要先遍历全局矢量地图lane_vmap查找行车路径中所有停止点（停止线跟前的轨迹点）的下标，（防盗标记：zhengkunxian）因为此函数是在行车矢量地图fine_vmap失效的情况下被调用的。后面还有与此处apply_stopline_acceleration函数同名的函数，但是参数列表里面stopline_search_radius被替换为fine_vmap，可以直接查询行车路径上的停止点，而不必去全局矢量地图lane_vmap中遍历搜索。
autoware_msgs::Lane apply_stopline_acceleration(const autoware_msgs::Lane& lane, double acceleration,
double stopline_search_radius, size_t ahead_cnt, size_t behind_cnt)
{
autoware_msgs::Lane l = lane;
std::vector<size_t> indexes = create_stop_indexes(lane_vmap, l, stopline_search_radius);//查找l中所有停止点（停止线跟前的轨迹点）的下标。
if (indexes.empty())
return l;
for (const size_t i : indexes)
l = apply_acceleration(l, acceleration, i, behind_cnt + 1, 0);
//修正l中路径点的速度，首先固定l内下标在[i，i+behind_cnt + 1)范围内的路径点速度都为0，
//随后根据加速情况下的动力学公式修正后面路径点的速度。
//总体而言，对于停止点和已经经过停止点后的一些路径点令车静止，再后面的路径点根据加速离开停止点修正原来的车速。
std::reverse(l.waypoints.begin(), l.waypoints.end());//反转l.waypoints中的所有元素。
std::vector<size_t> reverse_indexes;
for (const size_t i : indexes)
reverse_indexes.push_back(l.waypoints.size() - i - 1);//记录反转后的l.waypoints中停止点的下标，此时下标是从大到小。（防盗标记：zhengkunxian）
std::reverse(reverse_indexes.begin(), reverse_indexes.end());//反转reverse_indexes，此时下标变为正常的从小到大。
for (const size_t i : reverse_indexes)
l = apply_acceleration(l, acceleration, i, ahead_cnt + 1, 0);//反转后再调用apply_acceleration，
//总体而言，这里与上面调用apply_acceleration函数的情况相反，
//此处对于停止点和还未到停止点前的一些路径点令车静止，而这些路径点更前面的路径点根据减速到达停止点修正原来的车速。
//上面两次调用apply_acceleration函数，对车速进行修正，首先以停车点为中心修正附近路径点车速为0,接着运用匀减速/匀加速动力学//方程修正匀减速接近/匀加速离开停车点过程中的车速。
std::reverse(l.waypoints.begin(), l.waypoints.end());//恢复l.waypoints中路径点的顺序。
return l;
}
3.4.1 create_stop_indexes函数
首先遍历vmap寻找其中的所有停止点，接着遍历lane中的轨迹点（也就是行车路线上的轨迹点），计算其与停止点之间的距离，在stopline_search_radius内的即记录这个轨迹点的下标。也就是这个行车路径上的轨迹点足够靠近全局矢量地图vmap中的某个停止点，可以认为这个轨迹点就是行车路线上的停车点。总体而言就是先在全局矢量地图vmap中遍历寻找停止点，然后找到在自动驾驶车辆行车路径上的停止点。之所以需要先在全局矢量地图vmap中遍历寻找停止点，是因为缺失了行车轨迹矢量地图fine_vmap。
std::vector<size_t> create_stop_indexes(const lane_planner::vmap::VectorMap& vmap, const autoware_msgs::Lane& lane,
double stopline_search_radius)
{
std::vector<size_t> stop_indexes;
for (const vector_map::Point& p : create_stop_points(vmap))//遍历vmap中的所有停止点
{
size_t index = SIZE_MAX;
double distance = DBL_MAX;
for (size_t i = 0; i < lane.waypoints.size(); ++i)//遍历lane.waypoints中的轨迹点
{
vector_map::Point point = lane_planner::vmap::create_vector_map_point(lane.waypoints[i].pose.pose.position);
double d = hypot(p.bx - point.bx, p.ly - point.ly);//计算此停止点和所有行车路径上轨迹点的距离
if (d <= distance)//得到此停车点与所有行车路径上轨迹点的最小距离
{
index = i;
distance = d;
}
}
if (index != SIZE_MAX && distance <= stopline_search_radius)
//这个最小距离距离在搜索范围stopline_search_radius内即添加这个轨迹点的下标进stop_indexes
//也就是这个行车路径上的轨迹点足够靠近全局矢量地图vmap中的某个停止点，可以认为这个轨迹点就是行车路线上的停车点。
{
stop_indexes.push_back(index);
}
}
std::sort(stop_indexes.begin(), stop_indexes.end());
return stop_indexes;
}
create_stop_points函数
遍历vmap找到所有停车线跟前的轨迹点（停止点）。
std::vector<vector_map::Point> create_stop_points(const lane_planner::vmap::VectorMap& vmap)
{
std::vector<vector_map::Point> stop_points;
for (const vector_map::StopLine& s : vmap.stoplines)//遍历vmap.stoplines
{
for (const vector_map::Lane& l : vmap.lanes)//遍历vmap.lanes找到与StopLine s相连接的Lane l
{
if (l.lnid != s.linkid)
continue;
for (const vector_map::Node& n : vmap.nodes)//遍历vmap.nodes找到Lane l的起始点Node n
{
if (n.nid != l.bnid)
continue;
for (const vector_map::Point& p : vmap.points)//遍历vmap.points找到Node n对应的Point p
{
if (p.pid != n.pid)
continue;
bool hit = false;
for (const vector_map::Point& sp : stop_points)//遍历stop_points检查准备存入stop_points中的Point p是否跟stop_points中已存的Point的ID相撞。（防盗标记：zhengkunxian）
{
if (sp.pid == p.pid)
{
hit = true;
break;
}
}
if (!hit)
stop_points.push_back(p);//将Point p存入stop_points
}
}
}
}
return stop_points;
}
3.4.2 apply_acceleration函数
修正lane中路径点的速度，首先固定lane内下标在[start_index，start_index+fixed_cnt)范围内的路径点速度都为fixed_vel，随后根据动力学公式 v 2 = v 1 2 + 2 a x v_2 = \sqrt{{v_1}^2 + 2ax} v2=v12+2ax（加速情况）修正后面路径点的速度。
autoware_msgs::Lane apply_acceleration(const autoware_msgs::Lane& lane, double acceleration, size_t start_index,
size_t fixed_cnt, double fixed_vel)
{
autoware_msgs::Lane l = lane;
if (fixed_cnt == 0)
return l;
double square_vel = fixed_vel * fixed_vel;
double distance = 0;
for (size_t i = start_index; i < l.waypoints.size(); ++i)//从下标为start_index的路径点开始遍历l.waypoints。
{
if (i - start_index < fixed_cnt)
{
l.waypoints[i].twist.twist.linear.x = fixed_vel;//令下标范围在[start_index，start_index+fixed_cnt)范围内的路径点速度都为fixed_vel。（防盗标记：zhengkunxian）
continue;
}
//下面的代码对下标在[start_index+fixed_cnt，l.waypoints.size())范围内的路径点实施。
geometry_msgs::Point a = l.waypoints[i - 1].pose.pose.position;
geometry_msgs::Point b = l.waypoints[i].pose.pose.position;
distance += hypot(b.x - a.x, b.y - a.y);
double v = sqrt(square_vel + 2 * acceleration * distance);
if (v < l.waypoints[i].twist.twist.linear.x)
l.waypoints[i].twist.twist.linear.x = v;
else
break;//此函数适用于加速情况，而且默认储存的路径点速度为匀速，因此只要v大于某一路径点速度，后面的便不用再修正。
}
return l;
}
3.5 create_reduction函数
根据当前路径点所处Lane是否是弯道或者回旋道等情况确定速度降低率。
double create_reduction(const lane_planner::vmap::VectorMap& fine_vmap, int index)
{
const vector_map::DTLane& dtlane = fine_vmap.dtlanes[index];
if (lane_planner::vmap::is_straight_dtlane(dtlane))//如果是直道，保持原速。
return 1;
if (lane_planner::vmap::is_curve_dtlane(dtlane))//如果是弯道，需要根据情况降低车速。
{
if (lane_planner::vmap::is_crossroad_dtlane(dtlane))//交叉路口？
return lane_planner::vmap::compute_reduction(dtlane, crossroad_radius_min * crossroad_weight);
if (lane_planner::vmap::is_connection_dtlane(fine_vmap, index))//判断当前的弯道是是否用来连接直道，
//包括（1）弯道转直道；(2)两条直道间用来互相连接的弯道。
return 1;
return lane_planner::vmap::compute_reduction(dtlane, curve_radius_min * curve_weight);
}
if (lane_planner::vmap::is_clothoid_dtlane(dtlane))//如果是回旋道，需要降低车速。
return lane_planner::vmap::compute_reduction(dtlane, clothoid_radius_min * clothoid_weight);
return 1;
}
3.5.1 is_straight_dtlane函数
跟前面的is_curve_dtlane，is_crossroad_dtlane和is_clothoid_dtlane函数一样，根据地图内存的道路结构性数据做判断。
bool is_straight_dtlane(const vector_map::DTLane& dtlane)
{
return (dtlane.apara == 0 && dtlane.r == RADIUS_MAX);//半径无限大就意味着是直线。
}
3.5.2 compute_reduction函数
根据下式计算速度降低率 p p p：
p = 1 − w r . p=1-\frac{w}{r}. p=1−rw.
其中 w w w是传入函数的曲线权重， w w w是弯道半径。
double compute_reduction(const vector_map::DTLane& d, double w)
{
return (1 - fabs(1 / d.r) * w); // 0～1
}
3.5.3 is_connection_dtlane函数
判断当前的弯道是是否用来连接直道，包括（1）弯道转直道；(2)两条直道间用来互相连接的弯道。
bool is_connection_dtlane(const VectorMap& fine_vmap, int index)
{
const vector_map::DTLane& dtlane = fine_vmap.dtlanes[index];
int size = fine_vmap.dtlanes.size();
int change = 0;
int straight = 0;
for (int i = index - 1; i >= 0; --i) {//以下标index为始，检查前面的路径点
if (dtlane.r != fine_vmap.dtlanes[i].r) {//检查半径是否发生改变
++change;
if (is_straight_dtlane(fine_vmap.dtlanes[i]))//是否变成了直道
++straight;
break;//一旦发生变化就跳出循环
}
}
for (int i = index + 1; i < size; ++i) {//以下标index为始，检查后面的路径点
if (dtlane.r != fine_vmap.dtlanes[i].r) {
++change;
if (is_straight_dtlane(fine_vmap.dtlanes[i]))
++straight;
break;
}
}
if (change == 1 && straight == 1)//弯道转直道？
return true;
if (straight == 2)//两条直道中间的弯道连接？
return true;
return false;
}
3.6 create_waypoint_follower_dtlane函数
创建一个新的vector_map::DTLane类型数据，拷贝传入函数的vector_map::DTLane vd中除了did和pid外的属性数据。
autoware_msgs::DTLane create_waypoint_follower_dtlane(const vector_map::DTLane& vd)
{
autoware_msgs::DTLane wd;
wd.dist = vd.dist;
wd.dir = vd.dir;
wd.apara = vd.apara;
wd.r = vd.r;
wd.slope = vd.slope;
wd.cant = vd.cant;
wd.lw = vd.lw;
wd.rw = vd.rw;
return wd;
}
3.7 apply_crossroad_acceleration函数
apply_crossroad_acceleration函数的作用与实现流程大致与前面的apply_stopline_acceleration函数一样，只不过这里速度修正的中心点由前面的停止点换成了交叉口的起始点或末尾点。
autoware_msgs::Lane apply_crossroad_acceleration(const autoware_msgs::Lane& lane, double acceleration)
{
autoware_msgs::Lane l = lane;
//--------------------确定交叉口开始和结束的路径点下标--------------------
bool crossroad = false;
std::vector<size_t> start_indexes;
std::vector<size_t> end_indexes;
for (size_t i = 0; i < l.waypoints.size(); ++i)//遍历l中的路径点
{
vector_map::DTLane dtlane = lane_planner::vmap::create_vector_map_dtlane(l.waypoints[i].dtlane);
if (i == 0)
{
crossroad = lane_planner::vmap::is_crossroad_dtlane(dtlane);
continue;
}
if (crossroad)//前一个路径点处于交叉口
{
if (!lane_planner::vmap::is_crossroad_dtlane(dtlane))//当前路径点不处于交叉口
{
end_indexes.push_back(i - 1);//记录前一个路径点的下标至end_indexes
crossroad = false;
}
}
else//前一个路径点不处于交叉口
{
if (lane_planner::vmap::is_crossroad_dtlane(dtlane))//当前路径点处于交叉口
{
start_indexes.push_back(i);//记录前一个路径点的下标至start_indexes
crossroad = true;
}
}
}
if (start_indexes.empty() && end_indexes.empty())
return l;
for (const size_t i : end_indexes)
l = apply_acceleration(l, acceleration, i, 1, l.waypoints[i].twist.twist.linear.x);
//修正l中路径点的速度，首先固定l内下标在[i，i+1)范围内的路径点速度都为l.waypoints[i].twist.twist.linear.x，
//随后根据加速情况下的动力学公式修正后面路径点的速度。
//总体而言，对于作为交叉口末尾点的路径点令车保持原速，再后面的路径点根据加速离开交叉口末尾点修正原来的车速。
//---------和apply_stopline_acceleration函数内同样的套路，反转l.waypoints以加速离开过程替代减速接近过程---------（防盗标记：zhengkunxian）
std::reverse(l.waypoints.begin(), l.waypoints.end());
std::vector<size_t> reverse_start_indexes;
for (const size_t i : start_indexes)
reverse_start_indexes.push_back(l.waypoints.size() - i - 1);
std::reverse(reverse_start_indexes.begin(), reverse_start_indexes.end());
for (const size_t i : reverse_start_indexes)
l = apply_acceleration(l, acceleration, i, 1, l.waypoints[i].twist.twist.linear.x);
std::reverse(l.waypoints.begin(), l.waypoints.end());
return l;
}
create_vector_map_dtlane函数
create_vector_map_dtlane函数的作用及实现流程与前面的create_waypoint_follower_dtlane函数是一样的。
vector_map::DTLane create_vector_map_dtlane(const autoware_msgs::DTLane& wd)
{
vector_map::DTLane vd;
vd.dist = wd.dist;
vd.dir = wd.dir;
vd.apara = wd.apara;
vd.r = wd.r;
vd.slope = wd.slope;
vd.cant = wd.cant;
vd.lw = wd.lw;
vd.rw = wd.rw;
return vd;
}
3.8 apply_stopline_acceleration(const autoware_msgs::Lane& lane, double acceleration, const lane_planner::vmap::VectorMap& fine_vmap, size_t ahead_cnt, size_t behind_cnt)函数
本函数与前面apply_stopline_acceleration(const autoware_msgs::Lane& lane, double acceleration,double stopline_search_radius, size_t ahead_cnt, size_t behind_cnt)函数的作用是一样的：对车速进行修正，以停车点为中心修正附近路径点车速为0，接着运用匀减速/匀加速动力学方程修正匀减速接近/匀加速离开停车点过程中的车速（防盗标记：zhengkunxian）。但是中间流程相较于前一个apply_stopline_acceleration函数更简单些，其可以直接查询行车路径上的停止点，而不必去全局矢量地图lane_vmap中遍历搜索。
autoware_msgs::Lane apply_stopline_acceleration(const autoware_msgs::Lane& lane, double acceleration,
const lane_planner::vmap::VectorMap& fine_vmap, size_t ahead_cnt,
size_t behind_cnt)
{
autoware_msgs::Lane l = lane;
std::vector<size_t> indexes;
for (size_t i = 0; i < fine_vmap.stoplines.size(); ++i)//直接遍历行车矢量地图fine_vmap中的停止线即可
{
if (fine_vmap.stoplines[i].id >= 0)
indexes.push_back(i);
}
if (indexes.empty())
return l;
//----------------------后面的过程与前面一个apply_stopline_acceleration函数是一致的---------------
for (const size_t i : indexes)
l = apply_acceleration(l, acceleration, i, behind_cnt + 1, 0);
std::reverse(l.waypoints.begin(), l.waypoints.end());
std::vector<size_t> reverse_indexes;
for (const size_t i : indexes)
reverse_indexes.push_back(l.waypoints.size() - i - 1);
std::reverse(reverse_indexes.begin(), reverse_indexes.end());
for (const size_t i : reverse_indexes)
l = apply_acceleration(l, acceleration, i, ahead_cnt + 1, 0);
std::reverse(l.waypoints.begin(), l.waypoints.end());
return l;
}