YarpOpenraveRGBDSensor remove hard-coded depthWidth/depthHeight pairs

[jgvictores]

YarpOpenraveRGBDSensor remove hard-coded depthWidth/depthHeight pairs.

Attempt: Via https://github.com/rdiankov/openrave/blob/v0.9.0/plugins/basesensors/baseflashlidar3d.h#L116-L121 we can replace/remove the ugly hard-coded lines:

openrave-yarp-plugins/libraries/YarpPlugins/YarpOpenraveRGBDSensor/IRGBDSensorImpl.cpp

Lines 85 to 121 in 8e7dd25

	if( (depthHeight == 0) || (depthWidth == 0) )
	{
	if (sensorRanges.size()==3072)
	{
	depthWidth = 64;
	depthHeight = 48;
	}
	else if (sensorRanges.size()==12288)
	{
	depthWidth = 128;
	depthHeight = 96;
	}
	else if (sensorRanges.size()==49152)
	{
	depthWidth = 256;
	depthHeight = 192;
	}
	else if (sensorRanges.size()==307200)
	{
	depthWidth = 640;
	depthHeight = 480;
	}
	else if (sensorRanges.size()==4)
	{
	depthWidth = 2;
	depthHeight = 2;
	}
	else if (sensorRanges.size()==0)
	{
	CD_WARNING("sensorRanges.size()==0\n");
	}
	else
	{
	depthWidth = sensorRanges.size();
	depthHeight = 1;
	}
	}

[jgvictores]

Related: #74

[jgvictores]

Modifying X in openrave --module OpenraveYarpPluginLoader "env data/testwamcamera.env.xml open --device RGBDSensorWrapper --subdevice YarpOpenraveRGBDSensor --robotIndex 0 --depthSensorIndex X" --viewer qtcoin --collision ode:

Depth sensor 1 name: BarrettWAM:laser
[info] DeviceDriverImpl.cpp:92 open(): Laser min_angle: -2,356194   0,000000.
[info] DeviceDriverImpl.cpp:93 open(): Laser max_angle: 2,356194   0,000000.
[info] DeviceDriverImpl.cpp:94 open(): Laser resolution: 0,006109   0,000000.
[info] DeviceDriverImpl.cpp:95 open(): Laser min_range, max_range: 0,010000   5,000000.
[info] DeviceDriverImpl.cpp:96 open(): Laser time_increment: 0,000000   0,000000.
[info] DeviceDriverImpl.cpp:97 open(): Laser time_scan: 0,100000   0,000000.

Depth sensor 2 name: BarrettWAM:spinninglaser
[info] DeviceDriverImpl.cpp:92 open(): Laser min_angle: -2,356194   0,000000.
[info] DeviceDriverImpl.cpp:93 open(): Laser max_angle: 2,356194   0,000000.
[info] DeviceDriverImpl.cpp:94 open(): Laser resolution: 0,006109   0,000000.
[info] DeviceDriverImpl.cpp:95 open(): Laser min_range, max_range: 0,010000   3,000000.
[info] DeviceDriverImpl.cpp:96 open(): Laser time_increment: 0,000000   0,000000.
[info] DeviceDriverImpl.cpp:97 open(): Laser time_scan: 0,100000   0,000000.

Depth sensor 3 name: BarrettWAM:flashlidar
[info] DeviceDriverImpl.cpp:92 open(): Laser min_angle: -1,570796   0,000000.
[info] DeviceDriverImpl.cpp:93 open(): Laser max_angle: 1,570796   0,000000.
[info] DeviceDriverImpl.cpp:94 open(): Laser resolution: 0,000000   0,000000.
[info] DeviceDriverImpl.cpp:95 open(): Laser min_range, max_range: 0,000000   5,000000.
[info] DeviceDriverImpl.cpp:96 open(): Laser time_increment: 0,000000   0,000000.
[info] DeviceDriverImpl.cpp:97 open(): Laser time_scan: 0,200000   0,000000.

[jgvictores]

Where 1,2,3 are BaseLaser2D,BaseSpinningLaser2D,BaseFlashLidar3D respectively (https://github.com/rdiankov/openrave/blob/v0.9.0/src/data/testwamcamera.env.xml#L30-L71).

[jgvictores]

In our case, BaseFlashLidar3D:

• https://github.com/roboticslab-uc3m/asibot-openrave-models/blob/88667d8d68a2b5a97077439882de3e2a80c2aac4/openrave/models/asibot_kitchen_kinect.env.xml#L57
• https://github.com/roboticslab-uc3m/teo-openrave-models/blob/c0bb7458847013f5d77f285b7c59210b1c30005b/openrave/teo/teo.robot.xml#L83

[jgvictores]

Regarding the hard-coding of height/width of a BaseFlashLidar3D, I ran into rdiankov/openrave#237. The issue refers to openrave/plugins/basesensors/baseflashlidar3d.h not being installed. Very ugly hacks have to be introduced, including coping with the fact that height/width are within boost::shared_ptr<BaseFlashLidar3DGeom> _pgeom; inside a protected scope.

In the light of "[OpenRAVE-users] Accessing collided body information from lasers" (https://sourceforge.net/p/openrave/mailman/message/20092615/ and http://openrave-users-list.185357.n3.nabble.com/Accessing-collided-body-information-from-lasers-td185850.html) I see the most elegant code would require an upstream PR, and enable something like (note that this is currently not supported, see baseflashlidar3d):

if(depthSensorBasePtr->InterfaceBase::SupportsCmd("height"))
{
    std::stringstream ssdata;
    if( !depthSensorBasePtr->InterfaceBase::SendCommand(std::stringstream("height"), ssdata))
        return false;
    // parse value
}

[jgvictores]

In fact, these values could be computed if we had the correct min_angle + max_angle + resolution info:

Depth sensor 3 name: BarrettWAM:flashlidar
[info] DeviceDriverImpl.cpp:92 open(): Laser min_angle: -1,570796   0,000000.
[info] DeviceDriverImpl.cpp:93 open(): Laser max_angle: 1,570796   0,000000.
[info] DeviceDriverImpl.cpp:94 open(): Laser resolution: 0,000000   0,000000.