【学习笔记】通过雷达获取某一角度的距离信息

【知识储备】

雷达传感器消息类型一般为sensor_msgs/LaserScan

通过rosmsg show sensor_msgs/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]

此处感谢ROS之发布传感器数据（LaserScan和PointCloud）_ZONGXP的博客-CSDN博客_laserscan

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

---

【添加输出代码】

我采用的是ydlidar_g2雷达

查看在ros中的输出接口，位于/lidar/ydlidar_g2/src/ydlidar_node.cpp

/*
 *  YDLIDAR SYSTEM
 *  YDLIDAR ROS Node Client 
 *
 *  Copyright 2015 - 2018 EAI TEAM
 *  http://www.ydlidar.com
 * 
 */

#include "ros/ros.h"
#include "sensor_msgs/LaserScan.h"
#include "CYdLidar.h"
#include <vector>
#include <iostream>
#include <string>
#include <signal.h>

using namespace ydlidar;

#define ROSVerision "1.4.2"


std::vector<float> split(const std::string &s, char delim) {
    std::vector<float> elems;
    std::stringstream ss(s);
    std::string number;
    while(std::getline(ss, number, delim)) {
        elems.push_back(atof(number.c_str()));
    }
    return elems;
}


int main(int argc, char * argv[]) {
    ros::init(argc, argv, "ydlidar_node"); 
    printf("__   ______  _     ___ ____    _    ____  \n");
    printf("\\ \\ / /  _ \\| |   |_ _|  _ \\  / \\  |  _ \\ \n");
    printf(" \\ V /| | | | |    | || | | |/ _ \\ | |_) | \n");
    printf("  | | | |_| | |___ | || |_| / ___ \\|  _ <  \n");
    printf("  |_| |____/|_____|___|____/_/   \\_\\_| \\_\\ \n");
    printf("\n");
    fflush(stdout);
  
    std::string port;
    int baudrate=230400;
    std::string frame_id;
    bool reversion, resolution_fixed;
    bool auto_reconnect;
    double angle_max,angle_min;
    result_t op_result;
    std::string list;
    std::vector<float> ignore_array;  
    double max_range, min_range;
    double frequency;

    ros::NodeHandle nh;
    ros::Publisher scan_pub = nh.advertise<sensor_msgs::LaserScan>("scan", 1000);
    ros::NodeHandle nh_private("~");
    nh_private.param<std::string>("port", port, "/dev/ydlidar"); 
    nh_private.param<int>("baudrate", baudrate, 230400); 
    nh_private.param<std::string>("frame_id", frame_id, "laser_frame");
    nh_private.param<bool>("resolution_fixed", resolution_fixed, "true");
    nh_private.param<bool>("auto_reconnect", auto_reconnect, "true");
    nh_private.param<bool>("reversion", reversion, "true");
    nh_private.param<double>("angle_max", angle_max , 180);
    nh_private.param<double>("angle_min", angle_min , -180);
    nh_private.param<double>("range_max", max_range , 12.0);
    nh_private.param<double>("range_min", min_range , 0.1);
    nh_private.param<double>("frequency", frequency , 10.0);
    nh_private.param<std::string>("ignore_array",list,"");

    ignore_array = split(list ,',');
    if(ignore_array.size()%2){
        ROS_ERROR_STREAM("ignore array is odd need be even");
    }

    for(uint16_t i =0 ; i < ignore_array.size();i++){
        if(ignore_array[i] < -180 && ignore_array[i] > 180){
            ROS_ERROR_STREAM("ignore array should be between 0 and 360");
        }
    }

    CYdLidar laser;
    if(frequency<5){
       frequency = 7.0; 
    }
    if(frequency>12){
        frequency = 12;
    }
    if(angle_max < angle_min){
        double temp = angle_max;
        angle_max = angle_min;
        angle_min = temp;
    }

    ROS_INFO("[YDLIDAR INFO] Now YDLIDAR ROS SDK VERSION:%s .......", ROSVerision);
    laser.setSerialPort(port);
    laser.setSerialBaudrate(baudrate);
    laser.setMaxRange(max_range);
    laser.setMinRange(min_range);
    laser.setMaxAngle(angle_max);
    laser.setMinAngle(angle_min);
    laser.setReversion(reversion);
    laser.setFixedResolution(resolution_fixed);
    laser.setAutoReconnect(auto_reconnect);
    laser.setScanFrequency(frequency);
    laser.setIgnoreArray(ignore_array);
    bool ret = laser.initialize();
    if (ret) {
        ret = laser.turnOn();
        if (!ret) {
            ROS_ERROR("Failed to start scan mode!!!");
        }
    } else {
        ROS_ERROR("Error initializing YDLIDAR Comms and Status!!!");
    }
    ros::Rate rate(20);

    while (ret&&ros::ok()) {
        bool hardError;
        LaserScan scan;
        if(laser.doProcessSimple(scan, hardError )){
            sensor_msgs::LaserScan scan_msg;
            ros::Time start_scan_time;
            start_scan_time.sec = scan.system_time_stamp/1000000000ul;
            start_scan_time.nsec = scan.system_time_stamp%1000000000ul;
            scan_msg.header.stamp = start_scan_time;
            scan_msg.header.frame_id = frame_id;
            scan_msg.angle_min =(scan.config.min_angle);
            scan_msg.angle_max = (scan.config.max_angle);
            scan_msg.angle_increment = (scan.config.angle_increment);
            scan_msg.scan_time = scan.config.scan_time;
            scan_msg.time_increment = scan.config.time_increment;
            scan_msg.range_min = (scan.config.min_range);
            scan_msg.range_max = (scan.config.max_range);


            
           scan_msg.ranges = scan.ranges;

           
           int num = (scan_msg.angle_max-scan_msg.angle_min)/scan_msg.angle_increment;

           //choose the direction and range you want to test
           double expect_angle = 180;
           int range = 3;

           // get the direction value
           int test_direction = (expect_angle/360)*num;


            // get the distance value
               for (int i = -range; i <= num+range ; i++)
            {
                //determine if in the range we want
                if(i >test_direction-range && i<test_direction+range)
                {
                    double sum;
                    int test_angle = i*360/num;

                    //determine if direct angle close to the zero
                    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;
                    }

                    //determine if direct angle close to the last angle
                    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;
                    }
                    //Exclude the above situation,calculation average value of sum
                     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];
                    }
                }  
            }

            scan_msg.intensities =  scan.intensities;
            scan_pub.publish(scan_msg);
        }  
        rate.sleep();
        ros::spinOnce();
    }

    laser.turnOff();
    ROS_INFO("[YDLIDAR INFO] Now YDLIDAR is stopping .......");
    laser.disconnecting();
    return 0;
}

添加了以下部分

 作简单的注释

1）int num = (scan_msg.angle_max-scan_msg.angle_min)/scan_msg.angle_increment;

        获取被分割的角度（单元）数

2）double expect_angle = 180;  int range = 3;

        输入（更改）想要获取的角度值，还有范围值

3）int test_direction = (expect_angle/360)*num;

        角度值在实际扫描中的值

4）for (int i = -range; i <= num+range ; i++)

        for循环遍历range[]数组

5）if(i >test_direction-range && i<test_direction+range)

        达到想要的角度范围

6）if(i<=0) else if (i > num && i<test_direction+range-1) else if(i == test_direction+range-1)

        因为当角度在0度和359度会有一个突变，所以进行分开判断处理

7）sum += scan_msg.ranges[i];

        printf("aver_angle[%d]=%lf\n",test_angle,sum/(2*range-1));

        sum将期望角度范围内的距离值进行相加，最后输出平均距离值

【创建订阅者接受处理雷达数据】

为避免占用雷达串口，也可以采用创建一个订阅者来接受雷达数据

#include "ros/ros.h"
#include "sensor_msgs/LaserScan.h"
#include <vector>
#include <iostream>
#include <string>
#include <signal.h>

void callback(const sensor_msgs::LaserScan::ConstPtr &scan_msg){

    int num = (scan_msg->angle_max-scan_msg->angle_min)/scan_msg->angle_increment;

    //choose the direction and range you want to test
    double expect_angle = 180;
    int range = 3;

    // get the direction value
    int test_direction = (expect_angle/360)*num;


    // get the distance value
    for (int i = -range; i <= num+range ; i++)
    {
        //determine if in the range we want
        if(i >test_direction-range && i<test_direction+range)
        {
            double sum;
            int test_angle = i*360/num;

            //determine if direct angle close to the zero
            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;
            }

            //determine if direct angle close to the last angle
            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;
                }
                //Exclude the above situation,calculation average value of sum
                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];
                }
        }  
    }    
}

int main(int argc, char * argv[]) {

    ros::init(argc, argv, "ydlidar_recept_node"); 
    ros::NodeHandle n;
    ros::Subscriber scan_sub = n.subscribe("scan",10,callback);
    ros::spin(); 
    return 0;

}

同时在CMakeList.txt上添加add_executable(recept_laser src/recept_laser.cpp)和target_link_libraries(recept_laser ${catkin_LIBRARIES})，新建了cpp文件都要有此操作

主要留意接收数据后，如何指向消息内容

我在编写的时候，一开始将接收来的数据用scan_msg.angle_max（其他也是用scan.xxx）来表示，编译的时候就会报 has no member named ‘angle_max’等等这类没有找到成员的错误

 将其更改成scan_msg->angle_max，下面报的错也是用此种方法修改，就可以顺利编译了

在启动雷达的launch文件上添加启动刚刚编写好的程序

<node name="recept_laser"  pkg="ydlidar_g2"  type="recept_laser" output="screen" />

运行launch文件，查看实际运行效果