cartographer数据集激光扫描数据类型转化
1、前言：
本人想测试自己算法，想找一个公共的数据集来测试，所以找到cartographer的数据集，但是这个数据集的激光雷达的数类型是sensor_msgs/MultiEchoLaserScan这个类型。我平时用的是sensor_msgs/LaserScan这个类型的激光雷达数据。所以使用数据集的时候我就做了一层转化就可以了。
机器人最重要的传感器是雷达，即使没有雷达，我们也希望通过别的传感器来模拟雷达的数据，通俗点就是，我们按照ROS指定的雷达数据的形式来整理我们传感器的数据，以相同的格式发布到节点上。所以重要的一点就是知道雷达消息是怎样的。
2、雷达消息及含义
sensor_msgs/LaserScan
Raw Message Definition
# Single scan from a planar laser range-finder
#
# If you have another ranging device with different behavior (e.g. a sonar
# array), please find or create a different message, since applications
# will make fairly laser-specific assumptions about this data
Header header # timestamp in the header is the acquisition time of
# the first ray in the scan.
#
# in frame frame_id, angles are measured around
# the positive Z axis (counterclockwise, if Z is up)
# with zero angle being forward along the x axis
float32 angle_min # start angle of the scan [rad]
float32 angle_max # end angle of the scan [rad]
float32 angle_increment # angular distance between measurements [rad]
float32 time_increment # time between measurements [seconds] - if your scanner
# is moving, this will be used in interpolating position
# of 3d points
float32 scan_time # time between scans [seconds]
float32 range_min # minimum range value [m]
float32 range_max # maximum range value [m]
float32[] ranges # range data [m] (Note: values < range_min or > range_max should be discarded)
float32[] intensities # intensity data [device-specific units]. If your
# device does not provide intensities, please leave
# the array empty.
Compact Message Definition
std_msgs/Header headerfloat32 angle_minfloat32 angle_maxfloat32 angle_incrementfloat32 time_incrementfloat32 scan_timefloat32 range_minfloat32 range_maxfloat32[] rangesfloat32[] intensities
解释 ROS Answer
这是一个雷达的示意图，测量步长是0-1081，也就是一共有1081步，测量角度270度，这样的话角度分辨率就是（270/1081≈）0.25度。
以红线为X轴，绿线为Y轴建立坐标系，方向如图
angle_min=-135*(pi/180) ; //扫描的开始位置对应的角度，转换为弧度形式
angle_max= 135 * (pi/180); //扫描的结束位置，转换为弧度形式
angle_increment =0.25 * (pi/180); //两次扫描之间增加的角度，转换为弧度形式
//我们假设传感器每秒扫描50次，即每20毫秒连续发出1081道光束进行一次整场的扫描
//所以每两道扫描光速间隔（20ms/1081≈）0.0185ms
time_increment = (1 / 50) / (1081); //所以这个变量就是两次发射光速的时间间隔
scan_time = 0.02; //扫描时间，这个变量无论是官网的解析（time between scans [seconds]）还是大多数中文博客（# 扫描的时间间隔(s)）我觉得都没说清楚，我认为是有点歧义的，这篇外文博客解释为扫描时的时间，就是时间戳，实际上我看了一段程序上面是吧时间戳放在了头中，但是程序别的地方没有使用scan_time，所以暂时存疑，等我有机会了验证一下，先写扫描一场的时间0.02秒。
程序地址[Publishing Sensor Streams Over ROS](http://wiki.ros.org/navigation/Tutorials/RobotSetup/Sensors#Writing_Code_to_Publish_a_LaserScan_Message)
range_min =0 ; //可以测到的最近的距离，单位米
range_max = 20; // 可以测量的最远的距离
//对于这个雷达，距离就是1081个元素的数组，最大和最小测量范围这两个点的数据需要被丢弃，具体怎么丢弃我还没找到相关的解释，应该是接收方处理
ranges[0] = //角度为-135°处测量到的障碍物距离
ranges[1] = //角度为-135.75°处测量到的障碍物距离
.
.
.
//ranges[1080] = //角度为135°处测量到的障碍物距离
//下面这个是强度，同样是一个数组，如果传感器不能返回强度数据，就设置为空。强度的意思就是返回光的强度。代表了反射面的情况。
intensities[0]=
.
.
.
intensities[1080]=
sensor_msgs/MultiEchoLaserScan
Raw Message Definition
# Single scan from a multi-echo planar laser range-finder
#
# If you have another ranging device with different behavior (e.g. a sonar
# array), please find or create a different message, since applications
# will make fairly laser-specific assumptions about this data
Header header # timestamp in the header is the acquisition time of
# the first ray in the scan.
#
# in frame frame_id, angles are measured around
# the positive Z axis (counterclockwise, if Z is up)
# with zero angle being forward along the x axis
float32 angle_min # start angle of the scan [rad]
float32 angle_max # end angle of the scan [rad]
float32 angle_increment # angular distance between measurements [rad]
float32 time_increment # time between measurements [seconds] - if your scanner
# is moving, this will be used in interpolating position
# of 3d points
float32 scan_time # time between scans [seconds]
float32 range_min # minimum range value [m]
float32 range_max # maximum range value [m]
LaserEcho[] ranges # range data [m] (Note: NaNs, values < range_min or > range_max should be discarded)
# +Inf measurements are out of range
# -Inf measurements are too close to determine exact distance.
LaserEcho[] intensities # intensity data [device-specific units]. If your
# device does not provide intensities, please leave
# the array empty.
这个就不解释了，大家自己看。两个消息其实差不多，转化也是差不多的。
转化函数
LaserProc.h
#ifndef LASER_PROC_H
#define LASER_PROC_H
#include
#include
#include
#include
#include
#include
#include
#include
class LaserProc
{
public:
sensor_msgs::LaserScanPtr getFirstScan(const sensor_msgs::MultiEchoLaserScan& msg);
sensor_msgs::LaserScanPtr getLastScan(const sensor_msgs::MultiEchoLaserScan& msg);
sensor_msgs::LaserScanPtr getMostIntenseScan(const sensor_msgs::MultiEchoLaserScan& msg);
private:
void fillLaserScan(const sensor_msgs::MultiEchoLaserScan& msg, sensor_msgs::LaserScan& out);
size_t getFirstValue(const sensor_msgs::LaserEcho& ranges, float& range);
size_t getLastValue(const sensor_msgs::LaserEcho& ranges, float& range);
void getMostIntenseValue(const sensor_msgs::LaserEcho& ranges, const sensor_msgs::LaserEcho& intensities, float& range, float& intensity);
};
#endif
LaserProc.cpp
#include "LaserProc.h"
#include
using namespace std;
sensor_msgs::LaserScanPtr LaserProc::getFirstScan(const sensor_msgs::MultiEchoLaserScan& msg){
sensor_msgs::LaserScanPtr out(new sensor_msgs::LaserScan());
fillLaserScan(msg, *out);
out->ranges.resize(msg.ranges.size());
if(msg.ranges.size() == msg.intensities.size()){
out->intensities.resize(msg.intensities.size());
}
for(size_t i = 0; i < out->ranges.size(); i++){
size_t index = getFirstValue(msg.ranges[i], out->ranges[i]);
if(out->intensities.size() > 0){
if(msg.intensities[i].echoes.size() > 0){
out->intensities[i] = msg.intensities[i].echoes[index];
} else {
out->intensities[i] = 0.0;
}
}
}
return out;
}
sensor_msgs::LaserScanPtr LaserProc::getLastScan(const sensor_msgs::MultiEchoLaserScan& msg){
sensor_msgs::LaserScanPtr out(new sensor_msgs::LaserScan());
fillLaserScan(msg, *out);
out->ranges.resize(msg.ranges.size());
if(msg.ranges.size() == msg.intensities.size()){
out->intensities.resize(msg.intensities.size());
}
for(size_t i = 0; i < out->ranges.size(); i++){
size_t index = getLastValue(msg.ranges[i], out->ranges[i]);
if(out->intensities.size() > 0){
if(msg.intensities[i].echoes.size() > 0){
out->intensities[i] = msg.intensities[i].echoes[index];
} else {
out->intensities[i] = 0.0;
}
}
}
return out;
}
sensor_msgs::LaserScanPtr LaserProc::getMostIntenseScan(const sensor_msgs::MultiEchoLaserScan& msg){
sensor_msgs::LaserScanPtr out(new sensor_msgs::LaserScan());
fillLaserScan(msg, *out);
if(msg.ranges.size() == msg.intensities.size()){
out->ranges.resize(msg.ranges.size());
out->intensities.resize(msg.intensities.size());
} else {
std::stringstream ss;
ss << "getMostIntenseScan::Size of ranges does not equal size of intensities, cannot create scan.";
throw std::runtime_error(ss.str());
}
cout<<"max"<intensities.size(); i++){
getMostIntenseValue(msg.ranges[i], msg.intensities[i], out->ranges[i], out->intensities[i]);
// cout<ranges[i]<<" ";
max=max>out->ranges[i]?max:out->ranges[i];
}
cout<intensities.size(); i++){
if(max==out->ranges[i])
out->ranges[i]=0;
}
// cout< 0){
size_t index = 0;
range = ranges.echoes[index];
return index;
}
range = std::numeric_limits::quiet_NaN();
return 0; // Value doesn't matter
}
///< @TODO I'm assuming all laserscanners/drivers output the ranges in order received (shortest to longest). If this is not the case, please make an issue.
size_t LaserProc::getLastValue(const sensor_msgs::LaserEcho& ranges, float& range){
if(ranges.echoes.size() > 0){
size_t index = ranges.echoes.size()-1;
range = ranges.echoes[index];
return index;
}
range = std::numeric_limits::quiet_NaN();
return 0; // Value doesn't matter
}
void LaserProc::getMostIntenseValue(const sensor_msgs::LaserEcho& ranges, const sensor_msgs::LaserEcho& intensities, float& range, float& intensity){
if(intensities.echoes.size() == 0){
range = std::numeric_limits::quiet_NaN();
intensity = 0.0;
return;
}
std::vector::const_iterator max_iter = std::max_element(intensities.echoes.begin(), intensities.echoes.end());
size_t index = std::distance(intensities.echoes.begin(), max_iter);
if(ranges.echoes.size() > 0){
range = ranges.echoes[index];
intensity = *max_iter;
} else {
range = std::numeric_limits::quiet_NaN();
intensity = 0.0;
return;
}
}
效果图
下面是我自己的算法部分效果图
