ros sensor_msgs::laserscan 数据格式及velodyne_laserscan.cpp文件解析

    // Copyright (C) 2018, 2019 Kevin Hallenbeck, Joshua Whitley
    // All rights reserved.
    //
    // Software License Agreement (BSD License 2.0)
    //
    // Redistribution and use in source and binary forms, with or without
    // modification, are permitted provided that the following conditions
    // are met:
    //
    //  * Redistributions of source code must retain the above copyright
    //    notice, this list of conditions and the following disclaimer.
    //  * Redistributions in binary form must reproduce the above
    //    copyright notice, this list of conditions and the following
    //    disclaimer in the documentation and/or other materials provided
    //    with the distribution.
    //  * Neither the name of {copyright_holder} nor the names of its
    //    contributors may be used to endorse or promote products derived
    //    from this software without specific prior written permission.
    //
    // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
    // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
    // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
    // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
    // COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
    // INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
    // BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
    // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
    // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
    // LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
    // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
    // POSSIBILITY OF SUCH DAMAGE.
    
    #include "velodyne_laserscan/velodyne_laserscan.h"
    #include <sensor_msgs/point_cloud2_iterator.h>
    
    namespace velodyne_laserscan
    {
    
    VelodyneLaserScan::VelodyneLaserScan(ros::NodeHandle &nh, ros::NodeHandle &nh_priv) :
    nh_(nh), srv_(nh_priv), ring_count_(0)
    {
      ros::SubscriberStatusCallback connect_cb = boost::bind(&VelodyneLaserScan::connectCb, this);
       // 注册并绑定收到pointcloud2消息的回调函数 
    
    
    
      pub_ = nh.advertise<sensor_msgs::LaserScan>("scan", 10, connect_cb, connect_cb);
      //声明发布的话题
    
    
      srv_.setCallback(boost::bind(&VelodyneLaserScan::reconfig, this, _1, _2));
      //动态配置参数的回调函数，当设置的参数改变时回调调用
      
    }
    
    void VelodyneLaserScan::connectCb()
    {
      boost::lock_guard<boost::mutex> lock(connect_mutex_);
      if (!pub_.getNumSubscribers())
      {
    sub_.shutdown();
      }
      else if (!sub_)
      {
    sub_ = nh_.subscribe("velodyne_points", 10, &VelodyneLaserScan::recvCallback, this);
      }
    }
    
    //收到点云数据消息的回调实现，即转化数据并发布scan话题
    void VelodyneLaserScan::recvCallback(const sensor_msgs::PointCloud2ConstPtr& msg)
    {
      // Latch ring count
      // 一开始ring_count为零，有动态参数配置后或者第二次收到数据后，即可正常工作
      // 此块逻辑不够清晰
      
      if (!ring_count_)
      {
    // Check for PointCloud2 field 'ring'
    // 检查点去数据中是否有激光ID值，如果没有，则此点云数据无效，不处理数据。
    bool found = false;
    for (size_t i = 0; i < msg->fields.size(); i++)
    {
      if (msg->fields[i].datatype == sensor_msgs::PointField::UINT16)
      {
        if (msg->fields[i].name == "ring")
        {
          found = true;
          break;
        }
      }
    }
    
    
    if (!found)
    {
      ROS_ERROR("VelodyneLaserScan: Field 'ring' of type 'UINT16' not present in PointCloud2");
      return;
    }
    
    	//再次检查ring的值，感觉没鸟用，废代码
    for (sensor_msgs::PointCloud2ConstIterator<uint16_t> it(*msg, "ring"); it != it.end(); ++it)
    {
      const uint16_t ring = *it;
    
      if (ring + 1 > ring_count_)
      {
        ring_count_ = ring + 1;
      }
    }
    
    if (ring_count_)
    {
      ROS_INFO("VelodyneLaserScan: Latched ring count of %u", ring_count_);
    }
    else
    {
      ROS_ERROR("VelodyneLaserScan: Field 'ring' of type 'UINT16' not present in PointCloud2");
      return;
    }
      }
    
      // Select ring to use
      // 通过配置，获取需要抽取的激光ID,即需要取哪一条线（比如VLP16中抽第8号线）来发布scan，  
      // 并且给ring_count_赋值
      
      uint16_t ring;
    
      if ((cfg_.ring < 0) || (cfg_.ring >= ring_count_))
      {
    // Default to ring closest to being level for each known sensor
    if (ring_count_ > 32)
    {
      ring = 57;  // HDL-64E
    }
    else if (ring_count_ > 16)
    {
      ring = 23;  // HDL-32E
    }
    else
    {
      ring = 8;  // VLP-16
    }
      }
      else
      {
    ring = cfg_.ring;
      }
    
    //如果没有配置参数，则按默认的线来运行，VLP16的8号laser id 最接近水平
    
      ROS_INFO_ONCE("VelodyneLaserScan: Extracting ring %u", ring);
    
      // Load structure of PointCloud2
      //获取 PointCloud2 点云数据中x,y,z,强度,laserid的位置偏移，相当于数组索引
      //初始值为-1,当获取到后，变为相应位置的正数，判断是否存在
    
      int offset_x = -1;
      int offset_y = -1;
      int offset_z = -1;
      int offset_i = -1;
      int offset_r = -1;
    
    
      for (size_t i = 0; i < msg->fields.size(); i++)
      {
    if (msg->fields[i].datatype == sensor_msgs::PointField::FLOAT32)
    {
      if (msg->fields[i].name == "x")
      {
        offset_x = msg->fields[i].offset;
      }
      else if (msg->fields[i].name == "y")
      {
        offset_y = msg->fields[i].offset;
      }
      else if (msg->fields[i].name == "z")
      {
        offset_z = msg->fields[i].offset;
      }
      else if (msg->fields[i].name == "intensity")
      {
        offset_i = msg->fields[i].offset;
      }
    }
    else if (msg->fields[i].datatype == sensor_msgs::PointField::UINT16)
    {
      if (msg->fields[i].name == "ring")
      {
        offset_r = msg->fields[i].offset;
      }
    }
      }
    
      // Construct LaserScan message
      // 是否存在x,y,z值，如果不存在就报错返回，如果存在就初始化/scan消息
      // 初始化信息就可以看出/scan消息的结构组成
      
      if ((offset_x >= 0) && (offset_y >= 0) && (offset_r >= 0))
      {
     	// /scan 数据的精度，即range[]数组的大小，
     	//  size 是一圈对应的精度，因为angle_min,angle_max 在后面已经固定写死了
     	//  因此size 是（pi-(-pi))/resolution
    const float RESOLUTION = std::abs(cfg_.resolution);
    const size_t SIZE = 2.0 * M_PI / RESOLUTION;
    	
    
    sensor_msgs::LaserScanPtr scan(new sensor_msgs::LaserScan());
    scan->header = msg->header;
    
    //可以看到angle_increment 的值即为RESOLUTION     
    scan->angle_increment = RESOLUTION;
    
    	//从-pi 到 pi ，程序已经写死了
    scan->angle_min = -M_PI;
    scan->angle_max = M_PI;
    
    	// 截取0 到  200 米范围的数据，已经超VLP16量程了，所有数据都有效
    scan->range_min = 0.0;
    scan->range_max = 200.0;
    
    scan->time_increment = 0.0;
    	
    	//按分辨率推出的，ranges的大小在此定义
    scan->ranges.resize(SIZE, INFINITY);
    
    	// 标准的点云数据中各个数据的序号是有规律的，否则是不规则的数据
    	// 然而两者处理方式一样的，此处区分意义不大	
    if ((offset_x == 0) &&
        (offset_y == 4) &&
        (offset_i % 4 == 0) &&
        (offset_r % 4 == 0))
    {
      scan->intensities.resize(SIZE);
    
      const size_t X = 0;
      const size_t Y = 1;
      const size_t I = offset_i / 4;
      const size_t R = offset_r / 4;
      for (sensor_msgs::PointCloud2ConstIterator<float> it(*msg, "x"); it != it.end(); ++it)
      {
        const uint16_t r = *((const uint16_t*)(&it[R]));  // ring
    
    		// 对比laser id 是否与需要取的一致，如果不一致，则不更新range[]以及intensities[]
        if (r == ring)
        {
          const float x = it[X];  // x
          const float y = it[Y];  // y
          const float i = it[I];  // intensity
    		
    		  //求某一角度对应的序号，反正切值刚好是从-pi 到 pi
    		  //序号从angle_min开始升序,angle_min 为-pi,因此实际为：
    		  // bin=(atan2f(y,x)-angle_min)/angle_increment		 
          const int bin = (atan2f(y, x) + static_cast<float>(M_PI)) / RESOLUTION;
    
          if ((bin >= 0) && (bin < static_cast<int>(SIZE)))
          {
            //range[] 的值是平面距离
            scan->ranges[bin] = sqrtf(x * x + y * y);
            scan->intensities[bin] = i;
          }
        }
      }
    }
    else
    {
      ROS_WARN_ONCE("VelodyneLaserScan: PointCloud2 fields in unexpected order. Using slower generic method.");
    
      if (offset_i >= 0)
      {
        scan->intensities.resize(SIZE);
        sensor_msgs::PointCloud2ConstIterator<uint16_t> iter_r(*msg, "ring");
        sensor_msgs::PointCloud2ConstIterator<float> iter_x(*msg, "x");
        sensor_msgs::PointCloud2ConstIterator<float> iter_y(*msg, "y");
        sensor_msgs::PointCloud2ConstIterator<float> iter_i(*msg, "intensity");
        for ( ; iter_r != iter_r.end(); ++iter_x, ++iter_y, ++iter_r, ++iter_i)
        {
          const uint16_t r = *iter_r;  // ring
    
          if (r == ring)
          {
            const float x = *iter_x;  // x
            const float y = *iter_y;  // y
            const float i = *iter_i;  // intensity            
            const int bin = (atan2f(y, x) + static_cast<float>(M_PI)) / RESOLUTION;
    
            if ((bin >= 0) && (bin < static_cast<int>(SIZE)))
            {
              scan->ranges[bin] = sqrtf(x * x + y * y);
              scan->intensities[bin] = i;
            }
          }
        }
      }
      else
      {
        sensor_msgs::PointCloud2ConstIterator<uint16_t> iter_r(*msg, "ring");
        sensor_msgs::PointCloud2ConstIterator<float> iter_x(*msg, "x");
        sensor_msgs::PointCloud2ConstIterator<float> iter_y(*msg, "y");
    
        for (; iter_r != iter_r.end(); ++iter_x, ++iter_y, ++iter_r)
        {
          const uint16_t r = *iter_r;  // ring
    
          if (r == ring)
          {
            const float x = *iter_x;  // x
            const float y = *iter_y;  // y
            const int bin = (atan2f(y, x) + static_cast<float>(M_PI)) / RESOLUTION;
    
            if ((bin >= 0) && (bin < static_cast<int>(SIZE)))
            {
              scan->ranges[bin] = sqrtf(x * x + y * y);
            }
          }
        }
      }
    }
    
    	//发布 消息
    pub_.publish(scan);
      }
      else
      {
    ROS_ERROR("VelodyneLaserScan: PointCloud2 missing one or more required fields! (x,y,ring)");
      }
    }
    
    //动态参数回调，设置ring 以及revolution
    void VelodyneLaserScan::reconfig(VelodyneLaserScanConfig& config, uint32_t level)
    {
      cfg_ = config;
    }
    
    }  // namespace velodyne_laserscan