#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