pcl_to_image.h

Go to the documentation of this file.

#ifndef _PCL2IMAGE_H_
#define _PCL2IMAGE_H_

// [Eigen]
#include "Eigen/Core"

// [ROS]
#include "ros/ros.h"
#include "ros/callback_queue.h"
#include "image_transport/image_transport.h" 

// [PCL_ROS]
#include <pcl/ros/conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

#include <pcl/sample_consensus/model_types.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/ros/register_point_struct.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/extract_indices.h>

//pcl::toROSMsg
#include <pcl/io/pcd_io.h>

//iriutils
#include "mutex.h"

// [OpenCV]
 #include <cv_bridge/cv_bridge.h>
 #include <opencv2/imgproc/imgproc.hpp>
 #include <opencv2/highgui/highgui.hpp>

// [publisher subscriber headers]
#include "sensor_msgs/Image.h"
#include "sensor_msgs/PointCloud2.h"


typedef union {
    struct /*anonymous*/
    {
      unsigned char Blue;
      unsigned char Green;
      unsigned char Red;
      unsigned char Alpha;
    };
    float float_value;
    long long_value;
} RGBValue;

class PCLtoImage {
    public:
      PCLtoImage();
     
    private:

      double distance_threshold_;
      double cloud_remaining_proportion_threshold_;

      CMutex mutex;

      cv_bridge::CvImage cv_image;

      sensor_msgs::Image image_msg_;
      
      ros::NodeHandle nh_;

      pcl::PointCloud<pcl::PointXYZRGB> pcl_xyzrgb_;
      pcl::PointCloud<pcl::PointXYZ> pcl_xyzrgb2_;

      void image_from_sparse_pcl2(pcl::PointCloud<pcl::PointXYZRGB> cloud, cv::Mat& rgb_image);

      // [Subscribers]
      ros::Subscriber pcl2_sub_;

      // [Publishers]
      image_transport::ImageTransport it_;
      image_transport::Publisher image_pub_;

      void pcl2_sub_callback(const sensor_msgs::PointCloud2::ConstPtr& msg);
};
#endif