$search
00001 #include "pc2_filter.h" 00002 00003 // initialize node 00004 PCL2_Filter::PCL2_Filter() 00005 { 00006 } 00007 00008 PCL2_Filter::PCL2_Filter(int argc,char** argv) 00009 { 00010 00011 sub = n.subscribe(SUB_TOPIC,5,&PCL2_Filter::processPoints,this); 00012 00013 std::string pubtopic = SUB_TOPIC; 00014 00015 pubtopic += "_filtered"; 00016 pubtopic = "pclrgb"; 00017 pub = n.advertise<PointCloud>(pubtopic,5); 00018 00019 new_msg = sensor_msgs::PointCloud2::Ptr(new sensor_msgs::PointCloud2()); 00020 cloud_filtered = PointCloud::Ptr(new PointCloud()); 00021 //sor = pcl::VoxelGrid<sensor_msgs::PointCloud2>::Ptr(new pcl::VoxelGrid<sensor_msgs::PointCloud2>()); 00022 00023 n.param(std::string("leaf_size"), leaf_size,0.05); 00024 n.param(std::string("skip"), skip,10); 00025 00026 printf("leaf size = %.2f\n",leaf_size); 00027 printf("skip = %d\n",skip); 00028 00029 /* 00030 cloud_p = PointCloud::Ptr(new PointCloud); 00031 pcl::ModelCoefficients::Ptr coefficients_t(new pcl::ModelCoefficients()); 00032 pcl::PointIndices::Ptr inliers_t(new pcl::PointIndices()); 00033 00034 coefficients = coefficients_t; 00035 inliers = inliers_t; 00036 00037 // optional 00038 seg.setOptimizeCoefficients(true); 00039 00040 // Mandatory 00041 seg.setModelType(pcl::SACMODEL_PLANE); 00042 seg.setMethodType(pcl::SAC_RANSAC); 00043 seg.setMaxIterations(1000); 00044 seg.setDistanceThreshold(0.01); 00045 */ 00046 } 00047 00048 PCL2_Filter::~PCL2_Filter() 00049 { 00050 } 00051 00052 void PCL2_Filter::processPoints(const sensor_msgs::PointCloud2::ConstPtr& msg) 00053 { 00054 skip++; 00055 if(skip == 1) { 00056 sor.setInputCloud(msg); 00057 sor.setLeafSize(leaf_size,leaf_size,leaf_size); 00058 sor.filter(*new_msg); 00059 00060 pcl::fromROSMsg(*new_msg, *cloud_filtered); 00061 00062 cloud_filtered->header.stamp = ros::Time::now(); 00063 pub.publish(cloud_filtered); 00064 } 00065 else if(skip > 10) 00066 skip = 0; 00067 } 00068 00069 /* 00070 void PCL2_Filter::filterCloud(PointCloud::Ptr cloud_filtered) 00071 { 00072 int nr_points = cloud_filtered->points.size(); 00073 00074 while (cloud_filtered->points.size() > 0.3 * nr_points) 00075 { 00076 // Segment the largest planar component from the remaining cloud 00077 seg.setInputCloud(cloud_filtered); 00078 seg.segment(*inliers, *coefficients); 00079 00080 if( inliers->indices.size() == 0) 00081 { 00082 ROS_ERROR("Could not estimate a planar model for the given dataset."); 00083 break; 00084 } 00085 00086 00087 // Extract the inliers 00088 extract.setInputCloud(cloud_filtered); 00089 extract.setIndices(inliers); 00090 extract.setNegative(false); 00091 extract.filter(*cloud_p); 00092 ROS_INFO ("PointCloud representing the planar component: %d data points.", cloud_p->width * cloud_p->height); 00093 00094 extract.setNegative(true); 00095 extract.filter(*cloud_filtered); 00096 } 00097 00098 cloud_filtered->header.stamp = ros::Time::now(); 00099 pub.publish(cloud_filtered); 00100 } 00101 */ 00102 void PCL2_Filter::spin() 00103 { 00104 printf("in spin\n"); 00105 ros::spin(); 00106 }