speckle_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
#include <pcl/point_types.h>

// cob_3d_mapping_filters includes
//#include <cob_3d_mapping_common/point_types.h>
#include <cob_3d_mapping_filters/speckle_filter.h>
#include <cob_3d_mapping_filters/impl/speckle_filter.hpp>

//######################
//#### nodelet class####
class SpeckleFilter : public nodelet::Nodelet
{
public:
  typedef pcl::PointXYZ PointT;
  typedef pcl::PointCloud<PointT> PointCloud;

  // Constructor
  SpeckleFilter ()
  {
    //
  }

  // Destructor
  ~ SpeckleFilter ()
  {
  }

  void
  onInit ()
  {
    n_ = getPrivateNodeHandle ();

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

    int speckle_size;
    double speckle_range;
    n_.param ("speckle_size", speckle_size, 50);
    n_.param ("speckle_range", speckle_range, 0.1);
    filter_.setFilterParam (speckle_size, speckle_range);
  }

  void
  pointCloudSubCallback (const 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_;

  cob_3d_mapping_filters::SpeckleFilter<PointT> filter_;
};

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