I have a hard time publishing colored point clouds.
As the PointCloud2 message does not contain color information, I have chosen to publish the depth and rgb info as two separate streams and combining them in an external ROS node, as suggested here.
I tried to publish the depth as a PointCloud2 message (shown here), but i got the error:
using the new ros Interface. Also, filling the message was quite slow (even when I'm handling the sensor explicitly). Therefore, I've chosen to publish the depth data as a std_msgs::Float32MultiArray. RGB is published as a sensor_msgs::Image.read__sensor_msgs__PointCloud2: field data: expected uint8 array (simROS.publish @ 'RosInterface' plugin)
The problem I have is with registering the color to the points, as seen on the photo (1st being the point cloud in rviz; 2nd being the scene in V-Rep):
I use a single sensor with the following setup:
I handle the sensor in a non-threaded child script:
Code: Select all
if (sim_call_type==sim.syscb_init) then
sensorHandle=sim.getObjectHandle("sensor")
depth_pub = simROS.advertise('/vrep/depth', 'std_msgs/Float32MultiArray')
rgb_pub = simROS.advertise('/vrep/rgb', 'sensor_msgs/Image')
simROS.publisherTreatUInt8ArrayAsString(rgb_pub)
end
if (sim_call_type==sim.syscb_actuation) then
sim.handleVisionSensor(sensorHandle)
r,t1,u1=sim.readVisionSensor(sensorHandle)
tb_depth={}
tb_depth['data']=u1
simROS.publish(depth_pub, tb_depth)
t=sim.getSystemTime()
image=sim.getVisionSensorImage(sensorHandle,0,0,0,0,1)
tb_rgb={}
tb_rgb['header']={stamp=t, frame_id="camera_depth_frame"}
tb_rgb['height']=480
tb_rgb['width']=640
tb_rgb['encoding']='rgb8'
tb_rgb['step']=640 * 3
tb_rgb['data']=image
simROS.publish(rgb_pub, tb_rgb)
end
Code: Select all
using Point = pcl::PointXYZRGB;
using Cloud = pcl::PointCloud<Point>;
using MultiArray = std_msgs::Float32MultiArray;
Cloud::Ptr img2cloud(cv::Mat rgb, MultiArray depth_msg)
{
std::vector<float> depth = depth_msg.data;
if (rgb.cols != depth[0] or rgb.rows != depth[1]) {
std::cout << "rgb and depth image size mismatch" << '\n';
exit(0);
}
Cloud::Ptr cloud(new Cloud);
for (size_t u = 0; u < rgb.rows; u++) {
for (size_t v = 0; v < rgb.cols; v++) {
auto rgb_ints = rgb.at<cv::Vec3b>(u, v);
int w = 2 + 4 * (rgb.cols * u + v);
Point p;
p.x = depth[w], p.y = depth[w + 1], p.z = depth[w + 2];
p.r = (int) rgb_ints[0]; p.g = (int) rgb_ints[1]; p.b = (int) rgb_ints[2];
cloud->points.push_back(p);
}
}
cloud->width = cloud->points.size();
cloud->height = 1;
return cloud;
}
All I need is a correctly registered point cloud, and I don't really care how I accomplish it, so if you can't find an error I've made, I would like to be pointed in a different direction.
Thank you