ROS中PCL和LaserScan转换总结

当我们要对激光雷达的数据进行处理时，我们也需要将ROS获取的雷达数据，即sensor_msgs::LaserScan，转化为PCL点云库可以使用的数据，如pcl::PointCloud<T>．

pcl网站有详细的说明，这里说明一些基本的数据类型

基本的类型  pcl::PointCloud<T>, 其具有宽度和高度， width, height

typedef pcl::PointCloud<T> cloud

cloud.width =200 代表有200列

cloud.height=200   200行

点个数就是200*200

  模板T，包括有PointXYZ  PointXYZI   PointXYZRGB         PointXYZRGBNormal

like  pcl::PointCloud<PointXYZ>    pcl::PointCloud<PointXYZI>   

pcl::PointCloud<PointXYZRGB>    pcl::PointCloud<PointXYZRGBNormal>

PointXYZ is :  

struct PointXYZ
{
  float x;
  float y;
  float z;
  float padding;
};

ROS中有sensor_msgs/PointCloud   sensor_msgs/PointCloud2

File: sensor_msgs/PointCloud2.msg

Raw Message Definition

# This message holds a collection of N-dimensional points, which may
# contain additional information such as normals, intensity, etc. The
# point data is stored as a binary blob, its layout described by the
# contents of the "fields" array.

翻译: 此消息包含N维的点，其也可包含额外的一些信息包括点的法向量，强度等。

点数据存储为二进制,它的布局由“字段”数组的内容描述

# The point cloud data may be organized 2d (image-like) or 1d
# (unordered). Point clouds organized as 2d images may be produced by
# camera depth sensors such as stereo or time-of-flight.
翻译: 点云数据可以由2d如类似图像或1d无序序列组成。二维图像的点云可以由相机深度传感器（例如立体或飞行时间）产生。

# Time of sensor data acquisition, and the coordinate frame ID (for 3d
# points).

翻译：获取到数据的时间，以及原点所在坐标系
Header header

# 2D structure of the point cloud. If the cloud is unordered, height is
# 1 and width is the length of the point cloud.

翻译：点云的2d结构，如果点云无序，则其高度为1，宽度为点云的点数

width即点云列数， height即点云行数
uint32 height
uint32 width

# Describes the channels and their layout in the binary data blob.

翻译：描述二进制数据blob中的通道及其布局
PointField[] fields

bool    is_bigendian  # Is this data bigendian?  大端模式
uint32  point_step    # Length of a point in bytes一个点占字节数
uint32  row_step      # Length of a row in bytes 一行多少个字节
uint8[] data          # Actual point data, size is (row_step*height) 实际点云数据，空间大小是总的字节数  点云的二进制数据流！ 必须要单独解析，直接读取没有任何意义

bool is_dense         # True if there are no invalid points 判断points中的数据是否是有限的（有限为true）或者说是判断点云中的点是否包含 Inf/NaN这种值（包含为false）。

Compact Message Definition

std_msgs/Header header
uint32 height
uint32 width
sensor_msgs/PointField[] fields
bool is_bigendian
uint32 point_step
uint32 row_step
uint8[] data
bool is_dense

将LaserScan  转为 pcl::PointCloud<pcl::PointXYZ>，这个时pcl库可操作的数据类型

cloud_msg的类型是pcl::PointCloud<pcl::PointXYZ>

一般定义为智能指针

pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_msg = \

boost::shared_ptr< pcl::PointCloud<pcl::PointXYZ> >(new pcl::PointCloud<pcl::PointXYZ>());

pcl::PointXYZ& pt = cloud_msg.points[i];

pt.x= msgs->ranges[i] * cosdata[i];

pt.y= msgs->ranges[i] * sindata[i];

pt.z = 0;

/*******************

inline  void toPCL(const ros::Time &stamp, std::uint64_t &pcl_stamp)

{

pcl_stamp = stamp.toNSec() / 1000ull; // Convert from ns to us

}

**********************/

pcl_conversions::toPCL(msgs->header, cloud_msg.header);//toPCL对时间进行转换

//1000ull

pointcloudxyz_pub_.publish(cloud_msg);

其中 pointcloudxyz_pub_=  nh_.advertise< pcl::PointCloud<pcl::PointXYZ> >("pcl_cloud", 10);

但是在rviz中订阅这个话题时，其类型时sensor_msgs/PointCloud2

这个是参考了scan_tools里的 scan_to_cloud_converter包

在ROS中使用pcl::PointCloud<T>  注意需要header.frame_id 和header.stamp, 比如从一个pcd文件中获得点云要在rviz中显示

pcl::PointCloud<pcl::PointXYZ>::Ptr

target_cloud->header.frame_id = "odom";

pcl_conversions::toPCL(ros::Time::now(), target_cloud->header.stamp);

将LaserScan  转为 sensor_msgs/PointCloud2 ，一般多线雷达发布的话题的消息格式，rviz中可以直接订阅

将二维雷达 lasercan数据转为点云sensor_msgs/PointCloud2，需要pcl::PointCloud<pcl::PointXYZ>数据格式过渡。

LaserScan的data[i]的数据是测距的距离值，需要转为x y坐标。

sensor_msgs::PointCloud2 point_msg;

pcl::PointXYZI point_clond;

pcl::PointCloud<pcl::PointXYZ> cloud;

point_clond.x = msgs->ranges[i] * cosdata[i];

point_clond.y = msgs->ranges[i] * sindata[i];

point_clond.z =0;

point_clond.intensity = msgs->intensities[i];

cloud.push_back(point_clond); //

转为sensor_msgs::PointCloud2

pcl::toROSMsg(cloud, point_msg);