Go to the documentation of this file.00001 #include "pc2_filter.h"
00002
00003
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
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
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
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
00071
00072
00073
00074
00075
00076
00077
00078
00079
00080
00081
00082
00083
00084
00085
00086
00087
00088
00089
00090
00091
00092
00093
00094
00095
00096
00097
00098
00099
00100
00101
00102 void PCL2_Filter::spin()
00103 {
00104 printf("in spin\n");
00105 ros::spin();
00106 }