Segmenter.h
Go to the documentation of this file.
00001 
00013 #ifndef RAIL_SEGMENTATION_SEGMENTER_H_
00014 #define RAIL_SEGMENTATION_SEGMENTER_H_
00015 
00016 // RAIL Segmentation
00017 #include "SegmentationZone.h"
00018 
00019 // ROS
00020 #include <pcl_ros/point_cloud.h>
00021 #include <rail_manipulation_msgs/SegmentedObjectList.h>
00022 #include <rail_segmentation/RemoveObject.h>
00023 #include <ros/ros.h>
00024 #include <sensor_msgs/Image.h>
00025 #include <sensor_msgs/PointCloud2.h>
00026 #include <std_srvs/Empty.h>
00027 #include <tf/transform_listener.h>
00028 #include <tf2_ros/transform_listener.h>
00029 #include <visualization_msgs/Marker.h>
00030 #include <visualization_msgs/MarkerArray.h>
00031 
00032 // PCL
00033 #include <pcl/filters/conditional_removal.h>
00034 #include <pcl/point_types.h>
00035 #include <pcl/point_cloud.h>
00036 
00037 // BOOST
00038 #include <boost/thread/mutex.hpp>
00039 
00040 // C++ Standard Library
00041 #include <string>
00042 
00043 namespace rail
00044 {
00045 namespace segmentation
00046 {
00047 
00055 class Segmenter
00056 {
00057 public:
00059   static const bool DEFAULT_DEBUG = false;
00061   static const double SAC_EPS_ANGLE = 0.15;
00063   static const double SAC_DISTANCE_THRESHOLD = 0.01;
00065   static const int SAC_MAX_ITERATIONS = 100;
00067   static const double SURFACE_REMOVAL_PADDING = 0.005;
00069   static const int DEFAULT_MIN_CLUSTER_SIZE = 200;
00071   static const int DEFAULT_MAX_CLUSTER_SIZE = 10000;
00073   static const double CLUSTER_TOLERANCE = 0.02;
00075   static const float DOWNSAMPLE_LEAF_SIZE = 0.01;
00077   static const double MARKER_SCALE = 0.01;
00078 
00085   Segmenter();
00086 
00094   bool okay() const;
00095 
00096 private:
00104   void pointCloudCallback(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr &pc);
00105 
00115   const SegmentationZone &getCurrentZone() const;
00116 
00127   bool removeObjectCallback(rail_segmentation::RemoveObject::Request &req,
00128       rail_segmentation::RemoveObject::Response &res);
00129 
00140   bool clearCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res);
00141 
00152   bool segmentCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res);
00153 
00168   rail_manipulation_msgs::SegmentedObject findSurface(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr &in, const pcl::IndicesConstPtr &indices_in,
00169       const SegmentationZone &zone, const pcl::IndicesPtr &indices_out) const;
00170 
00180   void extractClusters(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr &in, const pcl::IndicesConstPtr &indices_in,
00181       std::vector<pcl::PointIndices> &clusters) const;
00182 
00193   void inverseBound(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr &in, const pcl::IndicesConstPtr &indices_in,
00194       const pcl::ConditionBase<pcl::PointXYZRGB>::Ptr &conditions, const pcl::IndicesPtr &indices_out) const;
00195 
00205   void extract(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr &in, const pcl::IndicesConstPtr &indices_in,
00206       const pcl::PointCloud<pcl::PointXYZRGB>::Ptr &out) const;
00207 
00216   double averageZ(const std::vector<pcl::PointXYZRGB, Eigen::aligned_allocator<pcl::PointXYZRGB> > &v) const;
00217 
00226   visualization_msgs::Marker createMarker(const pcl::PCLPointCloud2::ConstPtr &pc) const;
00227 
00237   sensor_msgs::Image createImage(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr &in,
00238       const pcl::PointIndices &cluster) const;
00239 
00241   bool debug_, okay_, first_pc_in_;
00243   int min_cluster_size_, max_cluster_size_;
00245   boost::mutex pc_mutex_, msg_mutex_;
00247   std::vector<SegmentationZone> zones_;
00248 
00250   ros::NodeHandle node_, private_node_;
00252   ros::ServiceServer segment_srv_, clear_srv_, remove_object_srv_;
00254   ros::Publisher segmented_objects_pub_, table_pub_, markers_pub_, table_marker_pub_, debug_pc_pub_, debug_img_pub_;
00256   ros::Subscriber point_cloud_sub_;
00258   tf::TransformListener tf_;
00260   tf2_ros::Buffer tf_buffer_;
00262   tf2_ros::TransformListener tf2_;
00263 
00265   pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr pc_;
00267   rail_manipulation_msgs::SegmentedObjectList object_list_;
00269   rail_manipulation_msgs::SegmentedObject table_;
00271   visualization_msgs::MarkerArray markers_;
00273   visualization_msgs::Marker table_marker_;
00274 
00275 };
00276 
00277 }
00278 }
00279 
00280 #endif


rail_segmentation
Author(s): Russell Toris , David Kent
autogenerated on Tue Sep 20 2016 03:52:24