36 #include <visualization_msgs/Marker.h>
37 #include <sensor_msgs/LaserScan.h>
38 #include <sensor_msgs/PointCloud2.h>
39 #include <tf/transform_listener.h>
40 #include <pcl_ros/transforms.h>
41 #include <pcl/conversions.h>
42 #include <pcl/point_cloud.h>
43 #include <pcl/point_types.h>
44 #include <pcl/segmentation/extract_polygonal_prism_data.h>
45 #include <pcl/filters/extract_indices.h>
46 #include <pcl/filters/project_inliers.h>
47 #include <pcl/filters/voxel_grid.h>
48 #include <points_from_volume/points_from_volume.h>
49 #define PFLN printf("FILE %s LINE %d\n",__FILE__, __LINE__);
64 void conversion (
const sensor_msgs::PointCloud2ConstPtr & pcmsg_in)
66 pcl::PointCloud<pcl::PointXYZ> pc_cut;
67 pcl::PointCloud<pcl::PointXYZ> processed_pc;
68 sensor_msgs::PointCloud2 pcmsg_out;
70 pcl::fromROSMsg(*pcmsg_in,pc_cut);
71 pc_cut.header.frame_id=pcmsg_in->header.frame_id;
73 for (
int i=0;i<(int)pc_cut.points.size();i++)
75 if (pc_cut.points[i].z>0.75 && pc_cut.points[i].z<1.5)
77 processed_pc.push_back(pc_cut.points[i]);
80 processed_pc.header.frame_id=pc_cut.header.frame_id;
81 pcl::toROSMsg(processed_pc,pcmsg_out);
83 pcmsg_out.header.stamp=pcmsg_in->header.stamp;
95 int main(
int argc,
char **argv)
97 ros::init(argc, argv,
"kinect_freq_mod");
99 ros::Rate loop_rate(10);
103 ros::Subscriber sub = n.subscribe (
"/point_cloud_from_kinect", 1,
conversion);
106 cloud_pub = n.advertise<sensor_msgs::PointCloud2>(
"/point_cloud_input", 1);
void conversion(const sensor_msgs::PointCloud2ConstPtr &pcmsg_in)
Point cloud treatment.
int main(int argc, char **argv)
Publish messages at a certain loop rate.