Initialize the rotation estimator pointer in realsense2withIMUDriver constructor

[GiulioRomualdi]

This fixes #35, moreover, I also wrote the documentation for the RealSense D435i in the README

cc @traversaro

[traversaro]

@GiulioRomualdi do you expect more fixes? Because it seems that we have several changes that would need a release, but if you have more fixes that you expect to do we can wait.

[GiulioRomualdi]

I just realize that with the documentation I wrote, I cannot get the output of the camera but just a port named /depthCamera/measures:o where I found the outcome of the imu.

I don't know how can I run two devices (multipleanalogsensorsserver and RGBDSensorWrapper) connected to the same subdevice (realsense2withIMU)

[Nicogene]

I just realize that with the documentation I wrote, I cannot get the output of the camera but just a port named /depthCamera/measures:o where I found the outcome of the imu.

I don't know how can I run two devices (multipleanalogsensorsserver and RGBDSensorWrapper) connected to the same subdevice (realsense2withIMU)

Using yarpdev as deployer I think is impossible to have two nws attached to the same device but it should work with yarprobotinterface

[GiulioRomualdi]

Hi @Nicogene thank you 😄 , I'm trying to write an xml file for a self-contained robot interface, I let you know.

[GiulioRomualdi]

This xml file allows me to get the camera along with the gyro and accelerometer

<?xml version="1.0" encoding="UTF-8" ?>
<!DOCTYPE robot PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">
<robot name="realsense" build=0 portprefix="/depthCamera">

  <device type="realsense2withIMU" name="camera">
    <group name="SETTINGS">
      <param name="depthResolution">(640 480)</param>
      <param name="rgbResolution">(640 480)</param>
      <param name="framerate">30</param>
      <param name="enableEmitter">true</param>
      <param name="alignmentFrame">RGB</param>
    </group>

    <group name="HW_DESCRIPTION">
      <param name="clipPlanes">(0.2 10.0)</param>
    </group>
  </device>

  <device type="RGBDSensorWrapper" name="rgbd_sensor_wrapper">
    <param name="period">20</param>
    <param name="name">/depthCamera</param>

    <action phase="startup" level="5" type="attach">
      <paramlist name="networks">
        <elem name="camera">camera</elem>
      </paramlist>
    </action>
    <action phase="shutdown" level="5" type="detach"/>
  </device>

  <device type="multipleanalogsensorsserver" name="imu_sensor_wrapper">
    <param name="period">10</param>
    <param name="name">/depthCamera_imu</param>

    <action phase="startup" level="5" type="attach">
      <paramlist name="networks">
        <elem name="camera">camera</elem>
      </paramlist>
    </action>
    <action phase="shutdown" level="5" type="detach"/>
  </device>

</robot>

however, I noticed that when multipleanalogsensorsserver is attached to the device the framerate is not respected. As you can see in the following image:

On the other hand, when multipleanalogsensorsserver does not run (and attached) I'm able to stream the videos at 30fps

Do you have any idea why this is happening?

[Nicogene]

Not sure if it is the same problem but, we experienced low fps passing from RGBDSensorWrapper (deprecated) to rgbdSensor_nws_yarp but we didn't have the chance to investigate it.

Could you try with rgbdSensor_nws_yarp ?

[GiulioRomualdi]

I think the problem is a bit more complex. multipleanalogsensorsserver and RGBDSensorWrapper are attached to the same device (realsense2withIMU) This means that these two threads (multipleanalogsensorsserver and RGBDSensorWrapper) will call the associated functions of to get the imu measurements and the images.
As you can notice here, every time a gyro or an accelerometer is requested wait_for_frames() method is called

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

Lines 369 to 372 in d4da73f

	try
	{
	dataframe = m_pipeline.wait_for_frames();
	}

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

Lines 421 to 424 in d4da73f

	try
	{
	dataframe = m_pipeline.wait_for_frames();
	}

The same happens when an image is requested

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

Lines 1028 to 1031 in d4da73f

	try
	{
	data = m_pipeline.wait_for_frames();
	}

wait_for_frames blocks the execution of the device.

I think this is happening with this configuration because we have two sources of data handled by two different threads (multipleanalogsensorsserver and RGBDSensorWrapper).

A possible solution (is to call wait_for_frames() in the run() of the realsense2Driver device and then the get methods just read the buffers without calling wait_for_frames()

[traversaro]

A possible solution (is to call wait_for_frames() in the run() of the realsense2Driver device and then the get methods just read the buffers without calling wait_for_frames()

Yes, this is tipically how YARP devices are implemented. Either you have a thread that blocks to wait for data, or you use callbacks to populate some internal buffers. Not sure if this is problematic for example for the image, as it means that we have some additional copy of the image.

[traversaro]

fyi @Nicogene @randaz81

[traversaro]

Not sure if this is problematic for example for the image, as it means that we have some additional copy of the image.

Looking at the code it seems that data is always copied to the rs2::frameset data structure every time wait_for_frames() is called, so I do not think that calling it in a thread would change the efficiency.

[Nicogene]

It is something that should be tested, but the solution provided by @GiulioRomualdi seems ok 👍🏻

[traversaro]

What do you think @GiulioRomualdi @Nicogene , should we merge? I do not think there is an high possibility of regression here as this was fixing something that was not working fine.

[Nicogene]

What do you think @GiulioRomualdi @Nicogene , should we merge? I do not think there is an high possibility of regression here as this was fixing something that was not working fine.

I would merge as well 👍🏻

[GiulioRomualdi]

As you prefer, but keep in mind that enabling the imu will slow down the rate at which the images arrive. So in case someone needs the IMu I would suggest using the ros2 node

[traversaro]

As you prefer, but keep in mind that enabling the imu will slow down the rate at which the images arrive. So in case someone needs the IMu I would suggest using the ros2 node

I agree, but this happens also before this PR, right?

[GiulioRomualdi]

so before this PR, right?

Before the PR the device segfaults if attached to a multipleanalogsensorserver

[traversaro]

Good point, so basically with this modification the method is not segfaulting anymore, but the performances are really bad, right? So probably it may be better to have a clear error, rather then a behaviour that seems ok but it actually bad. At this point, let's close this PR and open an issue, people interested in this feature may resurrect this PR if interested in this.

[traversaro]

Good point, so basically with this modification the method is not segfaulting anymore, but the performances are really bad, right? So probably it may be better to have a clear error, rather then a behaviour that seems ok but it actually bad. At this point, let's close this PR and open an issue, people interested in this feature may resurrect this PR if interested in this.

Issue in #37, for the time being we close the PR.