No RGB data in output pointcloud

[roboticlemon]

When running with pointcloud enable I can only seem to get mono output from the pointcloud output. I am checking RGB in Rviz and manually inspected the cloud output to verify that indeed it is only publishing mono data.

I can see that the plugin is always dropping into this else if:

realsense_gazebo_plugin/src/gazebo_ros_realsense.cpp

Lines 185 to 190 in 49ab134

	else if (this->image_msg_.data.size() == rows_arg * cols_arg)
	{
	// mono (or bayer? @todo; fix for bayer)
	iter_rgb[0] = image_src[i + j * cols_arg];
	iter_rgb[1] = image_src[i + j * cols_arg];
	iter_rgb[2] = image_src[i + j * cols_arg];

Since we are filling the this->image_msg_ data with both RGB colour images and MONO IR images here:

realsense_gazebo_plugin/src/gazebo_ros_realsense.cpp

Lines 90 to 92 in 49ab134

	fillImage(this->image_msg_, pixel_format, cam->ImageHeight(),
	cam->ImageWidth(), cam->ImageDepth() * cam->ImageWidth(),
	reinterpret_cast<const void *>(cam->ImageData()));

I'm suspecting that somehow only the MONO images are actually being used exclusively for:

realsense_gazebo_plugin/src/gazebo_ros_realsense.cpp

Line 167 in 49ab134

uint8_t *image_src = (uint8_t *)(&(this->image_msg_.data[0]));

It seems at least one other user may have experienced this issue too: #3

Any ideas or fixes would be much appreciated! Happy to contribute back if I figure something out too.

Cheers

[roboticlemon]

Here's a sample image:

[john-maidbot]

TLDR; Your suspicion is correct, you can get a colored pointcloud by doing the following things (quick and dirty solution):

1. replace image_msg_ with a map from camera_id to image message. (call it image_map)
2. use image_map to store the images in GazeboRosRealsense::OnNewFrame()
3. when you go to generate the pointcloud, replace the image_msg with image_map[<camera id for rgb image>] (be careful to only lookup the image message from image_map once per call to FillPointCloudHelper, otherwise the map lookups will hurt performance)
4. modify your urdf plugin setup so that the depth and color image cameras are collocated and have the same resolution and fov.
5. voila! colored pointclouds 😄

Detailed explanation and proposed long-term solution:
The mono issue you are referring to is because the image_msg_ member variable is reused in the GazeboRosRealsense::OnNewFrame() callback for the left IR, right IR, and rgb cameras. So by the time you reach the pointcloud generation in FillPointCloudHelper(), image_msg_ has been overwritten by one of the IR images.
One possible fix would be to replace image_msg_ with a map from camera_id to image messages, fill the appropriate image_msg[camera_id] with the image message in OnNewFrame(), and then when you go to generate the pointcloud, fill the pointcloud with the appropriate message (maybe give the user the choice about which image is used?).
However, that does not solve the problem fully because depending on your urdf plugin description setup, the color image may not be aligned with the depth image (and in reality they are definitely not aligned). This also does not handle the case where you are using an fov and/or resolution for the color image that is not equal to that of the depth image.
imho one long-term solution would be to have the gazebo camera sensors positioned realistically (i.e. actually separated in space) and add a function to the plugin that aligns the depth image to the color image before generating the pointcloud with rgb data. although some people might not appreciate the extra computational overhead when just trying to run a simulation, so you would also probably want an option to just generate a pointcloud without doing the alignment 😅

[dantenoguera]

could you just have separate image_msgs_ objects for the IR cameras and the RGB camera and work? Would that change the code a lot?

[corwin0815]

Greating everyone,
I'm recently facing the same problem, so I did the steps john-maidbot provided. And the result stays the same, RGB data does not merged into the pointcloud. Consider that I'm bad at coding and lack of experience, I believe I made some mistakes. Could anyone kindly post the modified code? It will helps me a lot, thanks.

[WizKhalista]

Hey everyone,
I have the same Problem of not being able to get a coloured Pointcloud. I didn´t get the solution mentioned by john-maidbot to work. Did anyone solve the issue?
Thank you in advance.