Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038 #ifndef PCL_ROS_SAC_SEGMENTATION_H_
00039 #define PCL_ROS_SAC_SEGMENTATION_H_
00040
00041 #include "pcl_ros/pcl_nodelet.h"
00042 #include <message_filters/pass_through.h>
00043
00044
00045 #include <pcl/segmentation/sac_segmentation.h>
00046
00047
00048 #include <dynamic_reconfigure/server.h>
00049 #include "pcl_ros/SACSegmentationConfig.h"
00050 #include "pcl_ros/SACSegmentationFromNormalsConfig.h"
00051
00052 namespace pcl_ros
00053 {
00054 namespace sync_policies = message_filters::sync_policies;
00055
00057
00061 class SACSegmentation : public PCLNodelet
00062 {
00063 typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
00064 typedef PointCloud::Ptr PointCloudPtr;
00065 typedef PointCloud::ConstPtr PointCloudConstPtr;
00066
00067 public:
00069 SACSegmentation () : min_inliers_(0) {}
00070
00074 inline void setInputTFframe (std::string tf_frame) { tf_input_frame_ = tf_frame; }
00075
00077 inline std::string getInputTFframe () { return (tf_input_frame_); }
00078
00082 inline void setOutputTFframe (std::string tf_frame) { tf_output_frame_ = tf_frame; }
00083
00085 inline std::string getOutputTFframe () { return (tf_output_frame_); }
00086
00087 protected:
00088
00089 int min_inliers_;
00090
00091
00093 ros::Publisher pub_indices_;
00094
00096 ros::Publisher pub_model_;
00097
00099 ros::Subscriber sub_input_;
00100
00102 boost::shared_ptr<dynamic_reconfigure::Server<SACSegmentationConfig> > srv_;
00103
00105 std::string tf_input_frame_;
00106
00108 std::string tf_input_orig_frame_;
00109
00111 std::string tf_output_frame_;
00112
00115 message_filters::PassThrough<PointIndices> nf_pi_;
00116
00118 virtual void onInit ();
00119
00124 void config_callback (SACSegmentationConfig &config, uint32_t level);
00125
00130 void input_indices_callback (const PointCloudConstPtr &cloud, const PointIndicesConstPtr &indices);
00131
00135 PointIndices indices_;
00136
00140 inline void
00141 indices_callback (const PointIndicesConstPtr &indices)
00142 {
00143 indices_ = *indices;
00144 }
00145
00149 inline void
00150 input_callback (const PointCloudConstPtr &input)
00151 {
00152 indices_.header = input->header;
00153 PointIndicesConstPtr indices;
00154 indices.reset (new PointIndices (indices_));
00155 nf_pi_.add (indices);
00156 }
00157
00158 private:
00160 boost::mutex mutex_;
00161
00163 pcl::SACSegmentation<pcl::PointXYZ> impl_;
00164
00166 boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointIndices> > > sync_input_indices_e_;
00167 boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, PointIndices> > > sync_input_indices_a_;
00168
00169 public:
00170 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00171 };
00172
00174
00177 class SACSegmentationFromNormals: public SACSegmentation
00178 {
00179 typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
00180 typedef PointCloud::Ptr PointCloudPtr;
00181 typedef PointCloud::ConstPtr PointCloudConstPtr;
00182
00183 typedef pcl::PointCloud<pcl::Normal> PointCloudN;
00184 typedef PointCloudN::Ptr PointCloudNPtr;
00185 typedef PointCloudN::ConstPtr PointCloudNConstPtr;
00186
00187 public:
00191 inline void setInputTFframe (std::string tf_frame) { tf_input_frame_ = tf_frame; }
00192
00194 inline std::string getInputTFframe () { return (tf_input_frame_); }
00195
00199 inline void setOutputTFframe (std::string tf_frame) { tf_output_frame_ = tf_frame; }
00200
00202 inline std::string getOutputTFframe () { return (tf_output_frame_); }
00203
00204 protected:
00205
00207 message_filters::Subscriber<PointCloudN> sub_normals_filter_;
00208
00210 ros::Subscriber sub_axis_;
00211
00213 boost::shared_ptr<dynamic_reconfigure::Server<SACSegmentationFromNormalsConfig> > srv_;
00214
00219 inline void
00220 input_callback (const PointCloudConstPtr &cloud)
00221 {
00222 PointIndices indices;
00223 indices.header.stamp = cloud->header.stamp;
00224 nf_.add (boost::make_shared<PointIndices> (indices));
00225 }
00226
00229 message_filters::PassThrough<PointIndices> nf_;
00230
00232 std::string tf_input_frame_;
00234 std::string tf_input_orig_frame_;
00236 std::string tf_output_frame_;
00237
00239 virtual void onInit ();
00240
00244 void axis_callback (const pcl::ModelCoefficientsConstPtr &model);
00245
00250 void config_callback (SACSegmentationFromNormalsConfig &config, uint32_t level);
00251
00257 void input_normals_indices_callback (const PointCloudConstPtr &cloud,
00258 const PointCloudNConstPtr &cloud_normals,
00259 const PointIndicesConstPtr &indices);
00260
00261 private:
00263 boost::mutex mutex_;
00264
00266 pcl::SACSegmentationFromNormals<pcl::PointXYZ, pcl::Normal> impl_;
00267
00269 boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, PointCloudN, PointIndices> > > sync_input_normals_indices_a_;
00270 boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointCloudN, PointIndices> > > sync_input_normals_indices_e_;
00271
00272 public:
00273 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00274 };
00275 }
00276
00277 #endif //#ifndef PCL_ROS_SAC_SEGMENTATION_H_