/home/joel/ros_workspace/lar3/perception/parking_detection/src/kinect_freq_mod.cpp

Go to the documentation of this file.

#include <stdio.h>
#include <ros/ros.h>
#include <visualization_msgs/Marker.h>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/PointCloud2.h>
#include <tf/transform_listener.h>
#include <pcl_ros/transforms.h>
#include <pcl/ros/conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/segmentation/extract_polygonal_prism_data.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/project_inliers.h>
#include <pcl/filters/voxel_grid.h>
#include <points_from_volume/points_from_volume.h>
#define PFLN printf("FILE %s LINE %d\n",__FILE__, __LINE__);

// Namespaces
using namespace std;

// Global vars
ros::Publisher cloud_pub;
ros::NodeHandle* p_n;

void conversion (const sensor_msgs::PointCloud2ConstPtr & pcmsg_in)
{
  pcl::PointCloud<pcl::PointXYZ> pc_cut;
  pcl::PointCloud<pcl::PointXYZ> processed_pc;
  sensor_msgs::PointCloud2 pcmsg_out;
  
  pcl::fromROSMsg(*pcmsg_in,pc_cut);
  pc_cut.header.frame_id=pcmsg_in->header.frame_id;

  for (int i=0;i<(int)pc_cut.points.size();i++)
  {
    if (pc_cut.points[i].z>0.75 && pc_cut.points[i].z<1.5)
    {
      processed_pc.push_back(pc_cut.points[i]);
    }
  }
  processed_pc.header.frame_id=pc_cut.header.frame_id;
  pcl::toROSMsg(processed_pc,pcmsg_out);
  
  pcmsg_out.header.stamp=pcmsg_in->header.stamp;
  cloud_pub.publish(pcmsg_out);
}



int main(int argc, char **argv)
{
  ros::init(argc, argv, "kinect_freq_mod");                             
  ros::NodeHandle n;
  ros::Rate loop_rate(10);
  p_n=&n;
  
  // Subscribe of the Kinect point cloud message
  ros::Subscriber sub = n.subscribe ("/point_cloud_from_kinect", 1, conversion);
  
  // Advertise of the resutant point cloud
  cloud_pub = n.advertise<sensor_msgs::PointCloud2>("/point_cloud_input", 1);
  
  while(ros::ok())
  {
        ros::spinOnce();
        loop_rate.sleep();
  }
  return 0;
}