雷达型号:robosens helios(32线)
系统平台:Linux18.04+ros
写在前面:新手写程序肯定不是最优程序,可以说是相对最笨的方法,但是能实现功能,发出来自己做个记录,希望能对像我一样的新手有所启发。
前期在激光雷达上实现了欧几里德聚类,见学习笔记:link.
在此基础上希望能够找到前方最近距离,考虑后期能够做前方避障用。后期因为前方最近距离实现了,一并把后方和左侧右侧的一起也做了。
划分区域网格
在数据处理的过程中把点云数据划分了前后左右四个区域网格。
激光雷达中心的左右前后各1m范围(红色区域)由于雷达下方有小车的存在,直接选择过滤掉不处理。同时为了减少计算量,只处理10m以内的数据。
判断每个cluster的在每个区域的最近点。
由于前期是根据欧式聚类已经形成每一个聚类后的cluster,同时上一步划分了区域,这里就需要对判断每个cluster在每个区域的最近点。
if ((p.x>=-1 && p.x<=1 && p.y>=-1 && p.y<=1) || p.x>&#