北科天绘 获取激光雷达线数

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_ros/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/filters/filter.h>
#include <pcl/point_types.h>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/CameraInfo.h>
#include "sensor_msgs/Imu.h"
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include "sensor_msgs/point_cloud2_iterator.h"

#define VPoint velodyne_pointcloud::PointXYZIR

void velodyne_callback(const sensor_msgs::PointCloud2ConstPtr& in_cloud_msg)
{


   pcl::PointCloud<VPoint> VPoint_cloud;//XYZIR 
   int obs_count=0;
   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<in_cloud_msg->fields.size();i++)
   {
       if (in_cloud_msg->fields[i].datatype == sensor_msgs::PointField::FLOAT32) {
           if (in_cloud_msg->fields[i].name == "x") {
               offset_x = in_cloud_msg->fields[i].offset;
           } else if (in_cloud_msg->fields[i].name == "y") {
               offset_y = in_cloud_msg->fields[i].offset;
           } else if (in_cloud_msg->fields[i].name == "z") {
               offset_z = in_cloud_msg->fields[i].offset;
           } else if (in_cloud_msg->fields[i].name == "intensity") {
               offset_i = in_cloud_msg->fields[i].offset;
           }
       } else if (in_cloud_msg->fields[i].datatype == sensor_msgs::PointField::INT32) {
           if (in_cloud_msg->fields[i].name == "laserid") {
               offset_r = in_cloud_msg->fields[i].offset;
              
            //    int laserid = in_cloud_msg->fields[i].laserid;
               std::cout<<"int laserid = in_cloud_msg->fields[i].datatype  :"<<in_cloud_msg->fields[i].datatype<<endl;

               std::cout<<in_cloud_msg->fields[i].name<<" in_cloud_msg->fields["<<i<<"].offset: "<< offset_r <<endl;
               pcl::uint32_t a = in_cloud_msg->point_step;
               std::cout<<" in_cloud_msg->point_step "<<in_cloud_msg->point_step <<endl;
           }
       }
   }
   

        // VPoint_cloud.resize(in_cloud_msg->size());/
        pcl::fromROSMsg(*in_cloud_msg, VPoint_cloud);//convert->XYZIR
        if((offset_x >= 0) && (offset_y >= 0) && (offset_z >=0) && (offset_i >= 0) && (offset_r >= 0))
        {
            
            for(sensor_msgs::PointCloud2ConstIterator<float> it(*in_cloud_msg, "x"); it != it.end(); ++it)
            {
                
                const int r = *((const int*)(&it[4]));
                const float x = it[0];
                const float y = it[1];
                const float z = it[2];
                const float intensity = it[3];
                // if(j%4==0)
                {
                    VPoint_cloud.points[obs_count].y = y;
                    VPoint_cloud.points[obs_count].x = x;
                    VPoint_cloud.points[obs_count].z = z;
                    VPoint_cloud.points[obs_count].ring = r;
                  
                    std::cout<<"VPoint_cloud.points["<<obs_count<<"].ring = "<<r<<endl;
                    VPoint_cloud.points[obs_count].intensity = intensity;
                    obs_count++;
                }
            }
        }

1.看了一下北科天绘的激光雷达驱动包中的数据格式定义：

/********************************************************************
 * $I
 * @Technic Support: <sdk@isurestar.com>
 * All right reserved, Sure-Star Coop.
 ********************************************************************/
#ifndef _POINT_TYPES_H_
#define _POINT_TYPES_H_
#pragma pack(1)
typedef struct {
  float x,y,z ;
  float intent;
  // int col;
  int laserid;//激光雷达线数
  float timeflag;
  float hangle;
  float pulseWidth;
  float range;
  unsigned char mirrorid;

}RFANS_XYZ_S;
#pragma pack()
#endif

2.查看了sensor_msgs/PointCloud2.msg的定义，发现还是在data字段中有该数据。并且通过北科天绘的驱动包，可以发现每一组数据，0 -3 为 x y z intent,4为laserid。

3.rosbag 播放数据包，发现数据的排列顺序为：

point[0]  --- laserid:0

point[1]  --- laserid:1

point[2]  --- laserid:2

point[3]  --- laserid:3

point[4]  --- laserid:4

point[5]  --- laserid:5

      :

point[15]  --- laserid:15

point[16]  --- laserid:0

4.问题

sensor_msgs::PointCloud2ConstIterator 报错，原因是没有加载头文件。

phe@phe:~/Desktop$ locate sensor_msgs 
phe@phe:~/Desktop$ cd /opt/ros/melodic/include/sensor_msgs/
phe@phe:/opt/ros/melodic/include/sensor_msgs$ grep -rn "PointCloud2ConstIterator" ./
./point_cloud2_iterator.h:171:/** Private base class for PointCloud2Iterator and PointCloud2ConstIterator
./point_cloud2_iterator.h:292:class PointCloud2ConstIterator : public impl::PointCloud2IteratorBase<T, const T, const unsigned char, const sensor_msgs::PointCloud2, PointCloud2ConstIterator>
./point_cloud2_iterator.h:295:  PointCloud2ConstIterator(const sensor_msgs::PointCloud2 &cloud_msg, const std::string &field_name) :
./point_cloud2_iterator.h:296:    impl::PointCloud2IteratorBase<T, const T, const unsigned char, const sensor_msgs::PointCloud2, sensor_msgs::PointCloud2ConstIterator>::PointCloud2IteratorBase(cloud_msg, field_name) {}

找到头文件：point_cloud2_iterator.h