00001 // Copyright (C) 2010-2011 Institut de Robotica i Informatica Industrial, CSIC-UPC. 00002 // Author 00003 // All rights reserved. 00004 // 00005 // This file is part of iri-ros-pkg 00006 // iri-ros-pkg is free software: you can redistribute it and/or modify 00007 // it under the terms of the GNU Lesser General Public License as published by 00008 // the Free Software Foundation, either version 3 of the License, or 00009 // at your option) any later version. 00010 // 00011 // This program is distributed in the hope that it will be useful, 00012 // but WITHOUT ANY WARRANTY; without even the implied warranty of 00013 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00014 // GNU Lesser General Public License for more details. 00015 // 00016 // You should have received a copy of the GNU Lesser General Public License 00017 // along with this program. If not, see <http://www.gnu.org/licenses/>. 00018 // 00019 // IMPORTANT NOTE: This code has been generated through a script from the 00020 // iri_ros_scripts. Please do NOT delete any comments to guarantee the correctness 00021 // of the scripts. ROS topics can be easly add by using those scripts. Please 00022 // refer to the IRI wiki page for more information: 00023 // http://wikiri.upc.es/index.php/Robotics_Lab 00024 00025 #ifndef _filter_table_alg_h_ 00026 #define _filter_table_alg_h_ 00027 00028 #include <iri_filter_table/FilterTableConfig.h> 00029 #include "mutex.h" 00030 #include <sensor_msgs/PointCloud2.h> 00031 #include <pcl/point_cloud.h> 00032 #include <pcl/point_types.h> 00033 #include <pcl/ros/conversions.h> 00034 #include <iostream> 00035 #include <pcl/ModelCoefficients.h> 00036 #include <pcl/sample_consensus/method_types.h> 00037 #include <pcl/sample_consensus/model_types.h> 00038 #include <pcl/segmentation/sac_segmentation.h> 00039 //#include <pcl/filters/voxel_grid.h> 00040 #include <pcl/filters/extract_indices.h> 00041 #include <pcl/filters/passthrough.h> 00042 #include <pcl/filters/radius_outlier_removal.h> 00043 00044 //include filter_table_alg main library 00045 00051 class FilterTableAlgorithm 00052 { 00053 protected: 00060 CMutex alg_mutex_; 00061 00062 // private attributes and methods 00063 float max_z_; //0.8 00064 float sac_dist_thr_; //0.001 00065 float filter_final_ratio_; //0.6 00066 float rad_radius_search_; //0.1 00067 00068 public: 00075 typedef iri_filter_table::FilterTableConfig Config; 00076 00083 Config config_; 00084 00093 FilterTableAlgorithm(void); 00094 00100 void lock(void) { alg_mutex_.enter(); }; 00101 00107 void unlock(void) { alg_mutex_.exit(); }; 00108 00116 bool try_enter(void) { return alg_mutex_.try_enter(); }; 00117 00129 void config_update(Config& new_cfg, uint32_t level=0); 00130 00131 // here define all filter_table_alg interface methods to retrieve and set 00132 // the driver parameters 00133 inline float set_max_z(float max_z) 00134 { 00135 this->max_z_=max_z; 00136 return this->max_z_; 00137 } 00138 00139 inline float set_sac_dist_thr(float sac_dist_thr) 00140 { 00141 this->sac_dist_thr_=sac_dist_thr; 00142 return this->sac_dist_thr_; 00143 } 00144 00145 inline float set_filter_final_ratio(float filter_final_ratio) 00146 { 00147 this->filter_final_ratio_=filter_final_ratio; 00148 return this->filter_final_ratio_; 00149 } 00150 00151 inline float set_rad_radius_search(float rad_radius_search) 00152 { 00153 this->rad_radius_search_=rad_radius_search; 00154 return this->rad_radius_search_; 00155 } 00156 00163 ~FilterTableAlgorithm(void); 00164 00165 pcl::PointCloud<pcl::PointXYZRGB>::Ptr filter_table(const sensor_msgs::PointCloud2::ConstPtr &cloud_blob); 00166 00167 }; 00168 00169 00170 #endif