方法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;
}