main.cpp 2.05 KB
Newer Older
Marius's avatar
wer  
Marius committed
1 2 3 4 5 6 7 8 9
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <geometry_msgs/Pose.h>
#include <chrono>
#include <comon/clockhelper.hpp>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
Marius's avatar
Marius committed
10
#include <pcl/filters/statistical_outlier_removal.h>
Marius's avatar
wer  
Marius committed
11

Marius's avatar
Marius committed
12
// Less typing. We are using a pointcloud that contains XYZ and addtionally RGBA data.
Marius's avatar
wer  
Marius committed
13 14 15 16 17 18 19 20 21 22 23 24 25 26 27
using CloudRGBA = pcl::PointCloud<pcl::PointXYZRGBA>;

// We are going to publish clouds ... Therefore we need a publisher!
ros::Publisher publisher;

// We are going to receive clouds ... Therefore we need a subscriber!
ros::Subscriber subscriber;


// The callback function. It gets called whenever we receive a new point loucd.
void callback(CloudRGBA::ConstPtr msgPtr) {
    // Print some information
    ROS_INFO("Received a cloud with %i points", msgPtr->size());


Marius's avatar
asd  
Marius committed
28
    // newline
Marius's avatar
Marius committed
29 30 31 32 33 34

    pcl::VoxelGrid<pcl::PointXYZRGBA> voxelFilter;
    voxelFilter.setInputCloud (msgPtr);

    /* ######### <MISSING> ########## */

Marius's avatar
wer  
Marius committed
35
    auto filteredPtr = boost::make_shared<CloudRGBA>();
Marius's avatar
Marius committed
36 37
    voxelFilter.filter(*filteredPtr);

Marius's avatar
wer  
Marius committed
38 39 40


    // Set the header of the message. Just some shabby details ...
Marius's avatar
Marius committed
41
    filteredPtr->header.frame_id = ""; /* ######### <MISSING> ########## */
Marius's avatar
wer  
Marius committed
42 43
    pcl_conversions::toPCL(ros::Time::now(), filteredPtr->header.stamp);

Marius's avatar
Marius committed
44

Marius's avatar
wer  
Marius committed
45 46 47 48 49 50 51

    // Print some information
    ROS_INFO("Published a cloud with %i points", filteredPtr->size());
}

int main(int argc, char** argv){
    // Init ROS stuff
Marius's avatar
Marius committed
52
    ros::init(argc, argv, "crs_cloud_processor");
Marius's avatar
wer  
Marius committed
53 54
    auto node = ros::NodeHandle();

Marius's avatar
Marius committed
55 56 57 58 59 60 61 62 63 64 65

    /* ######### <MISSING> ########## */
    /* ######### <MISSING> ########## */

    std::string subscribeTopic;
    /* ######### <MISSING> ########## */

    std::string advertiseTopic;
    /* ######### <MISSING> ########## */


Marius's avatar
wer  
Marius committed
66
    // Init subscriber and set topic
Marius's avatar
Marius committed
67
    subscriber = node.subscribe<CloudRGBA>(subscribeTopic, 1, callback);
Marius's avatar
wer  
Marius committed
68 69

    // Init publisher and set topic
Marius's avatar
Marius committed
70 71
    publisher = node.advertise<CloudRGBA>(advertiseTopic, 5);

Marius's avatar
wer  
Marius committed
72

Marius's avatar
Marius committed
73
    /* ######### <MISSING> ########## */
Marius's avatar
wer  
Marius committed
74 75

    return 0;
Marius's avatar
Marius committed
76
};