point_cloud_aggregator.cpp

Go to the documentation of this file.

#include <ros/ros.h>
#include <pluginlib/class_list_macros.h>
#include <nodelet/nodelet.h>

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>

#include <tf/transform_listener.h>

#include <sensor_msgs/PointCloud2.h>

#include <image_transport/image_transport.h>
#include <image_transport/subscriber_filter.h>

#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>

#include <rtabmap_ros/MsgConversion.h>

namespace rtabmap_ros
{

class PointCloudAggregator : public nodelet::Nodelet
{
public:
        PointCloudAggregator() : sync(NULL)
        {}

        virtual ~PointCloudAggregator()
        {
            if (sync!=NULL) delete sync;
        }

private:
        void clouds_callback(const sensor_msgs::PointCloud2ConstPtr & cloudMsg_1,
                             const sensor_msgs::PointCloud2ConstPtr & cloudMsg_2,
                             const sensor_msgs::PointCloud2ConstPtr & cloudMsg_3)
        {
                if(cloudPub_.getNumSubscribers())
                {
                        pcl::fromROSMsg(*cloudMsg_1, cloud1);
                        pcl::fromROSMsg(*cloudMsg_2, cloud2);
                        pcl::fromROSMsg(*cloudMsg_3, cloud3);
                    pcl::PointCloud<pcl::PointXYZ> totalCloud;
                    totalCloud = cloud1 + cloud2;
                    totalCloud += cloud3;
                    sensor_msgs::PointCloud2 rosCloud;
                    pcl::toROSMsg(totalCloud, rosCloud);
                    rosCloud.header.stamp = cloudMsg_1->header.stamp;
                    rosCloud.header.frame_id = cloudMsg_1->header.frame_id;
                    cloudPub_.publish(rosCloud);
                }
        }

    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2, sensor_msgs::PointCloud2, sensor_msgs::PointCloud2> MySyncPolicy;
        virtual void onInit()
        {
                ros::NodeHandle & nh = getNodeHandle();
                ros::NodeHandle & pnh = getPrivateNodeHandle();

                int queueSize = 5;
                pnh.param("queue_size", queueSize, queueSize);

                cloudSub_1_.subscribe(nh, "cloud1", 1);
                cloudSub_2_.subscribe(nh, "cloud2", 1);
                cloudSub_3_.subscribe(nh, "cloud3", 1);

        sync = new message_filters::Synchronizer<MySyncPolicy>(MySyncPolicy(queueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_);
        sync->registerCallback(boost::bind(&rtabmap_ros::PointCloudAggregator::clouds_callback, this, _1, _2, _3));

                cloudPub_ = nh.advertise<sensor_msgs::PointCloud2>("combined_cloud", 1);
        }

    message_filters::Synchronizer<MySyncPolicy>* sync;
        message_filters::Subscriber<sensor_msgs::PointCloud2> cloudSub_1_;
        message_filters::Subscriber<sensor_msgs::PointCloud2> cloudSub_2_;
        message_filters::Subscriber<sensor_msgs::PointCloud2> cloudSub_3_;
        pcl::PointCloud<pcl::PointXYZ> cloud1, cloud2, cloud3;

        ros::Publisher cloudPub_;
};

PLUGINLIB_EXPORT_CLASS(rtabmap_ros::PointCloudAggregator, nodelet::Nodelet);
}