《自动驾驶编程原理与应用》课件 第7章决策规划

1主讲：黄炜自动驾驶编程原理与应用2第七章决策规划知识点：了解主流轨迹规划方法；掌握自动驾驶系统决策规划功能包相关功能、实现原理和使用方法。重点：重点掌握基于A-Star和规则的轨迹规划方法，以及Autoware决策规划模块中ROS节点astar_avoid等。难点：决策规划功能模块与示例节点源码解析。自动驾驶编程原理与应用3引言决策规划作为自动驾驶的“大脑”，其主要功能是基于车辆定位和环境感知等信息合理地决策自动驾驶车辆的行为（比如停车、超车、变道、跟驰等），然后根据不同行为决策结果确定轨迹规划的约束条件，并规划生成满足约束条件的车速和路径。4引言根据对环境信息的掌握程度不同，速度和路径规划可分为基于全局地图先验信息的全局规划和基于局部传感器信息的局部规划。轨迹规划行为决策速度规划路径规划全局规划局部规划全局规划局部规划决策规划5引言全局规划是基于地图中具体的道路网络数据与交通信息规划出连接起始位置与目标位置的理想最优路径以及对应场景的车辆速度，其中结构化道路下的全局路径规划考虑的因素可能包括选择最近的道路、避开拥堵路段等，全局速度规划主要考虑的因素可能包括因素包括交通灯的状态、车辆与交通灯之间的距离、车辆的当前速度以及交通规则等。自动驾驶车辆在按照全局规划行驶的过程中会与不同交通参与者进行交互，因此需要自动驾驶系统根据局部传感器实时感知的交通环境信息进行相应的行为决策和轨迹规划。其中，轨迹规划的输出结果通常是由路径规划和速度规划两部分规划结果所构成的车辆位置的时空序列。6本章节目录7.1决策规划方法概述常见决策规划方法简介Autoware决策规划基本步骤7.2全局规划功能包Rule-Based速度规划方法节点lane_rule与源码解析7.3局部规划功能包A-Star路径规划方法节点astar_avoid与源码解析77.1决策规划方法概述87.1决策规划方法概述常见决策规划方法简介常见的行为决策方法有限状态机（FiniteStateMachine）决策树（DecisionTree）深度学习（DeepLearning）强化学习（ReinforcementLearning）等。目前在自动驾驶行为决策中应用较为广泛的方法是有限状态机，其通过预先设计逻辑规则的方式可以实现交通场景辨识和对应行为选择。行为决策方法97.1决策规划方法概述常见决策规划方法简介有限状态机是一种描述有限数量状态以及在这些状态之间的转移和动作等行为的数学模型，通过状态图表示时可以看作是由有限状态节点和表示状态转移逻辑的有向边所构成的有向图。有限状态机一般由当前所处状态（现态）、状态转移条件（条件）、状态转移中执行的动作（动作）、动作后的状态（次态）等元素构成，其中动作并不是每次状态转移时都必需的元素，当条件满足后，允许不执行任何动作直接转移至次态。有限状态机模型一般采用状态转移图进行表示。行为决策方法107.1决策规划方法概述常见决策规划方法简介以下图有限状态机模型为例，当现态为“状态I”时，若满足“条件I”，则执行“动作I”转移至次态“状态II”；同理，当现态为“状态II”时，若满足“条件II”，则执行“动作II”转移至次态“状态I”。行为决策方法117.1决策规划方法概述常见决策规划方法简介为了实现交通场景辨识和对应行为选择的集成，通常会在一个状态（一种交通场景描述）内部进一步设计对应状态机（对应行为行为决策方法选择），形成外层负责交通场景辨识、内层负责行为选择的分层有限状态机（HierarchicalFiniteStateMachine）结构。127.1决策规划方法概述常见决策规划方法简介基于分层有限状态机的行为决策方法的优势：允许针对不同场景进行并行开发；方便在开发过程中修正问题以及增设场景和行为；逻辑性和可解释性强，有助于分解复杂决策任务、保障决策的可靠性。行为决策方法137.1决策规划方法概述常见决策规划方法简介基于分层有限状态机的行为决策方法的不足：实际的自动驾驶场景千变万化，使用有限的交通场景描述和对应行为响应去覆盖所有的场景使用较为困难，同时该方法对于未知不安全场景的处理能力较为有限。该方法将自动驾驶场景切分成了一系列离散的交通场景进行辨识，可能会出现场景间转换过程突兀的问题，转换过程所产生的不连续或是不可预测的行为决策结果可能会导致自动驾驶行为看起来不够自然或是与人类的驾驶习惯不相符。行为决策方法147.1决策规划方法概述常见决策规划方法简介随着人工智能技术的发展，深度学习和强化学习等一些前沿算法的应用使得自动驾驶行为决策能力得到了一定程度的提升，能够更好地适应复杂变化的自动驾驶场景。虽然人工智能技术在行为决策方面弥补了前述有限状态机的一些不足，但是行为决策方法目前其开发过程仍然需要大量数据作为学习样本，且对训练数据质量有非常高的要求；本身存在内在机理不清晰（逻辑性和可解释性不足）、边界条件不确定（可靠性不足）等黑箱固有问题也不容忽视。157.1决策规划方法概述常见决策规划方法简介通常在轨迹规划过程中会先进行路径规划，再基于场景划分对路径规划生成的轨迹点进行相应的速度规划。经典的路径规划方法有图搜索算法（GraphSearchAlgorithm）、基于抽样算法（Sampling-basedAlgorithm）、基于优化算法（Optimization-basedAlgorithm）、曲线插值法（CurveInterpolationMethod）、人工势场法（ArtificialPotentialFieldMethod）等，近年来还有一些学者提出采用前沿的机器和深度学习算法（MachineandDeepLearningAlgorithm）以及元启发式优化算法（Meta-heuristicOptimizationAlgorithm）等可实现路径规划的方法。轨迹规划方法167.1决策规划方法概述常见决策规划方法简介比较常见的图搜索算法有Dijkstra算法和A-Star算法，其基本思想都是将状态空间通过确定的方式离散成一个图，再利用各种启发式图搜索算法搜索可行解乃至最优解。轨迹规划方法A-Star算法177.1决策规划方法概述常见决策规划方法简介基于抽样的路径规划算法有概率路线图算法（ProbabilisticRoad轨迹规划方法Map,PRM）、快速遍历随机树算法（Rapidly-exploringRandomTree,RRT）等，其通过在连续状态空间中逐步或批量抽样，再构建由离散状态空间样本连接的树或图，捕捉解空间的连通性，从而得出可行解或最优解。RRT算法187.1决策规划方法概述常见决策规划方法简介图搜索算法和抽样算法都是属于静态的路径规划方法，当自动驾驶环境发生变化时，静态规划方法只能重新寻找新的最优路径，导致原本规划的路径与新规划的路径之间缺乏平滑的过渡。因此在实际工程应用中，通常将笛卡尔坐标系（CartesianFrame）下的路径规划问题转化为Frenet坐标系下的优化问题进行动态规划以避免出现突兀的路径变化。轨迹规划方法197.1决策规划方法概述常见决策规划方法简介笛卡尔坐标系可以与Frenet坐标系互相转换，Frenet坐标系中任意坐标(s,d)表示该点到期望路径投影点的距离为d，沿期望路径投影轨迹规划方法点到原点坐标的距离为s。在Frenet坐标系下可以将车辆的纵向和横向运动进行解耦，从而有利于简化车辆的轨迹规划问题和运动控制问题。207.1决策规划方法概述常见决策规划方法简介优化算法通常利用目标函数的最大化或最小化对最优路径规划问题进行描述，再采用数值优化的方法对其在线求解，其中如何降低求解过程计算复杂度、提升计算速度仍是目前该方法研究的重点。轨迹规划方法217.1决策规划方法概述常见决策规划方法简介曲线插值法是指基于先验路点拟合生成一条满足约束条件平滑路轨迹规划方法径的方法，常用的曲线有回旋曲线、B样条曲线、多项式曲线等。虽然通过曲线插值算法获得的路径具备良好的平滑性和连续性，但是与其他方法相比所需的计算时间较长。227.1决策规划方法概述常见决策规划方法简介人工势场法是一种将目标位置与车辆之间的相互作用视为引力，障碍物与车辆之间的相互作用视为斥力，通过建立引力场和斥力场的势场函数对路径进行寻优的方法。采用人工势场法规划的路径简单平滑，但是该方法基于梯度下降可能出现局部最优的问题。轨迹规划方法23Autoware决策规划基本步骤（1）全局规划以运行官方示例数据为例，在终端输入以下命令开始全局规划：$cdautoware.ai$sourceinstall/setup.bash$roslaunchautoware_quickstart_examplesmy_mission_planning.launch7.1决策规划方法概述24Autoware决策规划基本步骤（1）全局规划为了查看启动文件my_mission_planning.launch的文本内容，在终端输入以下命令，在官方示例autoware_quickstart_examples路径下找到工作目录“./launch/rosbag_demo”，然后在rosbag_demo文件夹中打开启动文件my_mission_planning.launch：$cdautoware.ai/src/autoware/documentation/autoware_quickstart_examples$cdlaunch/rosbag_demo$geditmy_mission_planning.launch7.1决策规划方法概述<launch><!--settingpathparameter--><argname="topic_pose_stamped"default="/ndt_pose"/><argname="topic_twist_stamped"default="/estimate_twist"/><!--TabletUI--><!--<includefile="$(findruntime_manager)/launch_files/tablet_socket.launch"/>-->……<includefile="$(findwaypoint_maker)/launch/waypoint_loader.launch"><argname="load_csv"value="true"/><argname="multi_lane_csv"value="/home/autoware/.autoware/data/path/moriyama_path.txt"/></include><!--lane_navi--><!--<nodepkg="lane_planner"type="lane_navi"name="lane_navi"/>--><!--lane_rule--><nodepkg="lane_planner"type="lane_rule"name="lane_rule"/><!--lane_stop--><nodepkg="lane_planner"type="lane_stop"name="lane_stop"/><!--lane_select--><nodepkg="lane_planner"type="lane_select"name="lane_select"/></launch>25Autoware决策规划基本步骤（1）全局规划my_mission_planning.launch7.1决策规划方法概述启动文件通过导入多个启动文件到当前文件以及调用“lane_navi”、“lane_rule”等多个节点实现轨迹规划过程中所需全局速度规划等相关功能的调用。26Autoware决策规划基本步骤（2）局部规划使用快捷键Shift+Ctrl+T在终端新建标签，在新标签中输入以下命令开始局部规划：$cdautoware.ai$sourceinstall/setup.bash$roslaunchautoware_quickstart_examplesmy_motion_planning.launch7.1决策规划方法概述27Autoware决策规划基本步骤（2）局部规划为了查看启动文件my_motion_planning.launch的文本内容，在终端输入以下命令，在官方示例autoware_quickstart_examples路径下找到工作目录“./launch/rosbag_demo”，然后在rosbag_demo文件夹中打开启动文件my_motion_planning.launch：$cdautoware.ai/src/autoware/documentation/autoware_quickstart_examples$cdlaunch/rosbag_demo$geditmy_motion_planning.launch7.1决策规划方法概述<launch><!--VehicleContorl--><includefile="$(findruntime_manager)/launch_files/vehicle_socket.launch"/><!--obstacle_avoid--><includefile="$(findwaypoint_planner)/launch/astar_avoid.launch"/><!--velocity_set--><includefile="$(findwaypoint_planner)/launch/velocity_set.launch"/><!--pure_pursuit--><nodepkg="rostopic"type="rostopic"name="config_waypoint_follower_rostopic"args="pub-l/config/waypoint_followerautoware_config_msgs/ConfigWaypointFollower'{header:auto,param_flag:1,velocity:5.0,lookahead_distance:4.0,lookahead_ratio:2.0,minimum_lookahead_distance:6.0,displacement_threshold:0.0,relative_angle_threshold:0.0}'"/><includefile="$(findwaypoint_follower)/launch/pure_pursuit.launch"/><!--twist_filter--><includefile="$(findwaypoint_follower)/launch/twist_filter.launch"/></launch>28Autoware决策规划基本步骤（2）局部规划my_mission_planning.launch7.1决策规划方法概述启动文件主要通过导入“astar_avoid.launch”等多个启动文件到当前文件实现局部规划过程中所需避障规划等相关功能的调用。297.2全局规划功能包30Rule-Based速度规划方法速度规划作为轨迹时空解耦规划中的一个维度，本质是在规划好的路径上对车辆移动速度进行规划，决策车辆在路径上的移动行为。7.2全局规划功能包31Rule-Based速度规划方法基于规则（Rule-Based）的方法是一种根据预定义规则集以及获取的交通环境信息和车辆自身状态信息作决策或推理出相应速度行为的系统设计方法，核心是通过明确的规则处理输入信息并输出行为决策。在定义规则集时，全局的速度规划一般主要考虑交通规则的约束（包括交通信号灯、限速、停车让行等），而局部的速度规划则一般主要考虑如何避免与交通参与者、障碍物等发生碰撞以及乘坐舒适等。7.2全局规划功能包32节点lane_rule与源码解析（1）主函数流程解析在VSCode软件中打开文件夹autoware.ai下的代码空间src，然后在侧边栏中依次点击文件夹autoware下的子文件夹core_planning找到功能包lane_planner，选择节点文件夹nodes中的节点lane_rule，最后双击打开源文件lane_rule.cpp。源文件lane_rule.cpp中主函数的流程如图所示。7.2全局规划功能包33节点lane_rule与源码解析（1）主函数流程解析参数获取……intmain(intargc,char**argv){……intsub_vmap_queue_size;n.param<int>("/lane_rule/sub_vmap_queue_size",sub_vmap_queue_size,1);intsub_waypoint_queue_size;n.param<int>("/lane_rule/sub_waypoint_queue_size",sub_waypoint_queue_size,1);intsub_config_queue_size;n.param<int>("/lane_rule/sub_config_queue_size",sub_config_queue_size,1);intpub_waypoint_queue_size;n.param<int>("/lane_rule/pub_waypoint_queue_size",pub_waypoint_queue_size,1);boolpub_waypoint_latch;n.param<bool>("/lane_rule/pub_waypoint_latch",pub_waypoint_latch,true);intpub_marker_queue_size;n.param<int>("/lane_rule/pub_marker_queue_size",pub_marker_queue_size,10);boolpub_marker_latch;n.param<bool>("/lane_rule/pub_marker_latch",pub_marker_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);……}lane_rule.cpp文本中主函数的参数获取部分通过直接调用ros::param函数来访问参数服务器上的参数，并将其赋值给相应的变量，包括sub_vmap_queue_size、sub_waypoint_queue_size、sub_config_queue_size、pub_waypoint_queue_size、pub_waypoint_latch、pub_marker_queue_size、pub_marker_latch、pub_marker_queue_size、pub_marker_latch、waypoint_max、search_radius、curve_weight等。如果上述变量在参数服务器中没有找到对应的参数，则自动设置为默认值。7.2全局规划功能包……intmain(intargc,char**argv){……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);marker_pub=n.advertise<visualization_msgs::Marker>("/waypoint_debug",pub_marker_queue_size,pub_marker_latch);ros::Subscriberwaypoint_sub=n.subscribe("/lane_waypoints_array",sub_waypoint_queue_size,create_waypoint);ros::Subscriberpoint_sub=n.subscribe("/vector_map_info/point",sub_vmap_queue_size,cache_point);ros::Subscriberlane_sub=n.subscribe("/vector_map_info/lane",sub_vmap_queue_size,cache_lane);ros::Subscribernode_sub=n.subscribe("/vector_map_info/node",sub_vmap_queue_size,cache_node);ros::Subscriberstopline_sub=n.subscribe("/vector_map_info/stop_line",sub_vmap_queue_size,cache_stopline);ros::Subscriberdtlane_sub=n.subscribe("/vector_map_info/dtlane",sub_vmap_queue_size,cache_dtlane);ros::Subscriberconfig_sub=n.subscribe("/config/lane_rule",sub_config_queue_size,config_parameter);ros::spin();returnEXIT_SUCCESS;}节点lane_rule与源码解析（1）主函数流程解析lane_rule.cpp文本中主函数的话题发布和订阅部分发布和订阅通过查找源文件lane_rule.cpp中主函数发布和订阅的话题，可以快速确认节点lane_rule.cpp的输出和输入信息。7.2全局规划功能包3435节点lane_rule与源码解析（1）主函数流程解析在主函数中发布的话题有“/traffic_waypoints_array”、“/red_waypoints_array”、“/green_waypoints_array”、“/waypoint_debug”等，而订阅的话题则有“/lane_waypoints_array”、“/vector_map_info/point”、“/vector_map_info/lane”、“/vector_map_info/node”、“/vector_map_info/stop_line”、“/vector_map_info/dtlane”、“/config/lane_rule”等。其中，订阅话题时会通过回调函数（如create_waypoint函数、cache_point函数、cache_lane函数、cache_node函数、cache_stopline函数、cache_dtlane函数、config_parameter函数等）处理接收到的话题信息。发布和订阅7.2全局规划功能包36节点lane_rule与源码解析（2）关键子函数解析create_waypoint函数主要功能是通过订阅包含节点lane_navi搜索到的通往目的地可行路径相关信息的话题/lane_waypoints_array，进一步修正规划路径上的轨迹点速度。……intmain(intargc,char**argv){……ros::Subscriberwaypoint_sub=n.subscribe("/lane_waypoints_array",sub_waypoint_queue_size,create_waypoint);ros::Subscriberpoint_sub=n.subscribe("/vector_map_info/point",sub_vmap_queue_size,cache_point);ros::Subscriberlane_sub=n.subscribe("/vector_map_info/lane",sub_vmap_queue_size,cache_lane);ros::Subscribernode_sub=n.subscribe("/vector_map_info/node",sub_vmap_queue_size,cache_node);ros::Subscriberstopline_sub=n.subscribe("/vector_map_info/stop_line",sub_vmap_queue_size,cache_stopline);ros::Subscriberdtlane_sub=n.subscribe("/vector_map_info/dtlane",sub_vmap_queue_size,cache_dtlane);ros::Subscriberconfig_sub=n.subscribe("/config/lane_rule",sub_config_queue_size,config_parameter);……}lane_rule.cpp文本中主函数的话题发布和订阅部分create_waypoint函数7.2全局规划功能包37节点lane_rule与源码解析（2）关键子函数解析……namespace{……std::stringframe_id;lane_planner::vmap::VectorMapall_vmap;autoware_msgs::LaneArraycached_waypoint;……}……voidcreate_waypoint(constautoware_msgs::LaneArray&msg){std_msgs::Headerheader;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(constautoware_msgs::Lane&l:msg.lanes)cached_waypoint.lanes.push_back(create_new_lane(l,header));if(all_vmap.points.empty()||all_vmap.lanes.empty()||all_vmap.nodes.empty()||all_vmap.stoplines.empty()||all_vmap.dtlanes.empty()){traffic_pub.publish(cached_waypoint);return;}autoware_msgs::LaneArraytraffic_waypoint;autoware_msgs::LaneArrayred_waypoint;autoware_msgs::LaneArraygreen_waypoint;traffic_waypoint.id=red_waypoint.id=green_waypoint.id=msg.id;……}……lane_rule.cpp文本中create_waypoint函数Part1create_waypoint函数接收数据类型为autoware_msgs::LaneArray的消息msg作为输入，获取节点lane_navi发布的可行路径相关信息。7.2全局规划功能包create_waypoint函数38节点lane_rule与源码解析（2）关键子函数解析……namespace{……std::stringframe_id;lane_planner::vmap::VectorMapall_vmap;autoware_msgs::LaneArraycached_waypoint;……}……voidcreate_waypoint(constautoware_msgs::LaneArray&msg){std_msgs::Headerheader;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(constautoware_msgs::Lane&l:msg.lanes)cached_waypoint.lanes.push_back(create_new_lane(l,header));if(all_vmap.points.empty()||all_vmap.lanes.empty()||all_vmap.nodes.empty()||all_vmap.stoplines.empty()||all_vmap.dtlanes.empty()){traffic_pub.publish(cached_waypoint);return;}autoware_msgs::LaneArraytraffic_waypoint;autoware_msgs::LaneArrayred_waypoint;autoware_msgs::LaneArraygreen_waypoint;traffic_waypoint.id=red_waypoint.id=green_waypoint.id=msg.id;……}……lane_rule.cpp文本中create_waypoint函数Part1创建数据类型为std_msgs::Header的对象header，并使用ros::Time::now()将其时间戳设置为当前时间，帧ID设置为frame_id。7.2全局规划功能包create_waypoint函数39节点lane_rule与源码解析（2）关键子函数解析……namespace{……std::stringframe_id;lane_planner::vmap::VectorMapall_vmap;autoware_msgs::LaneArraycached_waypoint;……}……voidcreate_waypoint(constautoware_msgs::LaneArray&msg){std_msgs::Headerheader;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(constautoware_msgs::Lane&l:msg.lanes)cached_waypoint.lanes.push_back(create_new_lane(l,header));if(all_vmap.points.empty()||all_vmap.lanes.empty()||all_vmap.nodes.empty()||all_vmap.stoplines.empty()||all_vmap.dtlanes.empty()){traffic_pub.publish(cached_waypoint);return;}autoware_msgs::LaneArraytraffic_waypoint;autoware_msgs::LaneArrayred_waypoint;autoware_msgs::LaneArraygreen_waypoint;traffic_waypoint.id=red_waypoint.id=green_waypoint.id=msg.id;……}……lane_rule.cpp文本中create_waypoint函数Part1清空数据类型为autoware_msgs::LaneArray的消息cached_waypoint中的lanes并释放内存，同时将cached_waypoint的ID设置为输入消息msg的ID。7.2全局规划功能包create_waypoint函数40节点lane_rule与源码解析（2）关键子函数解析……namespace{……std::stringframe_id;lane_planner::vmap::VectorMapall_vmap;autoware_msgs::LaneArraycached_waypoint;……}……voidcreate_waypoint(constautoware_msgs::LaneArray&msg){std_msgs::Headerheader;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(constautoware_msgs::Lane&l:msg.lanes)cached_waypoint.lanes.push_back(create_new_lane(l,header));if(all_vmap.points.empty()||all_vmap.lanes.empty()||all_vmap.nodes.empty()||all_vmap.stoplines.empty()||all_vmap.dtlanes.empty()){traffic_pub.publish(cached_waypoint);return;}autoware_msgs::LaneArraytraffic_waypoint;autoware_msgs::LaneArrayred_waypoint;autoware_msgs::LaneArraygreen_waypoint;traffic_waypoint.id=red_waypoint.id=green_waypoint.id=msg.id;……}……lane_rule.cpp文本中create_waypoint函数Part1使用for循环语句遍历输入消息msg中代表每一条可行车道的Lane对象l，通过调用create_new_lane函数更新Lane对象l所有元素的header，并将其添加到cached_waypoint.lanes。7.2全局规划功能包create_waypoint函数41节点lane_rule与源码解析（2）关键子函数解析……namespace{……std::stringframe_id;lane_planner::vmap::VectorMapall_vmap;autoware_msgs::LaneArraycached_waypoint;……}……voidcreate_waypoint(constautoware_msgs::LaneArray&msg){std_msgs::Headerheader;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(constautoware_msgs::Lane&l:msg.lanes)cached_waypoint.lanes.push_back(create_new_lane(l,header));if(all_vmap.points.empty()||all_vmap.lanes.empty()||all_vmap.nodes.empty()||all_vmap.stoplines.empty()||all_vmap.dtlanes.empty()){traffic_pub.publish(cached_waypoint);return;}autoware_msgs::LaneArraytraffic_waypoint;autoware_msgs::LaneArrayred_waypoint;autoware_msgs::LaneArraygreen_waypoint;traffic_waypoint.id=red_waypoint.id=green_waypoint.id=msg.id;……}……lane_rule.cpp文本中create_waypoint函数Part1判断数据类型为lane_planner::vmap::VectorMap的矢量地图all_vmap是否有元素（点、车道、节点、停止线和车道线）为空，当判断语句为真时，直接发布消息到话题“/traffic_waypoints_array”。7.2全局规划功能包create_waypoint函数42节点lane_rule与源码解析（2）关键子函数解析……namespace{……std::stringframe_id;lane_planner::vmap::VectorMapall_vmap;autoware_msgs::LaneArraycached_waypoint;……}……voidcreate_waypoint(constautoware_msgs::LaneArray&msg){std_msgs::Headerheader;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(constautoware_msgs::Lane&l:msg.lanes)cached_waypoint.lanes.push_back(create_new_lane(l,header));if(all_vmap.points.empty()||all_vmap.lanes.empty()||all_vmap.nodes.empty()||all_vmap.stoplines.empty()||all_vmap.dtlanes.empty()){traffic_pub.publish(cached_waypoint);return;}autoware_msgs::LaneArraytraffic_waypoint;autoware_msgs::LaneArrayred_waypoint;autoware_msgs::LaneArraygreen_waypoint;traffic_waypoint.id=red_waypoint.id=green_waypoint.id=msg.id;……}……lane_rule.cpp文本中create_waypoint函数Part1对数据类型为autoware_msgs::LaneArray的普通路径点数组traffic_waypoint、红灯路径点数组red_waypoint和绿灯路径点数组green_waypoint进行初始化，并将它们的ID设置为输入消息msg的ID。7.2全局规划功能包create_waypoint函数43节点lane_rule与源码解析（2）关键子函数解析……int32pidfloat64bfloat64lfloat64hfloat64bxfloat64ly……Point.msg文本矢量地图all_vmap使用数据类型lane_planner::vmap::VectorMap储存决策规划所需的部分矢量图信息，包括点Point，车道Lane，节点Node，停止线Stopline等要素。相关要素的定义可查看“~/autoware.ai/src/autoware/messages/vector_map_msgs/msg”目录下对应的消息文件Point.msg，Lane.msg，Node.msg，Stopline.msg，DTLane.msg（包含车道线性元素和坡度信息等）。消息文件Point.msg中主要包含了激光点云中每一个点的识别编号pid，经度b，纬度l，高度h，平面坐标系下的横坐标bx和纵坐标ly等信息。消息文件Lane.msg中主要包含了每一条车道的识别编号lnid，起始Node的识别编号bnid，终点Node的识别编号fnid，当前车道分支/合并模式（0-正常；1-分支到左边；2-分支到右边；3-合并到左车道；4-合并到右车道；5-未知），合并到当前车道的车道识别编号blid2、blid3、blid4，当前车道分支出去的车道识别编号flid2、flid3、flid4等信息。7.2全局规划功能包create_waypoint函数……int32lnid……int32bnidint32fnidint32jctint32blid2int32blid3int32blid4int32flid2int32flid3int32flid4……Lane.msg文本44节点lane_rule与源码解析（2）关键子函数解析在create_waypoint函数的初始化部分，使用create_new_lane函数通过for循环语句将pose.header和twist.header更新为传入的header，从而实现对所有路径点的帧头信息的更新。……autoware_msgs::Lanecreate_new_lane(constautoware_msgs::Lane&lane,conststd_msgs::Header&header){autoware_msgs::Lanel=lane;l.header=header;for(autoware_msgs::Waypoint&w:l.waypoints){w.pose.header=header;w.twist.header=header;}returnl;}……lane_rule.cpp文本中create_new_lane函数7.2全局规划功能包create_waypoint函数节点lane_rule与源码解析（2）关键子函数解析……namespace{doubleconfig_acceleration=1;doubleconfig_stopline_search_radius=1;intconfig_number_of_zeros_ahead=0;intconfig_number_of_zeros_behind=0;……intwaypoint_max;doublesearch_radius;……lane_planner::vmap::VectorMaplane_vmap;……}……voidcreate_waypoint(constautoware_msgs::LaneArray&msg){……for(size_ti=0;i<msg.lanes.size();++i){autoware_msgs::Lanelane=create_new_lane(msg.lanes[i],header);lane_planner::vmap::VectorMapcoarse_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::VectorMapfine_vmap=lane_planner::vmap::create_fine_vmap(lane_vmap,lane_planner::vmap::LNO_ALL,coarse_vmap,search_radius,waypoint_max);if(fine_vmap.points.size()<2||!is_fine_vmap(fine_vmap,lane)){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);red_waypoint.lanes.push_back(lane);continue;}……lane_rule.cpp文本中create_waypoint函数Part27.2全局规划功能包create_waypoint函数45使用for循环语句遍历每个车道，通过调用create_new_lane函数更新Lane对象lane所有元素的帧头header生成新的车道信息。节点lane_rule与源码解析（2）关键子函数解析……namespace{doubleconfig_acceleration=1;doubleconfig_stopline_search_radius=1;intconfig_number_of_zeros_ahead=0;intconfig_number_of_zeros_behind=0;……intwaypoint_max;doublesearch_radius;……lane_planner::vmap::VectorMaplane_vmap;……}……voidcreate_waypoint(constautoware_msgs::LaneArray&msg){……for(size_ti=0;i<msg.lanes.size();++i){autoware_msgs::Lanelane=create_new_lane(msg.lanes[i],header);lane_planner::vmap::VectorMapcoarse_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::VectorMapfine_vmap=lane_planner::vmap::create_fine_vmap(lane_vmap,lane_planner::vmap::LNO_ALL,coarse_vmap,search_radius,waypoint_max);if(fine_vmap.points.size()<2||!is_fine_vmap(fine_vmap,lane)){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);red_waypoint.lanes.push_back(lane);continue;}……lane_rule.cpp文本中create_waypoint函数Part27.2全局规划功能包create_waypoint函数46使用create_coarse_vmap_from_lane函数创建一个只有轨迹点的粗略导航地图coarse_vmap。判断粗略导航地图coarse_vmap中的点是否少于2个，当判断语句为真时，则直接将车道信息存入普通路径点数组traffic_waypoint并继续下一个车道。节点lane_rule与源码解析（2）关键子函数解析……namespace{doubleconfig_acceleration=1;doubleconfig_stopline_search_radius=1;intconfig_number_of_zeros_ahead=0;intconfig_number_of_zeros_behind=0;……intwaypoint_max;doublesearch_radius;……lane_planner::vmap::VectorMaplane_vmap;……}……voidcreate_waypoint(constautoware_msgs::LaneArray&msg){……for(size_ti=0;i<msg.lanes.size();++i){autoware_msgs::Lanelane=create_new_lane(msg.lanes[i],header);lane_planner::vmap::VectorMapcoarse_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::VectorMapfine_vmap=lane_planner::vmap::create_fine_vmap(lane_vmap,lane_planner::vmap::LNO_ALL,coarse_vmap,search_radius,waypoint_max);if(fine_vmap.points.size()<2||!is_fine_vmap(fine_vmap,lane)){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);red_waypoint.lanes.push_back(lane);continue;}……lane_rule.cpp文本中create_waypoint函数Part27.2全局规划功能包create_waypoint函数47在只有行车路径上轨迹点数据的粗略导航地图基础上，通过调用create_fine_vmap函数生成具有完整行车路径上道路数据（具体包括当前点、车道线、停止线等信息）的精细导航地图fine_vmap。节点lane_rule与源码解析（2）关键子函数解析……namespace{doubleconfig_acceleration=1;doubleconfig_stopline_search_radius=1;intconfig_number_of_zeros_ahead=0;intconfig_number_of_zeros_behind=0;……intwaypoint_max;doublesearch_radius;……lane_planner::vmap::VectorMaplane_vmap;……}……voidcreate_waypoint(constautoware_msgs::LaneArray&msg){……for(size_ti=0;i<msg.lanes.size();++i){autoware_msgs::Lanelane=create_new_lane(msg.lanes[i],header);lane_planner::vmap::VectorMapcoarse_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::VectorMapfine_vmap=lane_planner::vmap::create_fine_vmap(lane_vmap,lane_planner::vmap::LNO_ALL,coarse_vmap,search_radius,waypoint_max);if(fine_vmap.points.size()<2||!is_fine_vmap(fine_vmap,lane)){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);red_waypoint.lanes.push_back(lane);continue;}……lane_rule.cpp文本中create_waypoint函数Part27.2全局规划功能包create_waypoint函数48通过对比lane中的数据与fine_vmap中储存的数据，使用is_fine_vmap函数判断精细导航地图fine_vmap是否正确生成。节点lane_rule与源码解析（2）关键子函数解析……namespace{doubleconfig_acceleration=1;doubleconfig_stopline_search_radius=1;intconfig_number_of_zeros_ahead=0;intconfig_number_of_zeros_behind=0;……intwaypoint_max;doublesearch_radius;……lane_planner::vmap::VectorMaplane_vmap;……}……voidcreate_waypoint(constautoware_msgs::LaneArray&msg){……for(size_ti=0;i<msg.lanes.size();++i){autoware_msgs::Lanelane=create_new_lane(msg.lanes[i],header);lane_planner::vmap::VectorMapcoarse_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::VectorMapfine_vmap=lane_planner::vmap::create_fine_vmap(lane_vmap,lane_planner::vmap::LNO_ALL,coarse_vmap,search_radius,waypoint_max);if(fine_vmap.points.size()<2||!is_fine_vmap(fine_vmap,lane)){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_