YarpOpenraveRGBDSensor depth consumes too much CPU + units! + image rotated!?

[jgvictores]

YarpOpenraveRGBDSensor depth consumes too much CPU (makes Chrome, but not Firefox, hang) and has ugly hard-coded lines, see below.

1. Does it really depend on the plugin, or is it simply due to the presence of the sensor?
2. If it is not the sensor's fault, the function being called to frequently?
3. Copy is currently pixel-by-pixel, maybe better bulk copy? Is this possible? Extra important note: image rotated! We did not notice this before because we were seeing it through numpy! :fearful:

PS: Originally from https://github.com/roboticslab-uc3m/openrave-yarp-plugins/issues/103, related to https://github.com/roboticslab-uc3m/openrave-yarp-plugins/issues/105

Also related:

• Issue "YarpOpenraveRGBDSensor publishes fixed positions of obstacles" https://github.com/roboticslab-uc3m/openrave-yarp-plugins/issues/87, solved by PR "https://github.com/roboticslab-uc3m/openrave-yarp-plugins/pull/88", mentions http://openrave.org/docs/latest_stable/coreapihtml/classOpenRAVE_1_1SensorBase_1_1LaserSensorData.html
• Possibly superseded by above point: ye oldie OpenraveYarpCoupled
• Simply google: openrave pointcloud2

[jgvictores]

• Does it really depend on the plugin, or is it simply due to the presence of the sensor?

OR without depth sensor: 10% CPU. OR with depth sensor: 10% CPU. OR with depth sensor and publishing plugin: 15% CPU. :+1:

[jgvictores]

• consumes too much CPU: rise from 10% to 15% does not seem dramatic (an can be turned on/off via OpenraveYarpPluginLoader e.g. openraveYarpPluginLoaderClient as at https://github.com/roboticslab-uc3m/teo-configuration-files/blob/6986197b1607f53db10477653e6ca653998fc522/share/applications/teoSimBase.xml#L9-L13
• units: fixed at https://github.com/roboticslab-uc3m/openrave-yarp-plugins/commit/bb6909f8aa311f467c023386d374afbe35feb586, can be visualized via the depthimage portmonitor as in https://github.com/roboticslab-uc3m/teo-configuration-files/commit/6986197b1607f53db10477653e6ca653998fc522
• image rotated: turns out to seem right, and in any case the robot model xmls should be adjusted