Add DepthCamera gz-sim plugin

[Nicogene]

It implements the gzyarp::DepthCamera device, that exposes both RGB and depth images via the yarp::dev::IRGBDSensor interface.

It fixes #106

[Nicogene]

I concluded the implementation of the device, I added the unit test and the tutorial, @xela-95 @traversaro it is ready for the review from my side.

I didn't implemented some features such as the horizontal and vertical flip or the timestamp on the image, they are implemented in gzyarp::Camera device, but they are not implemented in the gazebo-yarp-plugins depthcamera twin

[Nicogene]

Under apt it is failing as the CameraTest:

• CameraTest non deterministic crash with apt dependencies #200

Instead there other failures with conda to be addressed:

 Error:  |gz-sim-yarp-plugins.plugins.GzYarpDepthCamera| setRgbResolution: impossible to set resolution
Error:  |gz-sim-yarp-plugins.plugins.GzYarpDepthCamera| setRgbMirroring: impossible to set mirroring
Error:  |gz-sim-yarp-plugins.plugins.GzYarpDepthCamera| setDepthResolution: impossible to set resolution
Error:  |gz-sim-yarp-plugins.plugins.GzYarpDepthCamera| setDepthFOV: impossible to set FOV
Error:  |gz-sim-yarp-plugins.plugins.GzYarpDepthCamera| setDepthAccuracy: impossible to set accuracy
Error:  |gz-sim-yarp-plugins.plugins.GzYarpDepthCamera| setDepthClipPlanes: impossible to set clip planes
Error:  |gz-sim-yarp-plugins.plugins.GzYarpDepthCamera| setDepthMirroring: impossible to set mirroring
[Dbg] [Sensors.cc:457] SensorsPrivate::Stop
[Dbg] [Sensors.cc:443] SensorsPrivate::RenderThread stopped
[INFO] |yarp.os.impl.PortCoreOutputUnit|/depthcamera/rgbImage:o| output for route /depthcamera/rgbImage:o->udp->/RGBD_nwc/Image:o asking other side to close by out-of-band means
[INFO] |yarp.os.impl.PortCoreInputUnit|/RGBD_nwc/Image:o| Removing input from /depthcamera/rgbImage:o to /RGBD_nwc/Image:o
[INFO] |yarp.os.impl.PortCoreOutputUnit|/depthcamera/rgbImage:o| Removing output from /depthcamera/rgbImage:o to /RGBD_nwc/Image:o
[INFO] |yarp.os.impl.PortCoreOutputUnit|/depthcamera/depthImage:o| output for route /depthcamera/depthImage:o->udp->/RGBD_nwc/Depth:o asking other side to close by out-of-band means
[INFO] |yarp.os.impl.PortCoreInputUnit|/RGBD_nwc/Depth:o| Removing input from /depthcamera/depthImage:o to /RGBD_nwc/Depth:o
[INFO] |yarp.os.impl.PortCoreOutputUnit|/depthcamera/depthImage:o| Removing output from /depthcamera/depthImage:o to /RGBD_nwc/Depth:o
[INFO] |yarp.os.impl.PortCoreOutputUnit|/RGBD_nwc/rpc:o| Removing output from /RGBD_nwc/rpc:o to /depthcamera/rpc:i
[INFO] |yarp.os.impl.PortCoreInputUnit|/depthcamera/rpc:i| Removing input from /RGBD_nwc/rpc:o to /depthcamera/rpc:i
[Dbg] [SimulationRunner.cc:578] [Dbg] [SimulationRunner.cc:578] [Dbg] [SimulationRunner.cc:578] [Dbg] [SimulationRunner.cc:578] 3[Dbg] [SimulationRunner.cc:578] Exiting postupdate worker thread (0)
)
[Dbg] [SimulationRunner.cc:578] [Dbg] [SimulationRunner.cc:578] Exiting postupdate worker thread (2)
3)
[Dbg] [Sensors.cc:457] SensorsPrivate::Stop
[DEBUG] |gz-sim-yarp-plugins.plugins.GzYarpDepthCamera| Closing Gazebo Yarp Depth Camera Driver
[INFO] interrupt1 phase starting...
[INFO] interrupt1 phase finished.
[INFO] shutdown phase starting...
[INFO] Entering action level 5 of phase shutdown
[INFO] Executing detach action, level 5 on device depthcamera_nws_yarp with parameters []
[INFO] All actions for action level 5 of shutdown phase started. Waiting for unfinished actions.
[INFO] All actions for action level 5 of shutdown phase finished.
[INFO] Closing device depthcamera_nws_yarp
double free or corruption (!prev)

here is the bt that I got from wsl:

(gdb) bt
#0  __pthread_kill_implementation (no_tid=0, signo=6, threadid=<optimized out>) at ./nptl/pthread_kill.c:44
#1  __pthread_kill_internal (signo=6, threadid=<optimized out>) at ./nptl/pthread_kill.c:78
#2  __GI___pthread_kill (threadid=<optimized out>, signo=signo@entry=6) at ./nptl/pthread_kill.c:89
#3  0x00007ffff73e927e in __GI_raise (sig=sig@entry=6) at ../sysdeps/posix/raise.c:26
#4  0x00007ffff73cc8ff in __GI_abort () at ./stdlib/abort.c:79
#5  0x00007ffff73cd7b6 in __libc_message_impl (fmt=fmt@entry=0x7ffff75728d7 "%s\n") at ../sysdeps/posix/libc_fatal.c:134
#6  0x00007ffff744cff5 in malloc_printerr (str=str@entry=0x7ffff7575ae0 "double free or corruption (!prev)") at ./malloc/malloc.c:5772
#7  0x00007ffff744f12c in _int_free_merge_chunk (av=0x7fff9c000030, p=0x7fff9c00f940, size=1228816) at ./malloc/malloc.c:4679
#8  0x00007ffff744f43a in _int_free (av=0x7fff9c000030, p=<optimized out>, have_lock=<optimized out>) at ./malloc/malloc.c:4646
#9  0x00007ffff7451dae in __GI___libc_free (mem=0x7fff9c00f950) at ./malloc/malloc.c:3398
#10 0x00007ffff7be0483 in FreeAligned (ptr=0x7fff9c00f958 "") at /home/ngenesio/robotology/robotology-superbuild/src/YARP/src/libYARP_sig/src/yarp/sig/impl/IplImage.cpp:40
#11 0x00007ffff7be04de in iplDeallocateImage (image=0x7fff9c0052f0) at /home/ngenesio/robotology/robotology-superbuild/src/YARP/src/libYARP_sig/src/yarp/sig/impl/IplImage.cpp:52
#12 0x00007ffff7b987cc in ImageStorage::_free (this=0x555556b31a20) at /home/ngenesio/robotology/robotology-superbuild/src/YARP/src/libYARP_sig/src/yarp/sig/Image.cpp:204
#13 0x00007ffff7b9884e in ImageStorage::_free_complete (this=0x555556b31a20) at /home/ngenesio/robotology/robotology-superbuild/src/YARP/src/libYARP_sig/src/yarp/sig/Image.cpp:220
#14 0x00007ffff7b9bb3c in ImageStorage::~ImageStorage (this=0x555556b31a20, __in_chrg=<optimized out>) at /home/ngenesio/robotology/robotology-superbuild/src/YARP/src/libYARP_sig/src/yarp/sig/Image.cpp:119
#15 0x00007ffff7b98d9e in yarp::sig::Image::~Image (this=0x555556b374d8, __in_chrg=<optimized out>) at /home/ngenesio/robotology/robotology-superbuild/src/YARP/src/libYARP_sig/src/yarp/sig/Image.cpp:379
#16 0x00007fffba943e35 in yarp::sig::ImageOf<float>::~ImageOf (this=0x555556b374d8, __in_chrg=<optimized out>) at /home/ngenesio/robotology/robotology-superbuild/src/YARP/src/libYARP_sig/src/yarp/sig/Image.h:604
#17 0x00007fffba940a5e in RgbdSensor_nws_yarp::~RgbdSensor_nws_yarp (this=0x555556b371e0, __in_chrg=<optimized out>) at /home/ngenesio/robotology/robotology-superbuild/src/YARP/src/devices/networkWrappers/RGBDSensor_nws_yarp/RgbdSensor_nws_yarp.cpp:182
#18 0x00007fffba940b7e in RgbdSensor_nws_yarp::~RgbdSensor_nws_yarp (this=0x555556b371e0, __in_chrg=<optimized out>) at /home/ngenesio/robotology/robotology-superbuild/src/YARP/src/devices/networkWrappers/RGBDSensor_nws_yarp/RgbdSensor_nws_yarp.cpp:182
#19 0x00007fffba93f7c3 in rgbdSensor_nws_yarp_destroy (obj=0x555556b371e0) at /home/ngenesio/robotology/robotology-superbuild/build/src/YARP/src/devices/networkWrappers/RGBDSensor_nws_yarp/yarp_plugin_yarpmod_rgbdSensor_nws_yarp.cpp:30
#20 0x00007ffff7c6533c in yarp::os::SharedLibraryClassFactory<yarp::dev::DeviceDriver>::destroy (this=0x555556ac8030, obj=0x555556b371e0)
    at /home/ngenesio/robotology/robotology-superbuild/src/YARP/src/libYARP_os/src/yarp/os/SharedLibraryClassFactory.h:43
#21 0x00007ffff7c64194 in yarp::os::SharedLibraryClass<yarp::dev::DeviceDriver>::close (this=0x555556b31c68) at /home/ngenesio/robotology/robotology-superbuild/src/YARP/src/libYARP_os/src/yarp/os/SharedLibraryClass.h:69
#22 0x00007ffff7c6283e in yarp::os::SharedLibraryClass<yarp::dev::DeviceDriver>::~SharedLibraryClass (this=0x555556b31c68, __in_chrg=<optimized out>)

It seems a problem when the nws closes, I need to understand if it is a problem of the nws or of my device

[traversaro]

It seems that two different objects think to own the same image, so both try to delete it?

[Nicogene]

It seems that two different objects think to own the same image, so both try to delete it?

Yes it is strange that RgbdSensor_nws_yarp::~RgbdSensor_nws_yarp seems to be called twice

[traversaro]

RgbdSensor_nws_yarp

You can try to launch the device with a fake grabber, and see if there is the same problem to isolate it.

[Nicogene]

@xela-95 @traversaro now the CI is happy, this commit fixed the unit test but I am not sure why

• 4b2acc8 (#226)

[traversaro]

Thanks, I added a few comments. Can you avoid committing images as part of the repo and use images hosted in issues instead?

[Nicogene]

Feel free to squash all the commits, the history is quite messy

[xela-95]

Thanks a lot @Nicogene !! It looks good to me!

[traversaro]

Good to go for me! @xela-95 if you merge can you merge with squash (as suggested by @Nicogene) to avoid adding images to the git history? Thanks!