filter.cpp

Go to the documentation of this file.

//##################
//#### includes ####

// standard includes
//--
#include <sstream>
#include <fstream>


// ROS includes
#include <ros/ros.h>
#include <pcl/point_types.h>
#include <pcl_ros/transforms.h>
#include <pcl_ros/point_cloud.h>
#include <tf/transform_listener.h>
#include <tf_conversions/tf_kdl.h>
#include <pcl/io/pcd_io.h>
#include "cob_3d_registration/RegistrationPCD.h"
#include "cob_3d_registration/preprocessing/kinect_error.h"

#include <cob_3d_registration/registration_icp.h>


using namespace tf;

//####################
//#### node class ####
class FilterNode
{
  typedef pcl::PointXYZRGB Point;

public:
  // Constructor
  FilterNode()
    {
    onInit();
    }


  // Destructor
  ~FilterNode()
  {
  }

  void
  onInit()
  {
    //n_.param("aggregate_point_map/icp_max_iterations" ,icp_max_iterations ,50);

    point_cloud_pub_ = n_.advertise<pcl::PointCloud<Point> >("result_pc",1);
    point_cloud_sub_ = n_.subscribe("point_cloud2", 1, &FilterNode::pointCloudSubCallback, this);

    filter_ser_ = n_.advertiseService("filter_process", &FilterNode::filterService, this);
  }

  void
  pointCloudSubCallback(const pcl::PointCloud<Point>::Ptr& pc_in)
  {
    ROS_INFO("filter");

    pcl::PointCloud<Point> pc;
    preprocessing::KinectErrorGenerator<Point> filter;

    filter.setInputCloud(pc_in);
    filter.filter(pc);

    pc.header.frame_id="/head_cam3d_frame";
    point_cloud_pub_.publish(pc);
  }

  bool filterService(cob_3d_registration::RegistrationPCD::Request  &req,
                     cob_3d_registration::RegistrationPCD::Response &res )
  {
    ROS_INFO("filter...");

    pcl::PointCloud<Point> pc;
    pcl::io::loadPCDFile(req.pcd_fn, pc);

    preprocessing::KinectErrorGenerator<Point> filter;
    filter.setInputCloud(pc.makeShared());
    filter.filter(pc);

    pcl::io::savePCDFileASCII(req.img_fn, pc);

    return true;
  }


  ros::NodeHandle n_;
  ros::Time stamp_;


protected:

  ros::Subscriber point_cloud_sub_;
  ros::Publisher point_cloud_pub_;
  ros::ServiceServer filter_ser_;

};


int main(int argc, char **argv) {
  setVerbosityLevel(pcl::console::L_DEBUG);

  ros::init(argc, argv, "filter");

  FilterNode tn;

  ROS_INFO("Spinning node");
  ros::spin();
  return 0;
}