[openni_kinect plugin] Sensor image and depth image do not match timestamps

[helenol]

When simulating a dynamically moving kinect camera, the colors in the depth pointclouds do not match the camera image: that is, a random part of the image is mapped to the pointcloud.

This is because the driver does not actually check that the sensor image and depth image match timestamps. This is reproduceable across Ubuntu 14.04, Ubuntu 16.04, OS X 10.11.2., and gazebos 2 through 7.4, with both indigo and kinetic.

I instrumented the callbacks to the depth and sensor image to echo the ns part of their timestamps when they are called:

[Depth callback] Sensor time: 800000000 Depth sensor time: 0
[Image callback] Sensor time: 0 Depth sensor time: 0
[Depth callback] Sensor time: 0 Depth sensor time: 210000000
[Image callback] Sensor time: 210000000 Depth sensor time: 210000000
[Depth callback] Sensor time: 210000000 Depth sensor time: 400000000
[Image callback] Sensor time: 400000000 Depth sensor time: 400000000
[Depth callback] Sensor time: 400000000 Depth sensor time: 600000000
[Image callback] Sensor time: 600000000 Depth sensor time: 600000000
[Depth callback] Sensor time: 600000000 Depth sensor time: 810000000
[Image callback] Sensor time: 810000000 Depth sensor time: 810000000

As can be seen, the depth callback always comes first, so that the pointcloud is always matched to the previous RGB/grayscale image.

I hacked a quick solution in which stores the pointer to the latest depth image, and only calls FillPointCloud when the timestamps between the two match, here and again in the sensor callback:
https://github.com/ros-simulation/gazebo_ros_pkgs/blob/indigo-devel/gazebo_plugins/src/gazebo_ros_openni_kinect.cpp#L204-L211

But this is an ugly hack, but fixes it. Can upload my code to a branch if anyone is curious.

[kev-the-dev]

Seems to be a duplicate of #408, which was fixed. If this issue still occurs with latest versions of gazebo-plugins, comment here to reopen issue.