pointcloud to image conversion
Hello everyone, I am converting pointcloud to image and I can't view it in RVIZ. Am I converting it into image properly? My code is as follows:
#include <ros/ros.h>
#include <pcl/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/Image.h>
main (int argc, char **argv)
{
ros::init (argc, argv, "pcl_create");
ros::NodeHandle nh;
ros::Publisher pcl_pub = nh.advertise<sensor_msgs::PointCloud2> ("pcl_output", 1);
ros::Publisher image_pub = nh.advertise<sensor_msgs::Image> ("/image_topic", 30);
pcl::PointCloud<pcl::PointXYZRGB> cloud;
sensor_msgs::PointCloud2 output;
sensor_msgs::Image image;
// Fill in the cloud data
cloud.width = 10000;
cloud.height = 1;
cloud.points.resize(cloud.width * cloud.height);
for (size_t i = 0; i < cloud.points.size (); ++i)
{
cloud.points[i].x = 1024 * rand () / (RAND_MAX + 1.0f);
cloud.points[i].y = 1024 * rand () / (RAND_MAX + 1.0f);
cloud.points[i].z = 1024 * rand () / (RAND_MAX + 1.0f);
cloud.points[i].r = 255;
cloud.points[i].g = 255;
cloud.points[i].b = 255;
}
//Convert the cloud to ROS message
pcl::toROSMsg(cloud, output);
pcl::toROSMsg (cloud, image); //convert the cloud
output.header.frame_id = "odom";
image.header.frame_id = "odom";
ros::Rate loop_rate(1);
while (ros::ok())
{
pcl_pub.publish(output);
image_pub.publish (image); //publish our cloud image
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}
Rviz output is as follows:
Asked by San on 2016-02-21 21:25:56 UTC
Answers
I have faced a similar issue. The problem isn't that you aren't getting an image; the problem is that the pcl::toROSMsg (cloud, image);
is doing some weird conversions so that the image is no longer a 2D array of pixels, but one long 1D array of pixel values. If you do rostopic echo /image_topic
you will see that the messages on that topic don't have any timestamps, and these long arrays are being generated. Also the width will be 10000, height will be 1.
I tried playing around with the cloud's height and width properties but this didn't seem to change the resultant image's height or width.
For my projection-to-image conversion, I sent out my projected cloud as the output message of this node, then wrote another node for doing image processing (using OpenCV). Hope that helps!
Asked by lecktor on 2020-07-07 15:38:00 UTC
Comments
Were you able to figure out how to fix this? I have a similar issue and I am unable to convert the sensor msgs back to cv_image using cvbridge because of this. I am using ROS 1 Melodic
Asked by Abhinavgandhi09 on 2021-03-10 19:55:25 UTC