Fast access to multiple point clouds

Question

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.

---

Originally posted by ravijoshi on ROS Answers with karma: 1744 on 2018-05-14

Post score: 0

Answer 1

I would recommend reading up on copying classes in C++ here's a good description. The problem seems to be on this line:

PointCloudSubscriber pcs[] = { pc1, pc2, pc3 };

You've instantiated the pc1 ... pc3 classes using their constructors which in turn has subscribed them to the cloud topics. But the line above then creates three new class instances using the default C++ class assignment behaviour described here.

So now you have 6 instances of the PointCloudSubscriber class three which are receiving cloud messages and a different three which are having their size printed out on the screen (which are always zero because they are not receiving any messages).

You could instantiate the classes directly in the array to avoid this problem like this:

PointCloudSubscriber pcs[3];
pcs[0] = PointCloudSubscriber(nh, "/kinect1/sd/points", queue_size);
pcs[1] = PointCloudSubscriber(nh, "/kinect2/sd/points", queue_size);
pcs[2] = PointCloudSubscriber(nh, "/kinect3/sd/points", queue_size);

Now you'll only have three class instances and it should start behaving the way you expect.

---

Originally posted by PeteBlackerThe3rd with karma: 9529 on 2018-05-14

This answer was ACCEPTED on the original site

Post score: 1

---

Original comments

Comment by ravijoshi on 2018-05-15:
Suddenly I learned something new! Though, it turned out a particular case. Thanks a lot.