《自动驾驶编程原理与应用》课件 第6章环境感知

1主讲：黄炜自动驾驶编程原理与应用2第六章环境感知知识点：了解主流环境感知传感器技术及其感知信息处理方法；掌握自动驾驶系统环境感知功能包相关功能、实现原理和使用方法。重点：重点掌握欧式聚类算法原理和基于欧式聚类的激光雷达点云目标分割方法，以及Autoware环境感知模块中ROS节点lidar_euclidean_clustering和region_tlr等。难点：环境感知功能模块与示例节点源码解析。自动驾驶编程原理与应用3引言环境感知是指通过各种传感器对自动驾驶车辆周围的交通环境进行动态感知，其根据感知传感器来源的不同可以分为自主式环境感知和协同式环境感知。4引言自主式环境感知是指利用车载传感器（包括超声波雷达、毫米波雷达、视觉传感器、激光雷达等）采集车辆外在属性（例如道路、车辆和行人等）静、动态信息进行识别和理解，协同式环境感知则是利用无线通信技术将边缘节点获得的车辆周边交通环境信息与车载传感器感知信息进行交互融合。5引言常见的环境感知对象包括行驶路径、周围物体等。基于道路类型不同，自动驾驶车辆在结构化道路上识别行驶路径主要依据其对道路交通标线、行车道边缘线、路口导向线、导向车道线、人行横道线、道路出入口标线、道路隔离物等结构化道路元素的理解，而在非结构化道路上识别行驶路径主要依据其对可行驶路径的理解。此外，自动驾驶车辆还需识别自车的周边物体，例如车辆、行人、地面上可能影响汽车通过和安全行驶的其他各种移动或静止物体、各种交通标志和信号灯等，避免发生碰撞。6本章节目录6.1环境感知方法概述常见环境感知方法简介Autoware环境感知基本步骤6.2点云聚类功能包点云欧式聚类方法节点lidar_euclidean_clustering与源码解析6.3交通信号灯识别功能包颜色模型与转换方法节点region_tlr与源码解析76.1环境感知方法概述86.1环境感知方法概述常见环境感知方法简介在环境感知系统中部署不同类型传感器所涉及的信息采集原理和信息处理方式不同，目前比较常见的环境感知方法有超声波雷达感知、毫米波雷达感知、视觉感知以及激光雷达感知等。9常见环境感知方法简介超声波雷达感知的原理是通过发射头发出超声波脉冲感知周围物体，超声波脉冲经媒质（空气）传到障碍物表面，反射后经过媒质（空气）传到接收头，测量超声波脉冲从发射到接收所需的时间，根据媒质中的声速，求得从探头到障碍物表面之间的距离。超声波雷达感知6.1环境感知方法概述10常见环境感知方法简介超声波传感器主要技术指标：测量距离：测量汽车前后障碍物的短距超声波传感器的探测距离一般为15-250cm。测量精度：指传感器测量值与真实值的偏差，测量精度越高，感知信息越可靠。波束角：以传感器中轴线的延长线为轴线，到一侧能量强度减小一半处的角度。工作频率：一般工作频率为40kHz。抗干扰性能：使用环境中的噪声会干扰超声波传感器接收物体反射回来的超声波，要求超声波传感器具有一定的抗干扰能力。6.1环境感知方法概述超声波雷达感知11常见环境感知方法简介因为超声波有一定的扩散角度，所以超声波雷达一般只能用于探测物体的距离，无法用于探测物体的方位，感知范围一般在0.2-10m左右。除此之外，超声波雷达具有以下几个优点：a）结构简单，成本较低；b）对雨、雪、雾霾的穿透力强，可在恶劣天气下工作；c）对光照和色彩不敏感，可用于识别透明以及反射性差的物体；d）不容易受环境电磁场的干扰。6.1环境感知方法概述超声波雷达感知12常见环境感知方法简介毫米波雷达感知的原理是通过发射连续调频信号感知周围物体，当毫米波雷达发射的连续调频信号遇到前方目标时，会产生与发射信号有一定延时的回波，再通过毫米波雷达的混频器进行混频处理，混频后的结果与目标的相对距离和相对速度有关。6.1环境感知方法概述毫米波雷达感知13常见环境感知方法简介超声波属于机械波，是靠介质震动传递，而毫米波属于电磁波，是靠电磁耦合传播。毫米波雷达主要技术指标包括频带、带宽、测量距离、测量精度、最大视角等。6.1环境感知方法概述毫米波雷达感知14常见环境感知方法简介传统的毫米波雷达一般有12路通道（3根发射天线，4根接收天线），由于仅能提供距离、速度、方向角三个维度的信息，所以容易误把道路中静止的井盖、减速带、悬在空中的标识牌等当作是障碍物。6.1环境感知方法概述毫米波雷达感知15常见环境感知方法简介新一代的4D毫米波雷达由多个微波芯片级联，可拥有多达192路通道（12根发射天线，16根接收天线），是传统毫米波雷达的16倍。相比传统毫米波雷达，4D毫米波雷达通过天线合理布置可以精确探测物体的高度信息，从而根据高度信息区分可能影响自动驾驶车辆行驶的物体。毫米波雷达可用于探测物体的距离和方位，感知范围一般在0-100m左右，角分辨率高。此外，毫米波雷达对雨、雪、雾霾的穿透力强，可在恶劣天气下工作，同时对光照和色彩不敏感，可用于识别透明以及反射性差的物体。6.1环境感知方法概述毫米波雷达感知16常见环境感知方法简介视觉感知的原理是通过光学元件和成像装置获取外部环境图像信息，广义的视觉传感器主要由光源、镜头、图像传感器、模数转换器、图像处理器、图像存储器等组成，通常用图像分辨率来描述视觉传感器的性能。6.1环境感知方法概述视觉感知17常见环境感知方法简介视觉传感器主要技术指标：分辨率：指单位长度内可测得的像素点数。帧率：指视觉传感器每秒钟可以拍摄的图像数量。像素尺寸：指视觉传感器每个像素的大小。动态范围：指视觉传感器可以捕捉到的最大和最小光亮度之间的范围。一般而言，选择高动态范围的视觉传感器有利于在较暗环境及明暗差异较大环境下有效感知周围环境的信息，而选择中低像素的视觉传感器则有利于降低硬件计算处理的负担。6.1环境感知方法概述视觉感知18常见环境感知方法简介依据功能的不同，视觉传感器可以分为单目摄像头和双目摄像头。视觉传感器的感知范围一般在3-25m左右，测量精度不受物体表面材质、形状等因素影响。但是，视觉传感器的物理特性和识别原理导致其无法识别没有明显轮廓的物体，并且在低光照和雨、雪、雾霾等恶劣天气的条件下视觉传感器性能受影响较大。6.1环境感知方法概述视觉感知单目摄像头a）单目摄像头可用于行驶路径和周围物体的识别，但是需要依赖大量训练样本并且特征提取过程难以观测调整。19常见环境感知方法简介b）双目摄像头一般用于测量距离，通过对两幅图像视差的计算，依赖精确的三角测距，直接计算出传感器与前方物体之间的距离。6.1环境感知方法概述视觉感知双目摄像头d20常见环境感知方法简介激光雷达感知的原理是通过测算激光发射信号与激光回波信号的往返时间，从而计算出自车与目标之间的距离。6.1环境感知方法概述激光雷达感知21常见环境感知方法简介激光雷达传感器主要技术指标：线数：指激光雷达在垂直方向上能够发射和接收的激光束数量。视场角：指激光雷达在水平和垂直两个方向上所能覆盖的最大视角区域。视场分辨率：指激光雷达在视场角范围内所能感知的最小角度间隔。6.1环境感知方法概述视觉感知22常见环境感知方法简介激光雷达的类型按照有无机械旋转部件可分为机械激光雷达、固态激光雷达和混合激光雷达。6.1环境感知方法概述激光雷达感知固态激光雷达机械激光雷达镜子圆光栅伺服电机激光源圆光栅接收器被测物23常见环境感知方法简介机械激光雷达带有控制激光发射角度的旋转部件，体积较大，价格昂贵，测量精度相对较高，一般置于汽车顶部；固态激光雷达则依靠电子部件来控制激光发射角度，不需机械旋转部件，故尺寸较小，可安装与车体内；混合固态激光雷达工作时，单从外观上是看不到旋转部件的，通过内部玻璃片旋转的方式改变激光光束方向，实现多角度检测的需要。6.1环境感知方法概述激光雷达感知24常见环境感知方法简介激光雷达通过激光束扫描周围环境并收集的目标对象表面密集点的三维坐标，获取车辆周围环境中目标对象的三维点云数据，再利用三维点云数据所反映的几何形状和位置信息可有效识别道路、标志、障碍物等。虽然激光雷达具有方向性好，波束窄，侧角、测距精度高，不受地面杂波干扰等优点，但其具有受大气的光传输效应影响大，遇到浓雾、雨、雪天气可能无法正常工作等缺点。6.1环境感知方法概述激光雷达感知25常见环境感知方法简介车辆自动驾驶过程中仅靠单一传感器通常难以全面描述各种天气情况下的路况环境信息，所以自动驾驶车辆应配置多种不同传感手段获取汽车周边环境不同形式的信息，通过多信息融合对行驶环境进行感知，使自动驾驶车辆具有优良的环境适应能力。6.1环境感知方法概述多传感器融合感知26常见环境感知方法简介通过多传感器融合感知技术，不仅可以互补传感器感知范围提升自动驾驶系统环境感知的覆盖范围，而且在感知范围重叠区域又可以形成冗余校验，从而全面提高环境感知的可靠性与鲁棒性。6.1环境感知方法概述多传感器融合感知27Autoware环境感知基本步骤以运行官方示例数据为例，在终端输入以下命令开始环境感知：$cdautoware.ai$sourceinstall/setup.bash$roslaunchautoware_quickstart_examplesmy_detection.launch6.1环境感知方法概述28Autoware环境感知基本步骤为了查看启动文件my_detection.launch的文本内容，在终端输入以下命令，在官方示例autoware_quickstart_examples路径下找到工作目录“./launch/rosbag_demo”，然后在rosbag_demo文件夹中打开启动文件my_detection.launch：$cdautoware.ai/src/autoware/documentation/autoware_quickstart_examples$cdlaunch/rosbag_demo$geditmy_detection.launch6.1环境感知方法概述<launch>……<!--points2image--><nodepkg="points2image"type="points2image"name="points2image"/><!--lidar_euclidean_cluster_detect--><includefile="$(findlidar_euclidean_cluster_detect)/launch/lidar_euclidean_cluster_detect.launch"></include><!--trafficlightrecognition--><!--feat_proj--><!--<nodepkg="trafficlight_recognizer"type="feat_proj"name="feat_proj"/>--><!--region_tlr--><!--<includefile="$(findtrafficlight_recognizer)/launch/traffic_light_recognition.launch"/>--></launch>29Autoware环境感知基本步骤启动文件my_detection.launch通过导入“lidar_euclidean_cluster_detect.launch”、“traffic_light_recognition.launch”等多个启动文件到当前文件实现对车辆周围物体点云聚类提取结构化信息以及图像识别交通信号灯状态等相关功能的调用。my_detection.launch6.1环境感知方法概述306.2点云聚类功能包31点云欧式聚类方法点云聚类是指将激光雷达扫描周围环境获取的点云数据按照一定的规则或特征分成多个子类，使得同一子类中的点在某种度量下具有较高的相似性。通过点云聚类可以帮助识别道路上的障碍物，如车辆、行人等。经典的点云聚类算法有欧式聚类和K-means聚类等，本节内容着重介绍点云欧式聚类方法。欧式聚类（EuclideanClusterExtraction）是一种基于欧氏距离度量的聚类算法，具有算法原理简单、计算量小等优点。6.2点云聚类功能包32点云欧式聚类方法6.2点云聚类功能包

欧式距离33

6.2点云聚类功能包34

6.2点云聚类功能包35

6.2点云聚类功能包36

6.2点云聚类功能包构建KD-Tree37

6.2点云聚类功能包构建KD-Tree38

6.2点云聚类功能包构建KD-Tree39

6.2点云聚类功能包构建KD-Tree40

6.2点云聚类功能包

构建KD-Tree41

6.2点云聚类功能包KD-Tree搜索42

6.2点云聚类功能包KD-Tree搜索43

6.2点云聚类功能包KD-Tree搜索44

6.2点云聚类功能包KD-Tree搜索45

6.2点云聚类功能包KD-Tree搜索46

6.2点云聚类功能包KD-Tree搜索47

6.2点云聚类功能包KD-Tree搜索48

6.2点云聚类功能包KD-Tree搜索49节点lidar_euclidean_cluster_detect与源码解析启动文件my_detection.launch调用了功能包“~/autoware.ai/src/autoware/core_perception/lidar_euclidean_cluster_detect”目录下的启动文件“~/launch/lidar_euclidean_cluster_detect.launch”，用于启动点云聚类过程中所需的多个节点。6.2点云聚类功能包<launch>……<!--points2image--><nodepkg="points2image"type="points2image"name="points2image"/><!--lidar_euclidean_cluster_detect--><includefile="$(findlidar_euclidean_cluster_detect)/launch/lidar_euclidean_cluster_detect.launch"></include><!--trafficlightrecognition--><!--feat_proj--><!--<nodepkg="trafficlight_recognizer"type="feat_proj"name="feat_proj"/>--><!--region_tlr--><!--<includefile="$(findtrafficlight_recognizer)/launch/traffic_light_recognition.launch"/>--></launch>my_detection.launch运行启动文件“lidar_euclidean_cluster_detect.launch”会启动节点lidar_euclidean_cluster_detect。其中，节点lidar_euclidean_cluster_detect用于激光点云聚类，利用欧式聚类算法将无序的点云划分为具有相似特征的集合，为后续分析和应用提供结构化信息。<!----><launch><argname="points_node"default="/points_raw"/><argname="remove_ground"default="true"/><argname="downsample_cloud"default="false"/><argname="leaf_size"default="0.1"/><argname="cluster_size_min"default="20"/><argname="cluster_size_max"default="100000"/>……<nodepkg="lidar_euclidean_cluster_detect"type="lidar_euclidean_cluster_detect"name="lidar_euclidean_cluster_detect"output="screen"><paramname="points_node"value="$(argpoints_node)"/><paramname="remove_ground"value="$(argremove_ground)"/><paramname="downsample_cloud"value="$(argdownsample_cloud)"/><paramname="leaf_size"value="$(argleaf_size)"/><paramname="cluster_size_min"value="$(argcluster_size_min)"/><paramname="cluster_size_max"value="$(argcluster_size_max)"/><paramname="use_diffnormals"value="$(arguse_diffnormals)"/><paramname="pose_estimation"value="$(argpose_estimation)"/><paramname="keep_lanes"value="$(argkeep_lanes)"/><paramname="keep_lane_left_distance"value="$(argkeep_lane_left_distance)"/><paramname="keep_lane_right_distance"value="$(argkeep_lane_right_distance)"/><paramname="max_boundingbox_side"value="$(argmax_boundingbox_side)"/><paramname="clip_min_height"value="$(argclip_min_height)"/><paramname="clip_max_height"value="$(argclip_max_height)"/>……</launch>50lidar_euclidean_cluster_detect.launch节点lidar_euclidean_cluster_detect与源码解析6.2点云聚类功能包51节点lidar_euclidean_cluster_detect与源码解析双击打开功能包lidar_euclidean_cluster_detect目录下的CMakeLists.txt文件。节点lidar_euclidean_cluster_detect的源文件是功能包lidar_euclidean_cluster_detect目录下的“nodes/lidar_euclidean_cluster_detect/lidar_euclidean_cluster_detect.cpp”。……#EuclideanClusteradd_executable(lidar_euclidean_cluster_detectnodes/lidar_euclidean_cluster_detect/lidar_euclidean_cluster_detect.cppnodes/lidar_euclidean_cluster_detect/cluster.cpp)……CMakeLists.txt6.2点云聚类功能包52节点lidar_euclidean_cluster_detect与源码解析（1）主函数流程解析在VSCode软件中打开文件夹autoware.ai下的代码空间src，然后在侧边栏中依次点击文件夹autoware下的子文件夹core_perception找到功能包lidar_euclidean_cluster_detect，选择节点文件夹nodes6.2点云聚类功能包中的节点lidar_euclidean_cluster_detect，最后双击打开源文件lidar_euclidean_cluster_detect.cpp。源文件lidar_euclidean_cluster_detect.cpp中主函数的流程如图所示。53节点lidar_euclidean_cluster_detect与源码解析（1）主函数流程解析主函数通过ros::init()函数对节点lidar_euclidean_cluster_detect进行初始化。然后，创建节点句柄h用于话题发布和订阅，并创建私有节点句柄private_nh用于参数获取。……intmain(intargc,char**argv){ros::init(argc,argv,"euclidean_cluster");ros::NodeHandleh;ros::NodeHandleprivate_nh("~");……}lidar_euclidean_cluster_detect.cpp文本中主函数的初始化部分初始化6.2点云聚类功能包54节点lidar_euclidean_cluster_detect与源码解析（1）主函数流程解析通过私有句柄调用param函数从参数服务器上获取_downsample_cloud、",_remove_ground、_leaf_size、_cluster_size_min、_cluster_size_max等参数（与启动文件lidar_euclidean_cluster_detect.launch中所定义的参数一一对应）。参数获取……intmain(intargc,char**argv){……private_nh.param("downsample_cloud",_downsample_cloud,false);ROS_INFO("[%s]downsample_cloud:%d",__APP_NAME__,_downsample_cloud);private_nh.param("remove_ground",_remove_ground,true);ROS_INFO("[%s]remove_ground:%d",__APP_NAME__,_remove_ground);private_nh.param("leaf_size",_leaf_size,0.1);ROS_INFO("[%s]leaf_size:%f",__APP_NAME__,_leaf_size);private_nh.param("cluster_size_min",_cluster_size_min,20);ROS_INFO("[%s]cluster_size_min%d",__APP_NAME__,_cluster_size_min);private_nh.param("cluster_size_max",_cluster_size_max,100000);ROS_INFO("[%s]cluster_size_max:%d",__APP_NAME__,_cluster_size_max);private_nh.param("pose_estimation",_pose_estimation,false);……}lidar_euclidean_cluster_detect.cpp文本中主函数的参数获取部分6.2点云聚类功能包然后在此基础上检查各参数的配置情况。如果上述参数未在启动文件中进行配置，则自动设置为默认值。节点lidar_euclidean_cluster_detect与源码解析（1）主函数流程解析聚类半径选取……intmain(intargc,char**argv){……if(_use_multiple_thres){YAML::Nodedistances=YAML::Load(str_distances);YAML::Noderanges=YAML::Load(str_ranges);size_tdistances_size=distances.size();size_tranges_size=ranges.size();……for(size_ti_distance=0;i_distance<distances_size;i_distance++){_clustering_distances.push_back(distances[i_distance].as<double>());}for(size_ti_range=0;i_range<ranges_size;i_range++){_clustering_ranges.push_back(ranges[i_range].as<double>());}}……}lidar_euclidean_cluster_detect.cpp文本中主函数的聚类半径选取部分6.2点云聚类功能包从YAML格式的字符串中解析聚类半径str_distances、激光点到激光雷达的距离阈值str_ranges，并将处理后的数据存储在成员变量_clustering_distances和_clustering_ranges以供后续使用。55节点lidar_euclidean_cluster_detect与源码解析（1）主函数流程解析聚类半径选取……intmain(intargc,char**argv){……if(_use_multiple_thres){YAML::Nodedistances=YAML::Load(str_distances);YAML::Noderanges=YAML::Load(str_ranges);size_tdistances_size=distances.size();size_tranges_size=ranges.size();……for(size_ti_distance=0;i_distance<distances_size;i_distance++){_clustering_distances.push_back(distances[i_distance].as<double>());}for(size_ti_range=0;i_range<ranges_size;i_range++){_clustering_ranges.push_back(ranges[i_range].as<double>());}}……}lidar_euclidean_cluster_detect.cpp文本中主函数的聚类半径选取部分6.2点云聚类功能包由于激光雷达点云具有近密远疏的采样特性，点云随着距离的增大会变得稀疏。若聚类半径相同，则会导致近处若干障碍物聚为一类，远处同一障碍物被分为若干类。因此需要根据不同的距离范围使用不同的聚类半径。5657节点lidar_euclidean_cluster_detect与源码解析（1）主函数流程解析在主函数中发布的话题有“/points_cluster”、“/points_ground”、“/cluster_centroids”、“/points_lanes”、“/cloud_clusters”、“/objects”等，而订阅的话题则有“points_topic”。其中，订阅话题时会通过回调函数（velodyne_callback函数）处理接收到的话题信息。……intmain(intargc,char**argv){……_pub_cluster_cloud=h.advertise<sensor_msgs::PointCloud2>("/points_cluster",1);_pub_ground_cloud=h.advertise<sensor_msgs::PointCloud2>("/points_ground",1);_centroid_pub=h.advertise<autoware_msgs::Centroids>("/cluster_centroids",1);_pub_points_lanes_cloud=h.advertise<sensor_msgs::PointCloud2>("/points_lanes",1);_pub_clusters_message=h.advertise<autoware_msgs::CloudClusterArray>("/detection/lidar_detector/cloud_clusters",1);_pub_detected_objects=h.advertise<autoware_msgs::DetectedObjectArray>("/detection/lidar_detector/objects",1);……ros::Subscribersub=h.subscribe(points_topic,1,velodyne_callback);ros::spin();}lidar_euclidean_cluster_detect.cpp文本中主函数的话题发布和订阅部分发布和订阅6.2点云聚类功能包58节点lidar_euclidean_cluster_detect与源码解析（2）关键子函数解析velodyne_callback函数主要用于处理原始点云数据并执行完整的聚类流程。velodyne_callback函数6.2点云聚类功能包……intmain(intargc,char**argv){……_pub_cluster_cloud=h.advertise<sensor_msgs::PointCloud2>("/points_cluster",1);_pub_ground_cloud=h.advertise<sensor_msgs::PointCloud2>("/points_ground",1);_centroid_pub=h.advertise<autoware_msgs::Centroids>("/cluster_centroids",1);_pub_points_lanes_cloud=h.advertise<sensor_msgs::PointCloud2>("/points_lanes",1);_pub_clusters_message=h.advertise<autoware_msgs::CloudClusterArray>("/detection/lidar_detector/cloud_clusters",1);_pub_detected_objects=h.advertise<autoware_msgs::DetectedObjectArray>("/detection/lidar_detector/objects",1);……ros::Subscribersub=h.subscribe(points_topic,1,velodyne_callback);ros::spin();}lidar_euclidean_cluster_detect.cpp文本中主函数的话题发布和订阅部分59节点lidar_euclidean_cluster_detect与源码解析（2）关键子函数解析……voidvelodyne_callback(constsensor_msgs::PointCloud2ConstPtr&in_sensor_cloud){if(!_using_sensor_cloud){_using_sensor_cloud=true;pcl::PointCloud<pcl::PointXYZ>::Ptrcurrent_sensor_cloud_ptr(newpcl::PointCloud<pcl::PointXYZ>);pcl::PointCloud<pcl::PointXYZ>::Ptrremoved_points_cloud_ptr(newpcl::PointCloud<pcl::PointXYZ>);pcl::PointCloud<pcl::PointXYZ>::Ptrdownsampled_cloud_ptr(newpcl::PointCloud<pcl::PointXYZ>);pcl::PointCloud<pcl::PointXYZ>::Ptrinlanes_cloud_ptr(newpcl::PointCloud<pcl::PointXYZ>);pcl::PointCloud<pcl::PointXYZ>::Ptrnofloor_cloud_ptr(newpcl::PointCloud<pcl::PointXYZ>);pcl::PointCloud<pcl::PointXYZ>::Ptronlyfloor_cloud_ptr(newpcl::PointCloud<pcl::PointXYZ>);pcl::PointCloud<pcl::PointXYZ>::Ptrdiffnormals_cloud_ptr(newpcl::PointCloud<pcl::PointXYZ>);pcl::PointCloud<pcl::PointXYZ>::Ptrclipped_cloud_ptr(newpcl::PointCloud<pcl::PointXYZ>);pcl::PointCloud<pcl::PointXYZRGB>::Ptrcolored_clustered_cloud_ptr(newpcl::PointCloud<pcl::PointXYZRGB>);autoware_msgs::Centroidscentroids;autoware_msgs::CloudClusterArraycloud_clusters;……_using_sensor_cloud=false;}}……lidar_euclidean_cluster_detect.cpp文本中velodyne_callback函数Part1通过接收数据类型为sensor_msgs::PointCloud2的消息in_sensor_cloud作为输入，获取激光雷达点云数据。6.2点云聚类功能包velodyne_callback函数60节点lidar_euclidean_cluster_detect与源码解析（2）关键子函数解析……voidvelodyne_callback(constsensor_msgs::PointCloud2ConstPtr&in_sensor_cloud){if(!_using_sensor_cloud){_using_sensor_cloud=true;pcl::PointCloud<pcl::PointXYZ>::Ptrcurrent_sensor_cloud_ptr(newpcl::PointCloud<pcl::PointXYZ>);pcl::PointCloud<pcl::PointXYZ>::Ptrremoved_points_cloud_ptr(newpcl::PointCloud<pcl::PointXYZ>);pcl::PointCloud<pcl::PointXYZ>::Ptrdownsampled_cloud_ptr(newpcl::PointCloud<pcl::PointXYZ>);pcl::PointCloud<pcl::PointXYZ>::Ptrinlanes_cloud_ptr(newpcl::PointCloud<pcl::PointXYZ>);pcl::PointCloud<pcl::PointXYZ>::Ptrnofloor_cloud_ptr(newpcl::PointCloud<pcl::PointXYZ>);pcl::PointCloud<pcl::PointXYZ>::Ptronlyfloor_cloud_ptr(newpcl::PointCloud<pcl::PointXYZ>);pcl::PointCloud<pcl::PointXYZ>::Ptrdiffnormals_cloud_ptr(newpcl::PointCloud<pcl::PointXYZ>);pcl::PointCloud<pcl::PointXYZ>::Ptrclipped_cloud_ptr(newpcl::PointCloud<pcl::PointXYZ>);pcl::PointCloud<pcl::PointXYZRGB>::Ptrcolored_clustered_cloud_ptr(newpcl::PointCloud<pcl::PointXYZRGB>);autoware_msgs::Centroidscentroids;autoware_msgs::CloudClusterArraycloud_clusters;……_using_sensor_cloud=false;}}……lidar_euclidean_cluster_detect.cpp文本中velodyne_callback函数Part1通过标志位_using_sensor_cloud实现的简单互斥锁功能，以防止在处理当前帧时被新的点云消息中断。6.2点云聚类功能包velodyne_callback函数在函数末尾将标志位_using_sensor_cloud重置为false。61节点lidar_euclidean_cluster_detect与源码解析（2）关键子函数解析……voidvelodyne_callback(constsensor_msgs::PointCloud2ConstPtr&in_sensor_cloud){if(!_using_sensor_cloud){_using_sensor_cloud=true;pcl::PointCloud<pcl::PointXYZ>::Ptrcurrent_sensor_cloud_ptr(newpcl::PointCloud<pcl::PointXYZ>);pcl::PointCloud<pcl::PointXYZ>::Ptrremoved_points_cloud_ptr(newpcl::PointCloud<pcl::PointXYZ>);pcl::PointCloud<pcl::PointXYZ>::Ptrdownsampled_cloud_ptr(newpcl::PointCloud<pcl::PointXYZ>);pcl::PointCloud<pcl::PointXYZ>::Ptrinlanes_cloud_ptr(newpcl::PointCloud<pcl::PointXYZ>);pcl::PointCloud<pcl::PointXYZ>::Ptrnofloor_cloud_ptr(newpcl::PointCloud<pcl::PointXYZ>);pcl::PointCloud<pcl::PointXYZ>::Ptronlyfloor_cloud_ptr(newpcl::PointCloud<pcl::PointXYZ>);pcl::PointCloud<pcl::PointXYZ>::Ptrdiffnormals_cloud_ptr(newpcl::PointCloud<pcl::PointXYZ>);pcl::PointCloud<pcl::PointXYZ>::Ptrclipped_cloud_ptr(newpcl::PointCloud<pcl::PointXYZ>);pcl::PointCloud<pcl::PointXYZRGB>::Ptrcolored_clustered_cloud_ptr(newpcl::PointCloud<pcl::PointXYZRGB>);autoware_msgs::Centroidscentroids;autoware_msgs::CloudClusterArraycloud_clusters;……_using_sensor_cloud=false;}}……lidar_euclidean_cluster_detect.cpp文本中velodyne_callback函数Part1创建多个pcl::PointCloud<pcl::PointXYZI>类型的点云容器，用于不同处理阶段。6.2点云聚类功能包velodyne_callback函数62节点lidar_euclidean_cluster_detect与源码解析（2）关键子函数解析……voidvelodyne_callback(constsensor_msgs::PointCloud2ConstPtr&in_sensor_cloud){if(!_using_sensor_cloud){_using_sensor_cloud=true;pcl::PointCloud<pcl::PointXYZ>::Ptrcurrent_sensor_cloud_ptr(newpcl::PointCloud<pcl::PointXYZ>);pcl::PointCloud<pcl::PointXYZ>::Ptrremoved_points_cloud_ptr(newpcl::PointCloud<pcl::PointXYZ>);pcl::PointCloud<pcl::PointXYZ>::Ptrdownsampled_cloud_ptr(newpcl::PointCloud<pcl::PointXYZ>);pcl::PointCloud<pcl::PointXYZ>::Ptrinlanes_cloud_ptr(newpcl::PointCloud<pcl::PointXYZ>);pcl::PointCloud<pcl::PointXYZ>::Ptrnofloor_cloud_ptr(newpcl::PointCloud<pcl::PointXYZ>);pcl::PointCloud<pcl::PointXYZ>::Ptronlyfloor_cloud_ptr(newpcl::PointCloud<pcl::PointXYZ>);pcl::PointCloud<pcl::PointXYZ>::Ptrdiffnormals_cloud_ptr(newpcl::PointCloud<pcl::PointXYZ>);pcl::PointCloud<pcl::PointXYZ>::Ptrclipped_cloud_ptr(newpcl::PointCloud<pcl::PointXYZ>);pcl::PointCloud<pcl::PointXYZRGB>::Ptrcolored_clustered_cloud_ptr(newpcl::PointCloud<pcl::PointXYZRGB>);autoware_msgs::Centroidscentroids;autoware_msgs::CloudClusterArraycloud_clusters;……_using_sensor_cloud=false;}}……lidar_euclidean_cluster_detect.cpp文本中velodyne_callback函数Part1current_sensor_cloud_ptr（原始点云数据）、removed_points_cloud_ptr（移除近处点后的点云数据）、downsampled_cloud_ptr（降采样后的点云数据）、inlanes_cloud_ptr（保留车道区域后的点云数据）、nofloor_cloud_ptr（移除地面后的点云数据）、6.2点云聚类功能包velodyne_callback函数63节点lidar_euclidean_cluster_detect与源码解析（2）关键子函数解析……voidvelodyne_callback(constsensor_msgs::PointCloud2ConstPtr&in_sensor_cloud){if(!_using_sensor_cloud){_using_sensor_cloud=true;pcl::PointCloud<pcl::PointXYZ>::Ptrcurrent_sensor_cloud_ptr(newpcl::PointCloud<pcl::PointXYZ>);pcl::PointCloud<pcl::PointXYZ>::Ptrremoved_points_cloud_ptr(newpcl::PointCloud<pcl::PointXYZ>);pcl::PointCloud<pcl::PointXYZ>::Ptrdownsampled_cloud_ptr(newpcl::PointCloud<pcl::PointXYZ>);pcl::PointCloud<pcl::PointXYZ>::Ptrinlanes_cloud_ptr(newpcl::PointCloud<pcl::PointXYZ>);pcl::PointCloud<pcl::PointXYZ>::Ptrnofloor_cloud_ptr(newpcl::PointCloud<pcl::PointXYZ>);pcl::PointCloud<pcl::PointXYZ>::Ptronlyfloor_cloud_ptr(newpcl::PointCloud<pcl::PointXYZ>);pcl::PointCloud<pcl::PointXYZ>::Ptrdiffnormals_cloud_ptr(newpcl::PointCloud<pcl::PointXYZ>);pcl::PointCloud<pcl::PointXYZ>::Ptrclipped_cloud_ptr(newpcl::PointCloud<pcl::PointXYZ>);pcl::PointCloud<pcl::PointXYZRGB>::Ptrcolored_clustered_cloud_ptr(newpcl::PointCloud<pcl::PointXYZRGB>);autoware_msgs::Centroidscentroids;autoware_msgs::CloudClusterArraycloud_clusters;……_using_sensor_cloud=false;}}……lidar_euclidean_cluster_detect.cpp文本中velodyne_callback函数Part1onlyfloor_cloud_ptr（仅地面点云数据）、diffnormals_cloud_ptr（法线差异处理后的点云数据）、clipped_cloud_ptr（高度裁剪后的点云数据）、colored_clustered_cloud_ptr（带颜色的聚类结果）。6.2点云聚类功能包velodyne_callback函数64节点lidar_euclidean_cluster_detect与源码解析（2）关键子函数解析……voidvelodyne_callback(constsensor_msgs::PointCloud2ConstPtr&in_sensor_cloud){if(!_using_sensor_cloud){_using_sensor_cloud=true;pcl::PointCloud<pcl::PointXYZ>::Ptrcurrent_sensor_cloud_ptr(newpcl::PointCloud<pcl::PointXYZ>);pcl::PointCloud<pcl::PointXYZ>::Ptrremoved_points_cloud_ptr(newpcl::PointCloud<pcl::PointXYZ>);pcl::PointCloud<pcl::PointXYZ>::Ptrdownsampled_cloud_ptr(newpcl::PointCloud<pcl::PointXYZ>);pcl::PointCloud<pcl::PointXYZ>::Ptrinlanes_cloud_ptr(newpcl::PointCloud<pcl::PointXYZ>);pcl::PointCloud<pcl::PointXYZ>::Ptrnofloor_cloud_ptr(newpcl::PointCloud<pcl::PointXYZ>);pcl::PointCloud<pcl::PointXYZ>::Ptronlyfloor_cloud_ptr(newpcl::PointCloud<pcl::PointXYZ>);pcl::PointCloud<pcl::PointXYZ>::Ptrdiffnormals_cloud_ptr(newpcl::PointCloud<pcl::PointXYZ>);pcl::PointCloud<pcl::PointXYZ>::Ptrclipped_cloud_ptr(newpcl::PointCloud<pcl::PointXYZ>);pcl::PointCloud<pcl::PointXYZRGB>::Ptrcolored_clustered_cloud_ptr(newpcl::PointCloud<pcl::PointXYZRGB>);autoware_msgs::Centroidscentroids;autoware_msgs::CloudClusterArraycloud_clusters;……_using_sensor_cloud=false;}}……lidar_euclidean_cluster_detect.cpp文本中velodyne_callback函数Part1分别对数据类型为autoware_msgs::Centroids的聚类中心centroids和数据类型为autoware_msgs::CloudClusterArray的聚类结果cloud_clusters进行初始化。6.2点云聚类功能包velodyne_callback函数65节点lidar_euclidean_cluster_detect与源码解析（2）关键子函数解析……voidvelodyne_callback(constsensor_msgs::PointCloud2ConstPtr&in_sensor_cloud){if(!_using_sensor_cloud){……pcl::fromROSMsg(*in_sensor_cloud,*current_sensor_cloud_ptr);_velodyne_header=in_sensor_cloud->header;if(_remove_points_upto>0.0){removePointsUpTo(current_sensor_cloud_ptr,removed_points_cloud_ptr,_remove_points_upto);}else{removed_points_cloud_ptr=current_sensor_cloud_ptr;}……}}……lidar_euclidean_cluster_detect.cpp文本中velodyne_callback函数Part2利用pcl::fromROSMsg()函数将sensor_msgs::PointCloud2类型的输入点云数据in_sensor_cloud转化为pcl::PointCloud<pcl::PointXYZI>类型的原始点云数据current_sensor_cloud_ptr。6.2点云聚类功能包velodyne_callback函数66节点lidar_euclidean_cluster_detect与源码解析（2）关键子函数解析……voidvelodyne_callback(constsensor_msgs::PointCloud2ConstPtr&in_sensor_cloud){if(!_using_sensor_cloud){……pcl::fromROSMsg(*in_sensor_cloud,*current_sensor_cloud_ptr);_velodyne_header=in_sensor_cloud->header;if(_remove_points_upto>0.0){removePointsUpTo(current_sensor_cloud_ptr,removed_points_cloud_ptr,_remove_points_upto);}else{removed_points_cloud_ptr=current_sensor_cloud_ptr;}……}}……lidar_euclidean_cluster_detect.cpp文本中velodyne_callback函数Part2保存输入点云数据in_sensor_cloud的消息头（包含时间戳、坐标系等信息），用于后续发布处理结果时保持时间戳和坐标系一致。6.2点云聚类功能包velodyne_callback函数67节点lidar_euclidean_cluster_detect与源码解析（2）关键子函数解析……voidvelodyne_callback(constsensor_msgs::PointCloud2ConstPtr&in_sensor_cloud){if(!_using_sensor_cloud){……pcl::fromROSMsg(*in_sensor_cloud,*current_sensor_cloud_ptr);_velodyne_header=in_sensor_cloud->header;if(_remove_points_upto>0.0){removePointsUpTo(current_sensor_cloud_ptr,removed_points_cloud_ptr,_remove_points_upto);}else{removed_points_cloud_ptr=current_sensor_cloud_ptr;}……}}……通过之前私有句柄调用param函数从参数服务器上获取配置参数_remove_points_upto，当判断语句_remove_points_upto>0.0为真时，表示移除近点，则调用removePointsUpTo函数移除距离小于阈值_remove_points_upto的点云数据并将其存储在removed_points_cloud_ptr；否则不移除近点，直接将原始点云数据current_sensor_cloud_ptr存储在removed_points_cloud_ptr。6.2点云聚类功能包velodyne_callback函数近点移除lidar_euclidean_cluster_detect.cpp文本中velodyne_callback函数Part268节点lidar_euclidean_cluster_detect与源码解析（2）关键子函数解析通过之前私有句柄调用param函数从参数服务器上获取配置参数_downsample_cloud和_leaf_size决定是否进行降采样以及对体素叶大小进行设置。6.2点云聚类功能包velodyne_callback函数……voidvelodyne_callback(constsensor_msgs::PointCloud2ConstPtr&in_sensor_cloud){if(!_using_sensor_cloud){……if(_downsample_cloud)downsampleCloud(removed_points_cloud_ptr,downsampled_cloud_ptr,_leaf_size);elsedownsampled_cloud_ptr=removed_points_cloud_ptr;……}}……降采样lidar_euclidean_cluster_detect.cpp文本中velodyne_callback函数Part369节点lidar_euclidean_cluster_detect与源码解析（2）关键子函数解析当判断语句中标志位_downsample_cloud为真时，表示进行降采样，则调用downsampleCloud函数对removed_points_cloud_ptr进行体素滤波以降低点云数据量并将其存储在downsampled_cloud_ptr；否则不进行降采样，直接将移除近处点后的点云数据removed_points_cloud_ptr存储在downsampled_cloud_ptr。6.2点云聚类功能包velodyne_callback函数……voidvelodyne_callback(constsensor_msgs::PointCloud2ConstPtr&in_sensor_cloud){if(!_using_sensor_cloud){……if(_downsample_cloud)downsampleCloud(removed_points_cloud_ptr,downsampled_cloud_ptr,_leaf_size);elsedownsampled_cloud_ptr=removed_points_cloud_ptr;……}}……降采样lidar_euclidean_cluster_detect.cpp文本中velodyne_callback函数Part370节点lidar_euclidean_cluster_detect与源码解析（2）关键子函数解析通过之前私有句柄调用param函数从参数服务器上获取配置参数_clip_min_height和_clip_max_height指定的高度范围裁剪点云，通过调用clipCloud函数对downsampled_cloud_ptr进行高度裁剪（移除低于_clip_min_height和高于_clip_max_height的点云数据）并将其存储在clipped_cloud_ptr。6.2点云聚类功能包velodyne_callback函数……voidvelodyne_callback(constsensor_msg