Bugfix on the color channel of point clouds from DepthCamera

[GoncaloLeao]

Fixes #1865.

This PR fixes a bug on the fragment shader associated with the depth camera so that the color of each point in the point cloud is correct.

In order to test this in ROS, the depth camera plugin (which extends Gazebo's DepthCameraPlugin class) must correctly extract the colors from the const unsigned char *_image on the callback function virtual void OnNewImageFrame(const unsigned char *_image, unsigned int _width, unsigned int _height, unsigned int _depth, const std::string &_format). I created a fork of the gazebo_ros_pkgs repository where I corrected the GazeboRosDepthCamera class (https://github.com/GoncaloLeao/gazebo_ros_pkgs/blob/bugfix-rgb-on-depth-camera-cloud/gazebo_plugins/src/gazebo_ros_depth_camera.cpp). When this pull request is accepted, I will create a PR to correct the gazebo_ros_depth_camera plugin of the gazebo_ros_pkgs repository.

In order to test this bugfix, you can use the files I provide in attachment.

1. Download the source code for the gazebo_ros_depth_camera plugin from my fork of gazebo_ros_pkgs, from a branch named 'bugfix-rgb-on-depth-camera-cloud' (https://github.com/GoncaloLeao/gazebo_ros_pkgs/tree/bugfix-rgb-on-depth-camera-cloud).
2. Compile the depth camera plugin.
3. Add the Gazebo Model with a Depth Camera Plugin to your Gazebo models directory. The plugin to be used is the modified version of gazebo_ros_depth_camera.

test_sensor.zip contains all the files needed for the model. It is a modified version of the model of this tutorial: http://gazebosim.org/tutorials/?tut=ros_depth_camera
4. Run Gazebo with any world of your choice and add the depth camera model to the world.
5. On RViz, pay close attention to the colors of the point cloud being published on the topic /camera/depth/points. The colors should closely match the ones for the image topic /camera/color/image_raw.

Here I provide two screenshots of what I can see in Gazebo ...

... and RViz:

[iche033]

I wrote a test for this change in 7bca2ed. It places a green box in front of the camera and expects that for every point cloud that has a z value equal to the distance of the box, the color value should be some shade of green. It's currently passing. One weird issue is that when no objects are detected, i.e. z > box distance, the blue pixel value after unpacking the color component always returns 1 (small as it's out of 255). I think that's minor since the color component should not be used in that case. Can you take a look and apply the test to your branch if it looks good to you?

We should also add some documentation here on how to unpack the floating point color value into r, g, b. One place to add the documentation is here

[GoncaloLeao]

Hello @iche033, I ran the test in my local branch and it passed. I applied the change to the DepthCameraSensorTest class to my pull request. I am not sure why the blue pixel returns 1 and not 0 when no object are detected.

Regarding the documentation, I agree that it should be made clear how the RGB values can be extracted from the 4th channel. In my last commit, I added some comments above the function declaration you suggested. At first, I pondered explaining how to unpack with text only, but I ended up adding a small fragment of code to the comment. This will make it much clearer and remove ambiguity for anyone using the API. What do you think? Is it too much verbose? If so, feel free to make a suggestion for improvement.

[osrf-jenkins]

Build finished. 15 tests run, 0 skipped, 2 failed.

[osrf-jenkins]

Build finished. No test results found.

[osrf-jenkins]

Build finished. 1439 tests run, 0 skipped, 19 failed.

[osrf-jenkins]

Build finished. 2098 tests run, 0 skipped, 24 failed.

[osrf-jenkins]

Build finished. 15 tests run, 0 skipped, 2 failed.

[osrf-jenkins]

Build finished. 2145 tests run, 0 skipped, 32 failed.

[osrf-jenkins]

Build finished. No test results found.

[osrf-jenkins]

Build finished. 2091 tests run, 0 skipped, 22 failed.

[iche033]

Just a minor coding style suggestion otherwise the changes and the documentation look good.

[j-rivero]

@GoncaloLeao could you please pull changes in gazebo11 branch into your branch to get latest CI before merging?

[GoncaloLeao]

I applied the changes from gazebo11.
This branch should now be ready to merge.

[chapulina]

@GoncaloLeao , it looks like there are still conflicts preventing this from being merged.

[scpeters]

I just resolved conflicts and pushed to restart CI

[scpeters]

Thanks for submitting the fix and sorry for the delay in merging! If you are able to make a matching gazebo_ros_pkgs pull request I will be happy to review it.