amplitude_filter.cpp

Go to the documentation of this file.

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

// pcl includes
#define PCL_NO_PRECOMPILE
#include <pcl/filters/passthrough.h>
#include <pcl/filters/impl/passthrough.hpp>
#include <pcl/impl/pcl_base.hpp>

// cob_3d_mapping_filters includes
#include <cob_3d_mapping_common/point_types.h>

//######################
//#### nodelet class####
class AmplitudeFilter : public nodelet::Nodelet
{
public:
  typedef PointXYZA PointT;
  typedef pcl::PointCloud<PointT> PointCloud;

  // Constructor
  AmplitudeFilter ()
  {
    //
  }

  // Destructor
  ~ AmplitudeFilter ()
  {
  }

  void
  onInit ()
  {
    n_ = getPrivateNodeHandle ();

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

    int lim_min, lim_max;
    n_.param ("amplitude_min_threshold", lim_min, 2000);
    n_.param ("amplitude_max_threshold", lim_max, 60000);
    filter_.setFilterLimits (lim_min, lim_max);
    filter_.setFilterFieldName ("amplitude");
  }

  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, AmplitudeFilter, AmplitudeFilter, nodelet::Nodelet)