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
cartographer数据集激光扫描数据类型转化_第1张图片
这是一个雷达的示意图,测量步长是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;

  }

}

效果图

下面是我自己的算法部分效果图
cartographer数据集激光扫描数据类型转化_第2张图片

你可能感兴趣的:(C/C++,传感器融合,ROS,SLAM)