博客

1.我们知道通过激光传感器可以得到一帧数据下激光扫描到的点分别到激光雷达的距离，以下是/scan话题中包含的数据，我们需要关注ranges这个数据，它是一个vector，其中存放了在一帧激光内扫描到的点到激光雷达的距离，其中会包含nan,和inf，分别代表无效点和无穷大，另外某些距离小于某个数的点也是无意义的。

header: 
  seq: 718
  stamp: 
    secs: 1647483707
    nsecs: 944203693
  frame_id: "laser_frame"
angle_min: -2.26892805099
angle_max: 2.26892805099
angle_increment: 0.00582000007853
time_increment: 6.16712932242e-05
scan_time: 0.0666666701436
range_min: 0.0
range_max: 25.0
ranges: [0.5220000147819519, 0.5189999938011169, 0.5180000066757202, 
0.5139999985694885, 0.5109999775886536, 0.5109999775886536, nan, 
0.5049999952316284, nan, nan, nan, nan, nan, nan, nan, 0.3919999897480011, 
0.38999998569488525, 0.38600000739097595, 0.3869999945163727, 0.382999986410141, 
0.382999986410141, 0.3790000081062317, 0.38100001215934753, 0.37599998712539673, 
0.37599998712539673, 0.375, 0.37299999594688416, 0.3709999918937683,
 0.3659999966621399, 0.367000013589859, 0.3709999918937683, nan, nan, nan, nan, nan, nan, nan, 0.9150000214576721, 0.9179999828338623, 0.9229999780654907, 
0.9269999861717224, 0.9330000281333923, 0.9380000233650208, 0.9440000057220459, 
0.9490000009536743, 0.9570000171661377, 0.9620000123977661, 0.9639999866485596, 
nan, nan, nan, nan, 0.8029999732971191, 0.8009999990463257, 0.796999990940094, 
0.796999990940094, 0.7919999957084656, 0.7910000085830688, 0.7889999747276306, 0.7900000214576721, 0.800000011920929, 0.8069999814033508, nan, nan, nan, nan, 
0.6869999766349792, nan, 0.6949999928474426, 0.6949999928474426, 0.699999988079071, 
0.7020000219345093, 0.6949999928474426, 0.6919999718666077, 0.6980000138282776, 
nan, nan, 0.75, nan, 0.7490000128746033, 

2.我们需要统计这三个种类的点的个数，以计算在一帧激光数据中，存在多少有效点，并计算比例。

3.接下来我们先看看完整代码：

#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
#include <cmath>
 
double count = 0;
double range_threshold = 0.25;

class scan_validity
{
public:
    ros::NodeHandle nh;
    ros::Subscriber sub;

    scan_validity(ros::NodeHandle nh)
    {
        sub = nh.subscribe<sensor_msgs::LaserScan>("/scan", 1, &scan_validity::laserCallback,this);
    }

    void laserCallback(const sensor_msgs::LaserScanConstPtr& scan)
    {
        //定义一个vector，接收/scan话题中scan的ranges数据
        std::vector<float> ranges = scan->ranges;
        //计算ranges数据大小
        double a = ranges.size();
        //遍历ranges中的每个数
        for(std::vector<float>::iterator it = ranges.begin();it !=ranges.end();it++)
        {
            //判断是否为有效激光数据
            if(*it < range_threshold || std::isnan(*it)|| std::isinf(*it))
            {
                count++;
            }
        }
        // std::cout<<count<<std::endl;
        // std::cout<<a<<std::endl;

        //计算有效数据比率
        double rate = (a-count)/a*100;
        std::cout<<"有效激光数据的比例为"<<rate<<"%"<<std::endl;
        count=0;   
    }
};
 
int main(int argc, char **argv)
{
    
   ros::init(argc, argv, "scan_validity");

   ros::NodeHandle nh;

   scan_validity s(nh);
 
   ros::spin();
 
   return 0;
}

4.随后逐步分析，先来看看main函数。main函数中主要是实例化了一个类sacn_validity、s，我们的功能实现是在类中完成的，调用后，即可实现想要的结果。

int main(int argc, char **argv)
{
    
   ros::init(argc, argv, "scan_validity");

   ros::NodeHandle nh;

   scan_validity s(nh);
 
   ros::spin();
 
   return 0;
}

5.接着看定义的类scan_validiy，具体解释可以直接读代码注释

//包含必要头文件
#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
#include <cmath>

//count为计数器，range_threshold为阈值，小于这个数则也判断为无效点
double count = 0;
double range_threshold = 0.25;

class scan_validity
{
public:
//创建句柄，创建订阅者
    ros::NodeHandle nh;
    ros::Subscriber sub;

    scan_validity(ros::NodeHandle nh)
    {
        //订阅激光话题/scan中的数据
        sub = nh.subscribe<sensor_msgs::LaserScan>("/scan", 1, &scan_validity::laserCallback,this);
    }

    void laserCallback(const sensor_msgs::LaserScanConstPtr& scan)
    {
        //定义一个vector，接收/scan话题中scan的ranges数据
        std::vector<float> ranges = scan->ranges;
        //计算ranges数据大小
        double a = ranges.size();
        //遍历ranges中的每个数
        for(std::vector<float>::iterator it = ranges.begin();it !=ranges.end();it++)
        {
            //判断是否为有效激光数据
            if(*it < range_threshold || std::isnan(*it)|| std::isinf(*it))
            {
                count++;
            }
        }
        // std::cout<<count<<std::endl;
        // std::cout<<a<<std::endl;

        //计算有效数据比率
        double rate = (a-count)/a*100;
        std::cout<<"有效激光数据的比例为"<<rate<<"%"<<std::endl;
        count=0;   
    }
};
 

6.最后来看看结果：

robint01@robint01:~/robintros_ws$ rosrun quat_to_angle scan_validity 
有效激光数据的比例为69.3095%
有效激光数据的比例为68.6701%
有效激光数据的比例为68.9258%
有效激光数据的比例为69.0537%
有效激光数据的比例为68.5422%
有效激光数据的比例为68.9258%
有效激光数据的比例为68.1586%
有效激光数据的比例为68.4143%
有效激光数据的比例为67.9028%
有效激光数据的比例为68.4143%
有效激光数据的比例为67.9028%
有效激光数据的比例为67.2634%
有效激光数据的比例为67.7749%
有效激光数据的比例为67.9028%
有效激光数据的比例为67.9028%
有效激光数据的比例为67.3913%
有效激光数据的比例为67.9028%
有效激光数据的比例为67.2634%
有效激光数据的比例为67.5192%
^C有效激光数据的比例为68.0307%