Go to the documentation of this file.00001 #include <kinect_downsampler.h>
00002
00003 KinectV2Downsampler::KinectV2Downsampler(ros::NodeHandle nh, ros::NodeHandle n)
00004 : nh_(nh), rate_(n.param("loop_rate", 10)),
00005 frame_id_(n.param<std::string>("resized_pc_frame_id", "/resized_pc_frame"))
00006 {
00007 leaf_size_x_ = n.param("leaf_size_x", 0.3);
00008 leaf_size_y_ = n.param("leaf_size_y", 0.3);
00009 leaf_size_z_ = n.param("leaf_size_z", 0.3);
00010
00011 source_pc_sub_ = nh_.subscribe(n.param<std::string>("source_pc_topic_name", "/source_pointcloud"), 1, &KinectV2Downsampler::resizeCallback, this);
00012 resized_pc_pub_ = nh_.advertise<sensor_msgs::PointCloud2>(n.param<std::string>("resized_pc_topic_name", "/resized_pointcloud"), 1);
00013 }
00014
00015 void KinectV2Downsampler::resizeCallback(const sensor_msgs::PointCloud2::ConstPtr& source_pc)
00016 {
00017 pcl::PointCloud<pcl::PointXYZ> pcl_source;
00018 pcl::fromROSMsg(*source_pc, pcl_source);
00019 pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_source_ptr(new pcl::PointCloud<pcl::PointXYZ>(pcl_source));
00020 pcl::PointCloud<pcl::PointXYZ>::Ptr resized_pc_ptr (new pcl::PointCloud<pcl::PointXYZ>);
00021
00022 pcl::ApproximateVoxelGrid<pcl::PointXYZ> approximate_voxel_filter;
00023 approximate_voxel_filter.setLeafSize (leaf_size_x_, leaf_size_y_, leaf_size_z_);
00024 approximate_voxel_filter.setInputCloud (pcl_source_ptr);
00025 approximate_voxel_filter.filter (*resized_pc_ptr);
00026
00027 sensor_msgs::PointCloud2 resized_pc;
00028 pcl::toROSMsg(*resized_pc_ptr, resized_pc);
00029 resized_pc.header.stamp = ros::Time::now();
00030 resized_pc.header.frame_id = frame_id_;
00031 resized_pc_pub_.publish(resized_pc);
00032 }
00033
00034 void KinectV2Downsampler::run()
00035 {
00036 while(nh_.ok())
00037 {
00038 ros::spinOnce();
00039 rate_.sleep();
00040 }
00041 }