height_from_plane_map.h

Go to the documentation of this file.

#ifndef _HEIGHT_FROM_PLANE_MAP_H_
#define _HEIGHT_FROM_PLANE_MAP_H_

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

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

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

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

//iriutils
#include "mutex.h"

// [OpenCV]
// [publisher subscriber headers]
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include "sensor_msgs/PointCloud2.h"

// [services]
#include <dynamic_reconfigure/server.h>
#include <iri_pcl_filters/IriPclFiltersParametersConfig.h>

// static const char WINDOW[] = "Image window";

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

class HeightFromPlaneMap {
    public:
      HeightFromPlaneMap();
     
    private:

      CMutex mutex;

      ros::NodeHandle nh_;

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

      message_filters::TimeSynchronizer<pcl::ModelCoefficients, sensor_msgs::PointCloud2> *sync_;
      // [Subscribers]
      message_filters::Subscriber<pcl::ModelCoefficients> coefficients_sub_;
      message_filters::Subscriber<sensor_msgs::PointCloud2> pcl2_sub_; 

      // [Publishers]
      ros::Publisher pcl2_pub_;
      sensor_msgs::PointCloud2 pcl2_score_map_msg_;

      dynamic_reconfigure::Server<iri_pcl_filters::IriPclFiltersParametersConfig> dyn_reconfig_srv;
      
      void reconfigureCallback(iri_pcl_filters::IriPclFiltersParametersConfig &config, uint32_t level);

      void height_map_callback(const pcl::ModelCoefficients::ConstPtr& coefficients_msg, const sensor_msgs::PointCloud2::ConstPtr& pcl2_msg);
      
      float distance_from_plane(double x, double y, double z, std::vector<float> coefs);

};
#endif