setFeature function doesn't work on RealSenseD455

[andrearosasco]

I wrote a short Python script to connect to a realsense device and adjust different parameters.
The script works on iCub2.7 but not on ergoCub and R1.
I think this might be somehow connected to the camera model as both R1 and ergoCub use a D455 while iCub2.7 has a D415.

In every test the device has been started using the yarprobotinterface and not yarpdev.
This is a small script to test the correct behavior:

import yarp
import time
from yarp import IFrameGrabberControls

props = yarp.Property()
props.put('device', 'RGBDSensorClient')
props.put('localImagePort', '/RSGui/rgbImage:i')
props.put('localDepthPort', '/RSGui/depthImage:i')
props.put('localRpcPort', '/RSGui/rpc:i')

props.put('remoteImagePort', '/depthCamera/rgbImage:o')
props.put('remoteDepthPort', '/depthCamera/depthImage:o')
props.put('remoteRpcPort', '/depthCamera/rpc:i')

driver = yarp.PolyDriver(props)

yarp.Network.disconnect('/depthCamera/rgbImage:o', '/RSGui/rgbImage:i')
yarp.Network.disconnect('/depthCamera/depthImage:o', '/RSGui/depthImage:i')

iface = driver.viewIFrameGrabberControls()

for _ in range(10):
    iface.setFeature(yarp.YARP_FEATURE_GAIN, 1.0)
    time.sleep(1)
    iface.setFeature(yarp.YARP_FEATURE_GAIN, 0.0)
    time.sleep(1)

As a quick workaround, I'm working on setting the camera parameter directly, using the pyrealsense2 package.
Apparently, even with the yarp device connected to the camera, it is possible to connect and modify the parameters.

[andrearosasco]

I have tried connecting to a D455 and controlling the gain directly through librealsense and everything was working as expected. This is the script I used:

import time
import pyrealsense2 as rs

print('Connecting...')
pipeline = rs.pipeline()
config = rs.config()
config.enable_stream(rs.stream.depth)
config.enable_stream(rs.stream.color)
profile = pipeline.start(config)

rgb_sensor = profile.get_device().query_sensors()[1]
print('Connected')


while True:
    rgb_sensor.set_option(rs.option.gain, 0)
    time.sleep(1)
    rgb_sensor.set_option(rs.option.gain, 127)
    time.sleep(1)

The problem is that if the yarp realsense device is already using the realsense the script above can't connect to it and to change the parameters. This is weird since, if another process is reading images from the realsense using librealsense, the script can still connect to it and change gain, exposure, etc.

[Nicogene]

Hi @andrearosasco, not sure if you already found it but here is how is implemented in the device

yarp-device-realsense2/src/devices/realsense2/realsense2Driver.cpp

Lines 1235 to 1243 in d4da73f

	if(optionPerc2Value(RS2_OPTION_GAIN, m_color_sensor,value, valToSet))
	{
	b = setOption(RS2_OPTION_GAIN, m_color_sensor, valToSet);
	if (m_stereoMode)
	{
	if(optionPerc2Value(RS2_OPTION_EXPOSURE, m_depth_sensor, value, valToSet))
	{
	b &= setOption(RS2_OPTION_EXPOSURE, m_depth_sensor, valToSet);
	}

where setOption

yarp-device-realsense2/src/devices/realsense2/realsense2Driver.cpp

Lines 233 to 261 in d4da73f

	static bool setOption(rs2_option option,const rs2::sensor* sensor, float value)
	{
	if (!sensor)
	{
	return false;
	}
	// First, verify that the sensor actually supports this option
	if (!sensor->supports(option))
	{
	yCError(REALSENSE2) << "The option" << rs2_option_to_string(option) << "is not supported by this sensor";
	return false;
	}
	// To set an option to a different value, we can call set_option with a new value
	try
	{
	sensor->set_option(option, value);
	}
	catch (const rs2::error& e)
	{
	// Some options can only be set while the camera is streaming,
	// and generally the hardware might fail so it is good practice to catch exceptions from set_option
	yCError(REALSENSE2) << "Failed to set option " << rs2_option_to_string(option) << ". (" << e.what() << ")";
	return false;
	}
	return true;
	}

It seems that the calls are the same of the one of the librealsense example

[andrearosasco]

Thanks, @Nicogene! Yes, everything looks the same. The only thing I could think so is that for some reason on the d455 sensor->supports(option) returns false even if the option is supported.

[Nicogene]

Thanks, @Nicogene! Yes, everything looks the same. The only thing I could think so is that for some reason on the d455 sensor->supports(option) returns false even if the option is supported.

Good catch! Maybe it is a bug that can be reported in librealsense repository.

[xEnVrE]

Today I checked this issue and I found that - on ergoCub at least - when I try to set the options via RPC the device outputs an error due to this exception:

yarp-device-realsense2/src/devices/realsense2/realsense2Driver.cpp

Lines 191 to 203 in d4da73f

	try
	{
	rs2::option_range optionRange = sensor->get_option_range(option);
	value =(float) (perc * (optionRange.max - optionRange.min) + optionRange.min);
	}
	catch (const rs2::error& e)
	{
	// Some options can only be set while the camera is streaming,
	// and generally the hardware might fail so it is good practice to catch exceptions from set_option
	yCError(REALSENSE2) << "Failed to get option " << option << " range. (" << e.what() << ")";
	return false;
	}

where e.what() contains failed to set power state, which seems to me quite unrelated.

cc @Nicogene

[andrearosasco]

So I just tested the code again and it looks like it works. I didn't change anything but the version of yarp and of the yarp-device-realsense2 on ergoCub have probably changed at some point .

Current versions
Yarp: 3.8.1+157-20231016.1+gita0d408b7c
yapr-device-realsense: 47430cb
librealsense: b59b13671658910fc453a4a6bbd61f13ba6e83cc

[traversaro]

Another thing that may play a role here is the firmware version of the realsense. On ergoCubSN001 we could not get yarp-device-realsense2 to work until we updated the firmware version via the integrated gui of librealsense.

[xEnVrE]

Just to clarify, this has been tested on the new ergoCubSN001 while the issue was raised on the previous ergoCubSN000.

cc @Nicogene

[traversaro]

Just to clarify, this has been tested on the new ergoCubSN001 while the issue was raised on the previous ergoCubSN000.

cc @Nicogene

Indeed, I guess that the firmware version between the two realsense may be different.