jump_edge_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>

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

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

  // Constructor
  JumpEdgeFilter ()
  {
  }

  // Destructor
  ~ JumpEdgeFilter ()
  {
  }

  void
  onInit ()
  {
    n_ = getPrivateNodeHandle ();

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

    double angle;
    n_.param ("upper_angle_deg", angle, 170.0);
    filter_.setAngleThreshold (angle);
  }

  void
  pointCloudSubCallback (pcl::PointCloud<pcl::PointXYZI>::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::JumpEdgeFilter<PointT> filter_;
};

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