intensity_filter.cpp

Go to the documentation of this file.

//##################
//#### includes ####
// standard includes
//--
// ROS includes
#include <ros/ros.h>
#include <pluginlib/class_list_macros.h>
#include <nodelet/nodelet.h>
#include <pcl_ros/point_cloud.h>

// pcl includes
#include <pcl/point_types.h>
#include <pcl/filters/passthrough.h>

//######################
//#### nodelet class####
class IntensityFilter : public nodelet::Nodelet
{
public:
  typedef pcl::PointXYZI PointT;
  typedef pcl::PointCloud<PointT> PointCloud;

  // Constructor
  IntensityFilter ()
  {
    //
  }

  // Destructor
  ~ IntensityFilter ()
  {
  }

  void
  onInit ()
  {
    n_ = getPrivateNodeHandle ();

    point_cloud_sub_ = n_.subscribe ("point_cloud_in", 1, &IntensityFilter::pointCloudSubCallback, this);
    point_cloud_pub_ = n_.advertise<PointCloud> ("point_cloud_out", 1);

    int lim_min, lim_max;
    n_.param ("intensity_min_threshold", lim_min, 2000);
    n_.param ("intensity_max_threshold", lim_max, 60000);
    filter_.setFilterLimits (lim_min, lim_max);
    filter_.setFilterFieldName ("intensity");
  }

  void
  pointCloudSubCallback (PointCloud::ConstPtr pc)
  {
    PointCloud cloud_filtered;
    filter_.setInputCloud (pc);
    filter_.filter (cloud_filtered);

    point_cloud_pub_.publish (cloud_filtered);
  }

protected:
  ros::NodeHandle n_;
  ros::Subscriber point_cloud_sub_;
  ros::Publisher point_cloud_pub_;

  pcl::PassThrough<PointT> filter_;
};

PLUGINLIB_DECLARE_CLASS(cob_3d_mapping_filters, IntensityFilter, IntensityFilter, nodelet::Nodelet)