处理激光雷达信息，获取指定方向的雷达信息

在ROS上发布的LaserScan

# 测量的激光扫描角度，逆时针为正
# 设备坐标帧的0度面向前（沿着X轴方向）
Header header
float32 angle_min        # scan的开始角度 [弧度]
float32 angle_max        # scan的结束角度 [弧度]
float32 angle_increment  # 测量的角度间的距离 [弧度]
float32 time_increment   # 测量间的时间 [秒]
float32 scan_time        # 扫描间的时间 [秒]
float32 range_min        # 最小的测量距离 [米]
float32 range_max        # 最大的测量距离 [米]
float32[] ranges         # 测量的距离数据 [米] (注意: 值 < range_min 或 > range_max 应当被丢弃)
float32[] intensities    # 强度数据 [device-specific units]

关于激光雷达的坐标角度问题

雷达测距的起始方向是-180°按照逆时针旋转

 将想要的小车雷达的方位角转化为实际在ranges数组中的下标：

以下是一段人为生成雷达数据的代码

#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"laser_learning");

    ros::NodeHandle n;
    ros::Publisher scan_pub = n.advertise<sensor_msgs::LaserScan>("scan",50);

    ROS_INFO("Hello");
    unsigned int num_readings=100;
    double laser_frequency = 40;
    double ranges[num_readings];
    double intensities[num_readings];

    int count=0;
    ros::Rate r(10);
    while(n.ok()){
        for(unsigned int i=0;i<num_readings;i++){
            ranges[i]=count;
            intensities[i]=100+count;
        }

        ros::Time scan_time = ros::Time::now();

        sensor_msgs::LaserScan scan;
        scan.header.stamp = scan_time;
        scan.header.frame_id = "laser_frame";
        scan.angle_min=-1.57;
        scan.angle_max=1.57;
        scan.angle_increment=3.14/num_readings;
        scan.time_increment=(1/laser_frequency)/(num_readings);
        scan.range_min=0.0;
        scan.range_max=100.0;

        scan.ranges.resize(num_readings);
        scan.intensities.resize(num_readings);
        for(unsigned int i=0;i<num_readings;i++){
            scan.ranges[i]=ranges[i];
            scan.intensities[i]=intensities[i];
        }

        scan_pub.publish(scan);
        ++count;
        r.sleep();
    }
    //输出日志
    ROS_INFO("Hello");
    return 0;
}

如何获得雷达某一特定角度的距离信息呢

若想要某一角度上的距离信息，即将算出angle总和，根据angle_increment（每一角度间的间隔），得到被平均分为多少个角度（单元），同时，我们可以得知，这些角度（单元）会依次存放在ranges这个数组中，其值就是每个角度对应的距离值。我们只需要找到角度在这个数组中的位置，将其值输出就可以了。
 

大概的代码如下

可以在订阅雷达数据的回调函数中使用

int num = (scan_msg.angle_max-scan_msg.angle_min)/scan_msg.angle_increment;  // 获取被分割的角度（单元）

//输入（更改）想要获取的角度值，还有范围值
double expect_angle = 180;
int range = 3;
 
//角度值在实际扫描中的值
int test_direction = (expect_angle/360)*num;
  
// for循环遍历range[]数组  找到角度值对应在数组中的值
for (int i = -range; i <= num+range ; i++)
{
    //达到想要的角度范围
    if(i >test_direction-range && i<test_direction+range)
    {
        double sum;
        int test_angle = i*360/num;
 
        //因为当角度在0度和359度会有一个突变，所以进行分开判断处理
        if(i<=0)
        {
            int pre_i = i;
            i = num + i;
            sum += scan_msg.ranges[i];
            //printf("angle[%d]=%lf\n",test_angle,scan_msg.ranges[i]);
            i = pre_i;
        }
 
        //因为当角度在0度和359度会有一个突变，所以进行分开判断处理
        else if (i > num && i<test_direction+range-1)
        {
            int pre_i = i;
            i = i-num;
            sum += scan_msg.ranges[i];
            //printf("angle[%d]=%lf\n",test_angle,scan_msg.ranges[i]);
            i = pre_i;
        }
        //排除以上情况
        else if(i == test_direction+range-1)
        {
            sum += scan_msg.ranges[i];
            printf("aver_angle[%d]=%lf\n",test_angle,sum/(2*range-1));
            sum = 0;
        }
        else
        {
            sum += scan_msg.ranges[i];
        }
    }  
}

后记

在后面的学习中我发现对于雷达的理解有问题，雷达的坐标并不是0_180 0_-180 而是逆时针0——360

但是在实际运用中，使用上面的index公式发现也没有问题？但是我仔细阅读了我原来的代码，最后找到了问题所在，原来这是歪打正着，碰巧了。

众所周知，对于python数组而言，如果索引是负数那么就代表我从这个数组末尾开始数第n个数，所以这就导致我输入-30°时，获得的index下标是这样的：(-30°-min_angle)/180°*pi/angle_inc

如果我们的雷达扫描范围是0-360°，那么计算出来的下标就是负数，但是这个数对应这数组中(330°-min_angle)/180°*pi/angle_inc 下标的数是同一个，所以才不会错

相比如果是c++语言 数组索引没有负数这一说法，这个根据角度找ranges下标的算法就不成立了吧

以上的所有都是用的镭神LS01B 单线激光雷达

11.14 

最近才知道原来ranges中的数组下标是从正前方0°按逆时针旋转的   ranges中的数组下表对应角度和雷达旋转角度不是同方向的，雷达旋转是顺时针0-360