Data type conversion from PCL PointCloud <PointT> to ROS sensor_msgs/Image
I am doing RGBDSLAM with people filtering, i.e. I filtered out people from the point cloud and then output the remaining cluster for RGBDSLAM node to process. However, the RGBDSLAM node only subscribes to sensor_msgs/Image (rgb) and sensor_msgs/Image (depth). I tried to extract RGB and Depth messages directly from the pcl::PointCloud<pointt> before converting it into ROS sensor_msgs/Image. Here is part of my code in the cloud callback function:
typedef pcl::PointXYZRGB PointT;
typedef pcl::PointCloud<PointT> PointCloudT;
PointCloudT::Ptr no_man_cloud (new PointCloudT) ;
sensor_msgs::Image rgbImage ;
sensor_msgs::Image depthImage ;
// RGB Topic
pcl::toROSMsg ( *no_man_cloud, rgbImage ) ;
// Depth Topic
pcl::PointCloud<pcl::PointXYZ> cloud_xyz ;
PointCloudT cloud_xyzrgb ;
pcl::copyPointCloud (*no_man_cloud, cloud_xyz) ;
pcl::copyPointCloud (cloud_xyz, cloud_xyzrgb) ;
pcl::toROSMsg ( cloud_xyzrgb, depthImage ) ;
rtabmap_rgbImage_pub.publish (rgbImage) ;
rtabmapDepthImage_pub.publish (depthImage) ;
It worked in theory, but actually renders nothing on the sensor_msgs/Image depth. Since I am outputting depth and rgb images out a a processed cloud, I cannot extract corresponding topics directly from the RGB-D sensor. Bearing this in mind, this article: How to convert pointcloud2 to separate depth and rgb image messages may not help me. Is there a proper way to output depth and rgb images from point cloud? Thanks !
Hi @Megacephalo, I know it's fairly late, but could you please tell me how you resolved this issue? Cheers!
Hi @disha! Thanks for asking. I spent three to four days solving my own problem the hard way and still cannot figure it out completely. As the problem per se, I remember that five years ago, I did a work around to process the PointCloud in its sensor_msgs form. But as you brought this up again, I did try to solve it the way it should be solved. Although I did not get to a workable solution, I will give my take in the answer underneath.