In this example, we will see how to subscribe the generated point cloud on the topic pcl_output. After subscribing the point cloud, we apply a filter called the VoxelGrid class in PCL to down sample the input cloud by keeping the same centroid of the input cloud. You will get the example code pcl_filter.cpp
from the src folder of the package.
#include <ros/ros.h> #include <pcl/point_cloud.h> #include <pcl_conversions/pcl_conversions.h> #include <sensor_msgs/PointCloud2.h> //Vortex filter header #include <pcl/filters/voxel_grid.h> //Creating a class for handling cloud data class cloudHandler { public: cloudHandler() { //Subscribing pcl_output topics from the publisher //This topic can change according to the source of point cloud pcl_sub = nh.subscribe("pcl_output", 10, &cloudHandler::cloudCB, this); //Creating publisher for filtered cloud data pcl_pub = nh.advertise<sensor_msgs::PointCloud2>("pcl_filtered", 1); } //Creating cloud callback void cloudCB(const sensor_msgs::PointCloud2& input) { pcl::PointCloud<pcl::PointXYZ> cloud; pcl::PointCloud<pcl::PointXYZ> cloud_filtered; sensor_msgs::PointCloud2 output; pcl::fromROSMsg(input, cloud); //Creating VoxelGrid object pcl::VoxelGrid<pcl::PointXYZ> vox_obj; //Set input to voxel object vox_obj.setInputCloud (cloud.makeShared()); //Setting parameters of filter such as leaf size vox_obj.setLeafSize (0.1f, 0.1f, 0.1f); //Performing filtering and copy to cloud_filtered variable vox_obj.filter(cloud_filtered); pcl::toROSMsg(cloud_filtered, output); output.header.frame_id = "point_cloud"; pcl_pub.publish(output); } protected: ros::NodeHandle nh; ros::Subscriber pcl_sub; ros::Publisher pcl_pub; }; main(int argc, char** argv) { ros::init(argc, argv, "pcl_filter"); ROS_INFO("Started Filter Node"); cloudHandler handler; ros::spin(); return 0; }
This code subscribes the point cloud topic called /pcl_output, filters using VoxelGrid, and publishes the filtered cloud through the /cloud_filtered topic.