Displaying a 3D pointcloud within a ROS node

asked 2019-01-30 10:42:47 -0500

playeronros gravatar image

Hi,

I have a ROS node subscribing to a pointcloud on the /camera/depth_registered/points topic from a RealSense camera. Inside the callback function, I convert this PointCloud2 ROS message to a PCL pointcloud and attempt to display it. When I run this node, the pointcloud displays properly for about 10seconds (I can change the view, zoom etc.) then my computer hangs. Since I am using a smart pointer, I expect this to be automatically deleted. I tested the PCL pcl_visualizer_demo.cpp code and it works just fine (but this is not integrated with ROS).

I would really appreciate some help with this! I have pasted my code below:

boost::shared_ptr<pcl::visualization::PCLVisualizer> rgbVis (pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr cloud)
{
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
    viewer->setBackgroundColor (0, 0, 0);
    pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(cloud);
    viewer->addPointCloud<pcl::PointXYZRGB> (cloud, rgb, "sample cloud");
    viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud");
    viewer->addCoordinateSystem (1.0);
    viewer->initCameraParameters ();
    return (viewer);
}

void viewerRunner(boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer)
{
    while (!viewer->wasStopped ())
    {
        viewer->spinOnce (100);
        boost::this_thread::sleep (boost::posix_time::microseconds (100000));
    }
}

The callback and main function:

void posesCallback(const sensor_msgs::PointCloud2ConstPtr& cloud)
{
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr pcl_cloud (new pcl::PointCloud<pcl::PointXYZRGB>); 
    pcl::fromROSMsg(*cloud, *pcl_cloud);
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
    viewer = rgbVis(pcl_cloud);
    viewerRunner(viewer);
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "Vo3D");
    ros::NodeHandle nh;
    ROS_INFO("About to enter %s" , "Callback");
    ros::Subscriber sub = nh.subscribe<sensor_msgs::PointCloud2>("/camera/depth_registered/points", 1000, posesCallback);
    ros::spin();
    return 0;
}
edit retag flag offensive close merge delete

Comments

2

Your current setup creates new PCLVisualizer instances for every pointcloud that comes in on the topic /camera/depth_registered/points. With a typical depth sensor, that would be about 25 to 30 new instances per second.

Was that your intention?

gvdhoorn gravatar imagegvdhoorn ( 2019-01-30 12:41:51 -0500 )edit

No, I actually want to update the viewer as information arrives. Basically have a real-time viewer of the data

playeronros gravatar imageplayeronros ( 2019-01-31 04:43:40 -0500 )edit

Then this is probably not the way to do that.

Look for a method to "set" or "put" a new pointcloud in an existing PCLVisualizer instance, and update your code to not create new instances, but to pass the incoming pointcloud to one instance of PCLVisualizer that you keep around.

gvdhoorn gravatar imagegvdhoorn ( 2019-01-31 05:13:44 -0500 )edit

Thanks. I've implemented your suggestion and also changed the message queue to 10 (instead of 1000). Buffering 1000s dense pointcloud messages was a bad idea.

playeronros gravatar imageplayeronros ( 2019-01-31 08:35:44 -0500 )edit