Ask Your Question
0

Fast access to multiple point clouds

asked 2018-05-14 02:26:54 -0600

Ravi Joshi gravatar image

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.

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2018-05-14 05:01:20 -0600

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.

edit flag offensive delete link more

Comments

Suddenly I learned something new! Though, it turned out a particular case. Thanks a lot.

Ravi Joshi gravatar imageRavi Joshi ( 2018-05-15 02:03:20 -0600 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2018-05-14 02:26:54 -0600

Seen: 54 times

Last updated: May 14 '18