Fast access to multiple point clouds

edit

Hi,

I am using multiple Kinect sensors. I am trying to get these point clouds in real-time (as quickly as possible). I thought of creating multiple subscribers and use an array to access the data as this may be the fastest way to access point clouds. However, it didn't work. Please see the code below-

class PointCloudSubscriber
{
private:
    ros::Subscriber subscriber;
    void callback(const sensor_msgs::PointCloud2ConstPtr& msg);

public:
    std::string topic;
    sensor_msgs::PointCloud2 point_cloud;
    PointCloudSubscriber(ros::NodeHandle& node_handle, std::string topic, int queue_size);
};

void PointCloudSubscriber::callback(const sensor_msgs::PointCloud2ConstPtr& msg)
{
    // below commented line works
    // ROS_INFO_STREAM("Cloud [" << topic << "] size = " << msg->data.size());
    point_cloud = *msg;
}

PointCloudSubscriber::PointCloudSubscriber(ros::NodeHandle& node_handle, std::string topic_name, int queue_size)
{
    topic = topic_name;
    subscriber = node_handle.subscribe<sensor_msgs::PointCloud2>(
        topic, queue_size, &PointCloudSubscriber::callback, this);
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "listener_class");
    ros::NodeHandle nh("~");

    int queue_size = 1;
    PointCloudSubscriber pc1(nh, "/kinect1/sd/points", queue_size);
    PointCloudSubscriber pc2(nh, "/kinect2/sd/points", queue_size);
    PointCloudSubscriber pc3(nh, "/kinect3/sd/points", queue_size);

    PointCloudSubscriber pcs[] = { pc1, pc2, pc3 };
    ros::Rate loop_rate(10);
    while (ros::ok())
    {
        for (size_t i = 0; i < 3; i++)
            ROS_INFO_STREAM("Cloud [" << pcs[i].topic << "] size = " << pcs[i].point_cloud.data.size());

        ros::spinOnce();
        loop_rate.sleep();
    }

    return 0;
}

The print statement inside for loop always prints size = 0. However topic is printed correctly. The print state inside callback works as expected.

Please note that I also tried using message filters but I see a small delay in the data. Below is the code snippet-

void MultiplePointClouds::callback(
    const sensor_msgs::PointCloud2ConstPtr& msg1,
    const sensor_msgs::PointCloud2ConstPtr& msg2,
    const sensor_msgs::PointCloud2ConstPtr& msg3)
{
    ROS_DEBUG_STREAM("MultiplePointClouds callback received");
}

MultiplePointClouds::MultiplePointClouds()
{
    message_filters::Subscriber<sensor_msgs::PointCloud2> pc1_sub(nh, pc1_topic, 1);
    message_filters::Subscriber<sensor_msgs::PointCloud2> pc2_sub(nh, pc2_topic, 1);
    message_filters::Subscriber<sensor_msgs::PointCloud2> pc3_sub(nh, pc3_topic, 1);

    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2,
        sensor_msgs::PointCloud2, sensor_msgs::PointCloud2> SyncPolicy;
    message_filters::Synchronizer<SyncPolicy> sync(SyncPolicy(10), pc1_sub, pc2_sub, pc3_sub);
    sync.registerCallback(boost::bind(&MultiplePointClouds::callback, this, _1, _2, _3));

    ros::spin();
}

Any suggestions, please? My goal is to access all three point clouds in real-time at one place.

PS: I am using ROS Indigo on Ubuntu 14.04 LTS PC.