simple_segmentation_nodelet.h

Go to the documentation of this file.

#ifndef __SIMPLE_SEGMENTATION_NODELET_H__
#define __SIMPLE_SEGMENTATION_NODELET_H__

#include <boost/thread.hpp>

// PCL includes
//#include <pcl/surface/concave_hull.h>
#include <pcl/point_types.h>

// ROS includes
#include <nodelet/nodelet.h>
#include <pcl_ros/transforms.h>
#include <pcl_ros/point_cloud.h>
#include <tf_conversions/tf_eigen.h>
#include <dynamic_reconfigure/server.h>
#include <cob_3d_segmentation/segmentation_nodeletConfig.h>
#include <actionlib/server/simple_action_server.h>

// Package includes
#include "cob_3d_mapping_msgs/TriggerAction.h"
#include "cob_3d_mapping_common/point_types.h"
#include "cob_3d_features/organized_normal_estimation_omp.h"
#include "cob_3d_segmentation/impl/fast_segmentation.hpp"
#include <cob_3d_segmentation/cluster_conversion.h>

namespace cob_3d_segmentation
{
  class SimpleSegmentationNodelet : public nodelet::Nodelet
  {
    public:
    typedef pcl::PointCloud<pcl::PointXYZRGB> PointCloud;
    typedef pcl::PointCloud<pcl::Normal> NormalCloud;
    typedef pcl::PointCloud<PointLabel> LabelCloud;
    typedef FastSegmentation<pcl::PointXYZRGB, pcl::Normal, PointLabel>::ClusterPtr ClusterPtr;

    public:
    SimpleSegmentationNodelet()
      : one_()
      , seg_()
      , down_(new PointCloud)
      , segmented_(new PointCloud)
      , normals_(new NormalCloud)
      , labels_(new LabelCloud)
      , centroid_passthrough_(5.0f)
      , min_cluster_size_(100)
      , filter_(false)
      , downsample_(false)
      , colorize_(true)
      , enable_action_mode_(false)
      , is_running_(false)
    { }

    ~SimpleSegmentationNodelet()
    { if(as_) delete as_; }

    protected:
    void onInit();
    void configCallback(cob_3d_segmentation::segmentation_nodeletConfig& config, uint32_t levels);

    void actionCallback(const cob_3d_mapping_msgs::TriggerGoalConstPtr& goal);
    void topicCallback(const PointCloud::ConstPtr& cloud);
    void computeAndPublish();
    void publishClassifiedCloud();
    void computeTexture(ClusterPtr &c, Eigen::Affine3f &trf, unsigned int id);

    boost::mutex mutex_;

    ros::NodeHandle nh_;
    ros::Subscriber sub_points_;
    ros::Publisher pub_segmented_;
    ros::Publisher pub_classified_;
    ros::Publisher pub_shape_array_;
    actionlib::SimpleActionServer<cob_3d_mapping_msgs::TriggerAction>* as_;

    boost::shared_ptr<dynamic_reconfigure::Server<cob_3d_segmentation::segmentation_nodeletConfig> > config_server_;

    cob_3d_features::OrganizedNormalEstimationOMP<pcl::PointXYZRGB, pcl::Normal, PointLabel> one_;
    FastSegmentation<pcl::PointXYZRGB, pcl::Normal, PointLabel> seg_;
    ClusterConversion<pcl::PointXYZRGB> conv_;

    PointCloud::Ptr down_;
    PointCloud::Ptr segmented_;
    NormalCloud::Ptr normals_;
    LabelCloud::Ptr labels_;

    float centroid_passthrough_;
    int min_cluster_size_;
    bool filter_;
    bool downsample_;
    bool colorize_;
    bool enable_action_mode_;
    bool is_running_;
  };
}


#endif