00001 #ifndef _PLANAR_REMOVAL_H_ 00002 #define _PLANAR_REMOVAL_H_ 00003 00004 // [Eigen] 00005 #include "Eigen/Core" 00006 00007 // [ROS] 00008 #include "ros/ros.h" 00009 #include "ros/callback_queue.h" 00010 #include "image_transport/image_transport.h" 00011 00012 // [PCL_ROS] 00013 #include <pcl/ros/conversions.h> 00014 #include <pcl/point_cloud.h> 00015 #include <pcl/point_types.h> 00016 00017 #include <pcl/sample_consensus/model_types.h> 00018 #include <pcl/sample_consensus/method_types.h> 00019 #include <pcl/segmentation/sac_segmentation.h> 00020 #include <pcl/ros/register_point_struct.h> 00021 #include <pcl/filters/voxel_grid.h> 00022 #include <pcl/filters/extract_indices.h> 00023 00024 //pcl::toROSMsg 00025 #include <pcl/io/pcd_io.h> 00026 00027 //iriutils 00028 #include "mutex.h" 00029 00030 // [OpenCV] 00031 #include <cv_bridge/cv_bridge.h> 00032 #include <opencv2/imgproc/imgproc.hpp> 00033 #include <opencv2/highgui/highgui.hpp> 00034 00035 // [publisher subscriber headers] 00036 #include "sensor_msgs/Image.h" 00037 #include "sensor_msgs/PointCloud2.h" 00038 00039 // [services] 00040 #include <dynamic_reconfigure/server.h> 00041 #include <iri_pcl_filters/IriPclFiltersParametersConfig.h> 00042 00043 typedef union { 00044 struct /*anonymous*/ 00045 { 00046 unsigned char Blue; 00047 unsigned char Green; 00048 unsigned char Red; 00049 unsigned char Alpha; 00050 }; 00051 float float_value; 00052 long long_value; 00053 } RGBValue; 00054 00055 class PlanarRemoval { 00056 public: 00057 PlanarRemoval(); 00058 00059 private: 00060 00061 double distance_threshold_; 00062 double cloud_remaining_proportion_threshold_; 00063 int max_num_of_planes_removed_; 00064 00065 CMutex mutex; 00066 00067 cv_bridge::CvImage cv_image; 00068 00069 sensor_msgs::Image image_msg_; 00070 00071 ros::NodeHandle nh_; 00072 00073 pcl::PointCloud<pcl::PointXYZ> pcl_xyz_; 00074 pcl::PointCloud<pcl::PointXYZRGB> pcl_xyzrgb_; 00075 pcl::PointCloud<pcl::PointXYZ> pcl_xyzrgb2_; 00076 pcl::PointCloud<pcl::PointXYZ> pcl_planes_xyz_; 00077 pcl::PointCloud<pcl::PointXYZRGB> pcl_planes_xyzrgb_; 00078 00085 void removePlanesXYZRGB(pcl::PointCloud<pcl::PointXYZRGB> cloud, pcl::PointCloud<pcl::PointXYZRGB>& cloud_filtered_rgb, pcl::PointCloud<pcl::PointXYZRGB>& cloud_filtered, pcl::PointCloud<pcl::PointXYZRGB>& cloud_planes ); 00086 00090 void image_from_sparse_pcl2(pcl::PointCloud<pcl::PointXYZRGB> cloud, cv::Mat& rgb_image); 00091 00092 // [Subscribers] 00093 ros::Subscriber pcl2_sub_; 00094 00095 // [Publishers] 00096 ros::Publisher pcl2_pub_; 00097 ros::Publisher pcl2_planes_pub_; 00098 ros::Publisher coefficients_pub_; 00099 image_transport::ImageTransport it_; 00100 00101 sensor_msgs::PointCloud2 pcl2_filtered_msg_; 00102 sensor_msgs::PointCloud2 pcl2_planes_msg_; 00103 00104 image_transport::Publisher image_pub_; 00105 00106 dynamic_reconfigure::Server<iri_pcl_filters::IriPclFiltersParametersConfig> dyn_reconfig_srv; 00107 00108 void reconfigureCallback(iri_pcl_filters::IriPclFiltersParametersConfig &config, uint32_t level); 00109 00110 void pcl2_sub_callback(const sensor_msgs::PointCloud2::ConstPtr& msg); 00111 }; 00112 #endif