ROS 学习系列 -- 程序发送点云PointCloud2到Rviz显示

方法1  直接加载PCD文件：

#include <ros/ros.h>
// PCL specific includes
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

// Create a ROS publisher for the output point cloud
pub = nh.advertise<sensor_msgs::PointCloud2> ("filtered_plane", 1);  

pcl::PCLPointCloud2::Ptr cloud2(new pcl::PCLPointCloud2); 
pcl::io::loadPCDFile (argv[1], *cloud2);
// Convert to ROS data type
sensor_msgs::PointCloud2 output;
pcl_conversions::fromPCL(*cloud2, output);
output.header.frame_id = std::string("base_link");
  
// Publish the data
pub.publish (output);

方法2  代码组织点云数据

#include <sensor_msgs/PointCloud2.h>
#include <pcl/io/pcd_io.h>
#include <pcl/impl/point_types.hpp>
#include <pcl_ros/point_cloud.h>
#include <vector>
#include <pcl_conversions/pcl_conversions.h>

typedef pcl::PointCloud<pcl::PointXYZ> pclPointCloudXYZ;
typedef boost::shared_ptr<pclPointCloudXYZ> pclPointCloudXYZPtr;

void main() {
    pclPointCloudXYZPtr pcl_cloud( new pclPointCloudXYZ );
    BuildPclPointCloud( pcl_cloud );
    sensor_msgs::PointCloud2Ptr  pROSCloud(new sensor_msgs::PointCloud2);
    pcl::toROSMsg( *pcl_cloud, *pROSCloud );
    pROSCloud->header.frame_id = frame;
    pROSCloud->header.stamp = ros::Time::now();
}

template<typename PointCloudType > void
    BuildPclPointCloud( boost::shared_ptr<pcl::PointCloud<PointCloudType>> pCloud )
    {
        PointCloudType target_pt;
        int j = 0;
        for( int u = 0; u < width; u++ )
        {
            for(int v = 0; v < height; v++ )
            {
                
                target_pt.x =  x;
                target_pt.y =  y;
                target_pt.z =  z;
                pCloud->points.push_back( target_pt );
            }
        }
        pCloud->width = pCloud->points.size();
        pCloud->header.stamp = pcl_conversions::toPCL( ros::Time::now());
        pCloud->height = 1;
    }