Displaying a 3D pointcloud within a ROS node
Hi,
I have a ROS node subscribing to a pointcloud on the /camera/depthregistered/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 pclvisualizer_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;
}
Asked by playeronros on 2019-01-30 10:39:39 UTC
Comments
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?
Asked by gvdhoorn on 2019-01-30 13:41:51 UTC
No, I actually want to update the viewer as information arrives. Basically have a real-time viewer of the data
Asked by playeronros on 2019-01-31 05:43:40 UTC
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 ofPCLVisualizer
that you keep around.Asked by gvdhoorn on 2019-01-31 06:13:44 UTC
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.
Asked by playeronros on 2019-01-31 09:35:44 UTC