在ROS节点中解析并发送点云数据是非常基础的需求，下面我们将做简单的介绍。  

点云数据发送

  关于发送节点，只需要声明头文件：   
#include <sensor_msgs/PointCloud.h>
  定义消息发布者：   
ros::Publisher cloud_pub = n.advertise<sensor_msgs::PointCloud>("cloud", 50);
  然后在循环中将消息发布出去即可：   
//将数据通过消息发送出去
sensor_msgs::PointCloud cloud;
cloud.header.stamp = ros::Time::now();
cloud.header.frame_id = "sensor_frame";//帧id
cloud.points.resize(LINE*CIRCLEPT);
cloud.channels.resize(2);//设置增加通道数
cloud.channels[0].name = "intensity";//增加反射强度信道，并设置其大小，使与点云数量相匹配
cloud.channels[0].values.resize(LINE*CIRCLEPT);
cloud.channels[1].name = "distance";//增加反射强度信道，并设置其大小，使与点云数量相匹配
cloud.channels[1].values.resize(LINE*CIRCLEPT);
int i=0;
for (int l = 0; l < LINE; l++)
	for (int c = 0; c < CIRCLEPT; c++)
	{
		cloud.points[i].x = mdecoder.mpointcloud[l][c].x;
		cloud.points[i].y = mdecoder.mpointcloud[l][c].y;
		cloud.points[i].z = mdecoder.mpointcloud[l][c].z;
		cloud.channels[0].values[i] = mdecoder.mpointcloud[l][c].r;//设置反射强度
		cloud.channels[1].values[i] = mdecoder.mpointcloud[l][c].d;
		i++;
	}
cloud_pub.publish(cloud);
  有三点需要注意：  
  1. frame_id与rviz的Fixed Frame相对应，设置成什么在rviz中就应该做出相应设置。
  2. 结构体中只有x，y，z三个值，如果需要发布的信息中还有反射强度和距离等值，需要利用channels来进行。写法参考上面代码，按照上面写法，可以在rviz上看到反射强度的区别。
  3. 如果需要发送有序点云，则在点云结构体赋值的时候按顺序进行即可，无需其他操作。
  PS: 如果想要发布指定颜色的点云，可以将附加通道命名为“rgb”：   
cloud.channels[2].name = "rgb";
  然后通过如下的操作设置rgb的值即可：   
uint32_t rgb = (static_cast<uint32_t> (255) << 16 | static_cast<uint32_t> (0) << 8 | static_cast<uint32_t> (0));//rgb
float rgb_float =*reinterpret_cast<float*> (&rgb);
cloud.channels[2].values[i] = rgb_float ; //设置类型

点云数据接收

 

对于sensor_msgs/PointCloud

  同样需要包含头文件：   
#include <sensor_msgs/PointCloud.h>
  订阅消息节点：   
ros::Subscriber sub = n.subscribe ("cloud", 1, getcloud);//订阅cloud节点
  定义回调函数：   
//回调函数
void getcloud (const sensor_msgs::PointCloudConstPtr& clouddata)
{
	//其中i为索引值，如下直接通过索引取得接收到的数据对象
	clouddata->points[i].x;
	clouddata->points[i].y;
	clouddata->points[i].z;
	clouddata->channels[0].values[i];
	clouddata->channels[1].values[i]
}
  如果发送过来的点云是按顺序赋值的，则取的时候也按照同样的顺序获取即可获得有序点云。  

对于sensor_msgs/PointCloud2

  有部分雷达驱动给的节点是"sensor_msgs/PointCloud2"格式的，读取的时候则需要先做转换。   包含头文件   
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud_conversion.h>
  之后将PointCloud2先转为PointCloud再以该形式进行处理即可。代码如下   
void getcloud(const sensor_msgs::PointCloud2 &msg)
{
    sensor_msgs::PointCloud clouddata;
	sensor_msgs::convertPointCloud2ToPointCloud(msg, clouddata);
	sensor_msgs::PointCloud cloud;
    cloud.header.stamp = ros::Time::now();
    cloud.header.frame_id = "PERCEPTION2020"; //帧id
    cloud.points.resize(clouddata.points.size());
    cloud.channels.resize(1);             //设置增加通道数
    cloud.channels[0].name = "intensity"; //增加反射强度信道，并设置其大小，使与点云数量相匹配
    cloud.channels[0].values.resize(clouddata.points.size());

    for (int i = 0; i < clouddata.points.size(); i++)
    {
        cloud.points[i].x = clouddata.points[i].x;
        cloud.points[i].y = clouddata.points[i].y;
        cloud.points[i].z = clouddata.points[i].z;
        cloud.channels[0].values[i] = clouddata.channels[0].values[i];    //设置反射强度
    }
    pub->publish(cloud);
}
  另外，sensor_msgs/PointCloud2格式可以直接获得pcd的点云帧数据。   查看bag包信息：   
rosbag info xxx.bag
  将bag包中的点云转为pcd格式   
rosrun pcl_ros bag_to_pcd xxx.bag /rfans_driver/rfans_points pcd
  最后一个参数为输出文件的文件夹名称，没有则会被创建。倒数第二个参数为话题名称。 不过用这个命令转出来的PCD文件是二进制形式的，而且Windows的PCLViewer无法读取，原因暂时不明。