Segmenter.h

Go to the documentation of this file.

#ifndef RAIL_SEGMENTATION_SEGMENTER_H_
#define RAIL_SEGMENTATION_SEGMENTER_H_

// RAIL Segmentation
#include "SegmentationZone.h"

// ROS
#include <pcl_ros/point_cloud.h>
#include <rail_manipulation_msgs/SegmentedObjectList.h>
#include <rail_segmentation/RemoveObject.h>
#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/PointCloud2.h>
#include <std_srvs/Empty.h>
#include <tf/transform_listener.h>
#include <tf2_ros/transform_listener.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>

// PCL
#include <pcl/filters/conditional_removal.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>

// BOOST
#include <boost/thread/mutex.hpp>

// C++ Standard Library
#include <string>

namespace rail
{
namespace segmentation
{

class Segmenter
{
public:
  static const bool DEFAULT_DEBUG = false;
  static const double SAC_EPS_ANGLE = 0.15;
  static const double SAC_DISTANCE_THRESHOLD = 0.01;
  static const int SAC_MAX_ITERATIONS = 100;
  static const double SURFACE_REMOVAL_PADDING = 0.005;
  static const int DEFAULT_MIN_CLUSTER_SIZE = 200;
  static const int DEFAULT_MAX_CLUSTER_SIZE = 10000;
  static const double CLUSTER_TOLERANCE = 0.02;
  static const float DOWNSAMPLE_LEAF_SIZE = 0.01;
  static const double MARKER_SCALE = 0.01;

  Segmenter();

  bool okay() const;

private:
  void pointCloudCallback(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr &pc);

  const SegmentationZone &getCurrentZone() const;

  bool removeObjectCallback(rail_segmentation::RemoveObject::Request &req,
      rail_segmentation::RemoveObject::Response &res);

  bool clearCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res);

  bool segmentCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res);

  double findSurface(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr &in, const pcl::IndicesConstPtr &indices_in,
      const double z_min, const double z_max, const pcl::IndicesPtr &indices_out) const;

  void extractClusters(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr &in, const pcl::IndicesConstPtr &indices_in,
      std::vector<pcl::PointIndices> &clusters) const;

  void inverseBound(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr &in, const pcl::IndicesConstPtr &indices_in,
      const pcl::ConditionBase<pcl::PointXYZRGB>::Ptr &conditions, const pcl::IndicesPtr &indices_out) const;

  void extract(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr &in, const pcl::IndicesConstPtr &indices_in,
      const pcl::PointCloud<pcl::PointXYZRGB>::Ptr &out) const;

  double averageZ(const std::vector<pcl::PointXYZRGB, Eigen::aligned_allocator<pcl::PointXYZRGB> > &v) const;

  visualization_msgs::Marker createMarker(const pcl::PCLPointCloud2::ConstPtr &pc) const;

  sensor_msgs::Image createImage(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr &in,
      const pcl::PointIndices &cluster) const;

  bool debug_, okay_, first_pc_in_;
  int min_cluster_size_, max_cluster_size_;
  boost::mutex pc_mutex_, msg_mutex_;
  std::vector<SegmentationZone> zones_;

  ros::NodeHandle node_, private_node_;
  ros::ServiceServer segment_srv_, clear_srv_, remove_object_srv_;
  ros::Publisher segmented_objects_pub_, markers_pub_, debug_pc_pub_, debug_img_pub_;
  ros::Subscriber point_cloud_sub_;
  tf::TransformListener tf_;
  tf2_ros::Buffer tf_buffer_;
  tf2_ros::TransformListener tf2_;

  pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr pc_;
  rail_manipulation_msgs::SegmentedObjectList object_list_;
  visualization_msgs::MarkerArray markers_;
};

}
}

#endif