#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/PCLPointCloud2.h>
#include <pcl/conversions.h>
#include <pcl_ros/transforms.h>
#include <pcl/filters/passthrough.h>
ros::Publisher pub;
//回调函数
void
cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud_msg)
{
pcl::PCLPointCloud2 pcl_pc2; //声明pcl pointcloud2
pcl_conversions::toPCL(*cloud_msg,pcl_pc2); //ros pointcloud2 转 pcl pointcloud2
pcl::PointCloud<pcl::PointXYZ>::Ptr temp_cloud(new pcl::PointCloud<pcl::PointXYZ>); //声明pcl pointxyz
pcl::fromPCLPointCloud2(pcl_pc2,*temp_cloud); //pcl pointcloud2 转 pcl pointxyz
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>); //声明pcl pointxyz
std::cout << "PointCloud before filtering has: " << temp_cloud->points.size () << " data points." << std::endl;
// 创建滤波器对象
pcl::PassThrough<pcl::PointXYZ> pass;
pass.setInputCloud (temp_cloud);
pass.setFilterFieldName ("z");
pass.setFilterLimits (0.0, 1.0);
pass.filter (*cloud_filtered);
//再将滤波后的点云的数据格式转换为ROS下的数据格式发布出去
pcl::PCLPointCloud2 pcl_pc3; //声明pcl pointcloud2
sensor_msgs::PointCloud2 output; //声明ros pointcloud2
pcl::toPCLPointCloud2(*cloud_filtered,pcl_pc2); //pcl pointxyz 转 pcl pointcloud2
pcl_conversions::moveFromPCL(pcl_pc2, output); //pcl pointcloud2 转 ros pointcloud2
pub.publish (output);
}
int
main (int argc, char** argv)
{
ros::init (argc, argv, "my_pcl_tutorial");
ros::NodeHandle nh;
ros::Subscriber sub = nh.subscribe<sensor_msgs::PointCloud2> ("/camera/depth/color/points", 1, cloud_cb);
pub = nh.advertise<sensor_msgs::PointCloud2> ("output", 1);
ros::spin ();
}