【Knowledge Reserve】
The radar sensor message type is generally sensor_msgs/LaserScan
Use rosmsg show sensor_msgs/LaserScan to view the specifications of the message itself:
#
# 测量的激光扫描角度，逆时针为正
# 设备坐标帧的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]Thanks here to ROS for publishing sensor data (LaserScan and PointCloud)_ZONGXP's blog-CSDN blog_laserscan
If you want the distance information at a certain angle, you need to calculate the total angle. According to the angle_increment (the interval between each angle), you can get how many angles (units) it is divided into. At the same time, we can know that these angles (units) ) will be stored in the ranges array in turn, and its value is the distance value corresponding to each angle. We only need to find the position of the angle in this array and output its value.
[Add output code]
I use ydlidar_g2 radar
View the output interface in ros, located at /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;
}
Added the following section
make simple comments
1）int num = (scan_msg.angle_max-scan_msg.angle_min)/scan_msg.angle_increment;
Get the number of divided angles (units)
2）double expect_angle = 180; int range = 3;
Enter (change) the angle value you want to obtain, and the range value
3）int test_direction = (expect_angle/360)*num;
The value of the angle value in the actual scan
4）for (int i = -range; i <= num+range ; i++)
for loop traverses range[] array
5）if(i >test_direction-range && i<test_direction+range)
Reach the desired angle range
6）if(i<=0) else if (i > num && i<test_direction+range-1) else if(i == test_direction+range-1)
Because there will be a sudden change when the angle is between 0 degrees and 359 degrees, separate judgment processing is performed.
7）sum += scan_msg.ranges[i];
printf("aver_angle[%d]=%lf\n",test_angle,sum/(2*range-1));
sum adds the distance values within the desired angle range and finally outputs the average distance value
[Create a subscriber to accept processing of radar data]
In order to avoid occupying the radar serial port, you can also create a subscriber to receive radar data.
#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;
}
At the same time, add add_executable(recept_laser src/recept_laser.cpp) and target_link_libraries(recept_laser ${catkin_LIBRARIES}) to CMakeList.txt. This operation must be done when creating a new cpp file.
Mainly pay attention to how to point to the message content after receiving the data.
When I was writing, I initially expressed the received data as scan_msg.angle_max (others also use scan.xxx). When compiling, I would get errors such as has no member named 'angle_max' and other types of no member found.
Change it to scan_msg->angle_max. The error reported below is also modified in this way, and it can be compiled smoothly.
Add to the launch file that starts the radar to start the program you just wrote.
<node name="recept_laser" pkg="ydlidar_g2" type="recept_laser" output="screen" />
Run the launch file to see the actual running effect
