基于激光雷达实现三边定位算法开发记录（一）——查找激光最强点

22年暑假实习过程中第一个开发任务，基于激光雷达实现三边定位开发功能 

开发平台为ubuntu 18.04 + ros melodic

日期：2022.7.18

本次实现：由于实现三边定位需要先找反光柱的圆心，目前计划先找出激光雷达获取反光柱反射激光的最强点，该点到激光雷达的距离加反光柱半径近似于圆心，误差在于激光雷达本身精度误差，可以接受。因此，首先要找出激光最强点。

（一）准备工作

延用公司框架建立测试用的工作空间和功能包。

core——存放launch文件

HAL——存放基于硬件的算法代码

hardware——存放硬件相关的代码，这里已经存放了激光雷达的驱动程序

（二）代码编写

在HAL文件夹中新建trilateration功能包，并在其src文件下新建trilateration.cpp文件，然后开始编程

定义结构体point用于存放激光信息

struct Point {
  long index;
  float range;
  float intensity;
  float x;
  float y;
};

在main函数中订阅激光话题，获取激光数据，并将其传递给激光处理函数scan_CB

int main(int argc, char *argv[]) {
  ros::init(argc, argv, "trilateration");
  ros::NodeHandle nh;
  ros::Subscriber sub = nh.subscribe("/scan", 1, scan_CB);

  ros::spin();
}

在激光处理函数中将激光数据传递给查找激光最大值函数findMax，找出激光强度最大的点并将该点数据打印

//处理激光信息
void scan_CB(sensor_msgs::LaserScan scan) {

  for (int i = 0; i < 5; i++) {
    auto max = findMax(scan);//查找激光数据最大值
    std::cout<<"激光最大数据:"<<"指数:"<<max.index<<"范围:"<<max.range<<"强度:"<<max.intensity <<"x轴:"<<max.x <<"y轴:"<<max.y <<std::endl;
    }
  }

查找最大值函数将获取的激光数据遍历，查找激光强度最大的点，并将该点数据返回

//查找最大值
Point findMax(const sensor_msgs::LaserScan &scan) {
  Point max{};
  for (long i = 0; i < scan.ranges.size(); i++) {    //遍历数据
    double intensity = scan.intensities[i];
    if (intensity > max.intensity) {    //排序 找激光强度最大的点
      max.index = i;
      max.range = scan.ranges[i];
      max.intensity = intensity;
      max.x = max.range * cos(scan.angle_min + scan.angle_increment * max.index);
      max.y = max.range * sin(scan.angle_min + scan.angle_increment * max.index);
    }
  }
  return max;
}

基本代码编写完毕，开始测试

（三）准备测试

在core的launch文件夹中新建trilateration.launch文件，用于启动该节点，现在分开写有利于后面往里添加参数

<launch>
    <node pkg="trilateration" type="trilateration" name="trilateration" output="screen/">
</launch>

再新建一个launch文件用于集成激光，三边定位，rviz的启动节点

<launch>

    <!--激光雷达-->
    <include file="$(find core)/launch/config/laser.launch"/>

    <!--三边定位-->
    <include file="$(find core)/launch/config/trilateration.launch"/>

    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find core)/config/rviz/rviz.rviz"/>

</launch>

在工作空间目录下新建start.sh脚本文件用于快速启动测试

#！/bin/bash

source ./devel/setup.sh 
roslaunch core view_laser.launch

（四）运行效果

编译通过后在工作空间下打开终端，运行脚本文件后即可看见结果

（五）问题和下一目标

问题：同一时间只能找出一个点，不适用于三边定位需要找出多个反光柱的任务

目标：将激光信息分区域，一个区域即位一个反光柱，再找出个反光柱的反光最强点