kinect_downsampler.cpp
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 }


point_cloud_reducer
Author(s): CIR-KIT
autogenerated on Thu Jun 6 2019 20:19:55