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

【知识储备】

雷达传感器消息类型一般为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 testdouble expect_angle = 180;int range = 3;// get the direction valueint test_direction = (expect_angle/360)*num;// get the distance valuefor (int i = -range; i <= num+range ; i++){//determine if in the range we wantif(i >test_direction-range && i<test_direction+range){double sum;int test_angle = i*360/num;//determine if direct angle close to the zeroif(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 angleelse 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 sumelse 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 testdouble expect_angle = 180;int range = 3;// get the direction valueint test_direction = (expect_angle/360)*num;// get the distance valuefor (int i = -range; i <= num+range ; i++){//determine if in the range we wantif(i >test_direction-range && i<test_direction+range){double sum;int test_angle = i*360/num;//determine if direct angle close to the zeroif(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 angleelse 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 sumelse 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文件上添加启动刚刚编写好的程序

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