search

robotology / gz-sim-yarp-plugins

YARP plugins for Modern Gazebo (gz-sim).
BSD 3-Clause "New" or "Revised" License
8 stars 1 forks source link

Implement a new plugin using Robot Interface #10

Closed lucapa17 closed 1 year ago

lucapa17 commented 1 year ago

Recalling the meeting on last wednesday, we should proceed with this new task. As usual, we would like to divide this issue in different sub-steps to better understand the problem. What do you suggest us to start with?

GiacomoBisio commented 1 year ago

Sorry for the inconvenience, but we are not sure how to proceed and to divide the problem in sub-steps

traversaro commented 1 year ago

Hello, sorry I was out for a conference last week (in general, if you want to write messages with more priority just mention people, for example @traversaro in my case).

traversaro commented 1 year ago

Anyhow, I have a bit of backlog, I should be able to reply to this issue this evening or tomorrow, sorry!

GiacomoBisio commented 1 year ago

Don’t worry! Thank you very much again.

traversaro commented 1 year ago

Sorry, I am back to this. So, a possible division in substeps is:

traversaro commented 1 year ago

If you have any doubt just ask or let's have a call.

lucapa17 commented 1 year ago

Sorry but in the last days we were a bit busy. We completed the first step (91dadcb82a3747f4b8141af59f4ce3fc8b0c5c30) and the second step (ed87cc14973cf8d20a48d30a7c31d271f50b6fc8) but we have some problems with the third one (e1571720ec83cabc0926be851387ca010bf01536). In particular we have this error:

[DEBUG] |yarp.dev.PolyDriver|fakeIMU| Parameters are (device fakeIMU)
[INFO] |yarp.device.fakeIMU| Using default period of  0.01  s
[INFO] |yarp.dev.PolyDriver|fakeIMU| Created device <fakeIMU>. See C++ class fakeIMU for documentation.
[DEBUG] Reading file prova.xml
[DEBUG] yarprobotinterface: using xml parser for DTD v3.x
[DEBUG] Reading file prova.xml
[DEBUG] Preprocessor complete in:  5.00679e-06 s
[INFO] startup phase starting...
[INFO] Opening device prova_multipleanalogsensorsserver with parameters [("robotName" = "/prova"), ("name" = "/prova"), ("period" = "100")]
[DEBUG] |yarp.dev.PolyDriver|prova_multipleanalogsensorsserver| Parameters are (device multipleanalogsensorsserver) (id prova_multipleanalogsensorsserver) (name "/prova") (period 100) (robotName "/prova")
[INFO] |yarp.dev.PolyDriver|prova_multipleanalogsensorsserver| Created wrapper <multipleanalogsensorsserver>. See C++ class MultipleAnalogSensorsServer for documentation.
[INFO] Entering action level 5 of phase startup
[INFO] Executing attach action, level 5 on device prova_multipleanalogsensorsserver with parameters [("device" = "fakeIMU")]
[ERROR] Target device fakeIMU (network = ... ) does not exist.
[ERROR] Cannot run attach action on device prova_multipleanalogsensorsserver
[INFO] All actions for action level 5 of startup phase started. Waiting for unfinished actions.
[INFO] All actions for action level 5 of startup phase finished.
[WARNING] There was some problem running actions for startup phase . Please check the log and your configuration
[INFO] startup phase finished.
Error entering startup phase.

It seems that the FakeIMU device is created correctly, but for some reason it is not found during the attach phase.

traversaro commented 1 year ago

Great! This is indeed why I wanted you to do the steps. When you create a device outside of the yarprobotinterface, you need to explicitly "pass" their pointers of the devices to the robotinterface. To do so, you need to call the yarp::robotinterface::Robot::setExternalDevices method. You can find example of use of this method in other repos in this organization: https://github.com/search?q=org%3Arobotology+setExternalDevices&type=code .

lucapa17 commented 1 year ago

Great! This is indeed why I wanted you to do the steps. When you create a device outside of the yarprobotinterface, you need to explicitly "pass" their pointers of the devices to the robotinterface. To do so, you need to call the yarp::robotinterface::Robot::setExternalDevices method. You can find example of use of this method in other repos in this organization: https://github.com/search?q=org%3Arobotology+setExternalDevices&type=code .

Okay done! We completed step3 (4f6f4556ed9b98c06256bb3ed3c110dc92a618fb). Thank you!

lucapa17 commented 1 year ago
  • S4: Implement a simple Gazebo plugin that loads the .xml file via the libYARP_robotinterface library launches the multipleanalogsensorsserver device

We proceeded with step 4 (5a52755440a64b64db1aa699e54548d8c5f07028). For the moment we tried using the .xml file of step1 and step2 (i.e. the one with both fakeIMU and multipleanalogsensorsserver devices) and it works.

Now we are trying to use the .xml file of step3 (i.e. the one with only the multipleanalogsensorsserver device) and to launch the fakeIMU not via the .xml but via the code, exactly as we did in step3. In this case we have the following error:

[DEBUG] Reading file prova.xml
[DEBUG] yarprobotinterface: using xml parser for DTD v3.x
[DEBUG] Reading file prova.xml
[DEBUG] Preprocessor complete in:  2.09808e-05 s
[DEBUG] |yarp.dev.PolyDriver|fakeIMU| Parameters are (device fakeIMU)
[INFO] |yarp.device.fakeIMU| Using default period of  0.01  s
[INFO] |yarp.dev.PolyDriver|fakeIMU| Created device <fakeIMU>. See C++ class fakeIMU for documentation.
[INFO] startup phase starting...
[INFO] Opening device prova_multipleanalogsensorsserver with parameters [("robotName" = "/prova"), ("name" = "/prova"), ("period" = "100")]
[DEBUG] |yarp.dev.PolyDriver|prova_multipleanalogsensorsserver| Parameters are (device multipleanalogsensorsserver) (id prova_multipleanalogsensorsserver) (name "/prova") (period 100) (robotName "/prova")
[INFO] |yarp.dev.PolyDriver|prova_multipleanalogsensorsserver| Created wrapper <multipleanalogsensorsserver>. See C++ class MultipleAnalogSensorsServer for documentation.
[INFO] Entering action level 5 of phase startup
[INFO] Executing attach action, level 5 on device prova_multipleanalogsensorsserver with parameters [("device" = "fakeIMU")]
[INFO] prova_multipleanalogsensorsserver is not an IWrapper. Trying IMultipleWrapper
[INFO] |yarp.os.Port|/prova/measures:o| Port /prova/measures:o active at tcp://172.29.38.79:10002/
[INFO] |yarp.os.Port|/prova/rpc:o| Port /prova/rpc:o active at tcp://172.29.38.79:10003/
[INFO] All actions for action level 5 of startup phase started. Waiting for unfinished actions.
[INFO] All actions for action level 5 of startup phase finished.
[INFO] startup phase finished.
Stack trace (most recent call last) in thread 8637:
#6    Object "[0xffffffffffffffff]", at 0xffffffffffffffff, in 
#5    Object "/lib/x86_64-linux-gnu/libc.so.6", at 0x7ff19a89b9ff, in 
#4    Object "/lib/x86_64-linux-gnu/libc.so.6", at 0x7ff19a809b42, in 
#3    Object "/lib/x86_64-linux-gnu/libstdc++.so.6", at 0x7ff196dd32b2, in 
#2    Object "/lib/x86_64-linux-gnu/libYARP_os.so.3", at 0x7ff167830ffc, in 
#1    Object "/lib/x86_64-linux-gnu/libYARP_os.so.3", at 0x7ff167761b8f, in 
#0    Object "/usr/lib/x86_64-linux-gnu/yarp/yarp_multipleanalogsensorsserver.so", at 0x7ff16292dfa9, in 
Segmentation fault (Address not mapped to object [0x30])

It seems to do everything correctly (also the attach phase), but at the end we have this segmentation fault.

traversaro commented 1 year ago

Which version of YARP are you using? We had some fixes recently to mas-related devices.

GiacomoBisio commented 1 year ago

Which version of YARP are you using? We had some fixes recently to mas-related devices.

We are using YARP version 3.7.0

traversaro commented 1 year ago

Yes, I think you may need https://github.com/robotology/yarp/pull/2970 and https://github.com/robotology/yarp/pull/2967 that was only included in YARP 3.8.1 . Depending on how you installed YARP, it changes how to updaate it. If the issue persists after updating to YARP 3.8.1 and recompiling all the software that depends from YARP and you compiled from source, probably it would be helpful to compile the code in Debug mode and collect a detailed stacktrace of the crash.

GiacomoBisio commented 1 year ago

Yes, I think you may need robotology/yarp#2970 and robotology/yarp#2967 that was only included in YARP 3.8.1 . Depending on how you installed YARP, it changes how to updaate it. If the issue persists after updating to YARP 3.8.1 and recompiling all the software that depends from YARP and you compiled from source, probably it would be helpful to compile the code in Debug mode and collect a detailed stacktrace of the crash.

We installed YARP from binary, so we tried to install it again (to have the more recent version), but the version was still 3.7.0. So we proceeded installing YARP from source and the resulting version is the 3.8.1 as we wished.

However, if we launch our program, the following error occurs:

[DEBUG] Reading file prova.xml
[DEBUG] yarprobotinterface: using xml parser for DTD v3.x
[DEBUG] Reading file prova.xml
[INFO] Yarprobotinterface was started using the following enable_tags: 
[INFO] Yarprobotinterface was started using the following disable_tags: 
[DEBUG] List of all enable attributes found in the include tags: 
[DEBUG] List of all disable attributes found in the include tags: 
[DEBUG] Preprocessor complete in:  0.000127077 s
[ERROR] |yarp.os.YarpPluginSettings| Cannot find "fakeIMU" plugin (not built in, and no .ini file found for it)Check that YARP_DATA_DIRS leads to at least one directory with plugins/fakeIMU.ini or share/yarp/plugins/fakeIMU.ini in it
[ERROR] |yarp.dev.PolyDriver|fakeIMU| Could not find device <fakeIMU>
[ERROR] Failed to open fakeIMU device.

Indeed if we look into usr/local/share/yarp/plugins we cannot find the fakeIMU.ini file. We noticed that during the configuration of YARP, in particular when we used cmake .., we had:

...
--  [x] Plugin library: yarpmod (YARP_COMPILE_DEVICE_PLUGINS)
--  [x]   Plugin: yarpmod_AudioPlayerWrapper (ENABLE_yarpmod_AudioPlayerWrapper)
--  [x]   Plugin: yarpmod_AudioRecorderWrapper (ENABLE_yarpmod_AudioRecorderWrapper)
--  [x]   Plugin: yarpmod_audioFromFileDevice (ENABLE_yarpmod_audioFromFileDevice)
--  [x]   Plugin: yarpmod_audioToFileDevice (ENABLE_yarpmod_audioToFileDevice)
--  [ ]   Plugin: yarpmod_openNI2DepthCamera (dependencies unsatisfied: "YARP_HAS_OpenNI2"(ENABLE_yarpmod_openNI2DepthCamera)
--  [ ]   Plugin: yarpmod_fakeDepthCamera (ENABLE_yarpmod_fakeDepthCamera)
--  [ ]   Plugin: yarpmod_fakebot (ENABLE_yarpmod_fakebot)
--  [ ]   Plugin: yarpmod_fakeMotionControl (ENABLE_yarpmod_fakeMotionControl)
--  [ ]   Plugin: yarpmod_fakeAnalogSensor (ENABLE_yarpmod_fakeAnalogSensor)
--  [ ]   Plugin: yarpmod_fakeBattery (ENABLE_yarpmod_fakeBattery)
--  [ ]   Plugin: yarpmod_fakeIMU (ENABLE_yarpmod_fakeIMU)
--  [ ]   Plugin: yarpmod_fakeJoypad (ENABLE_yarpmod_fakeJoypad)
...

In particular, we noticed that yarpmod_fakeIMU plugin is not marked. We don't know if the problem can be related to this fact or can be something else.

traversaro commented 1 year ago

You need to enable an appropriate option when configuring the project, you can see the available options with the ccmake command instead of cmake.

traversaro commented 1 year ago

I checked in https://github.com/robotology/robotology-superbuild/blob/master/cmake/BuildYARP.cmake#L84 and indeed the option to enable is YARP_COMPILE_ALL_FAKE_DEVICES .

lucapa17 commented 1 year ago

Okay thank you! Now we have the last version of yarp (3.8.1).

However, we have the same problem in step4 that we had with YARP 3.7.0:

[DEBUG] Reading file prova.xml
[DEBUG] yarprobotinterface: using xml parser for DTD v3.x
[DEBUG] Reading file prova.xml
[INFO] Yarprobotinterface was started using the following enable_tags: 
[INFO] Yarprobotinterface was started using the following disable_tags: 
[DEBUG] List of all enable attributes found in the include tags: 
[DEBUG] List of all disable attributes found in the include tags: 
[DEBUG] Preprocessor complete in:  9.29832e-06 s
[DEBUG] |yarp.dev.PolyDriver|fakeIMU| Parameters are (device fakeIMU)
[INFO] |yarp.device.fakeIMU| Using default period of  0.01  s
[INFO] |yarp.device.fakeIMU| sensorName:  sensorName
[INFO] |yarp.dev.PolyDriver|fakeIMU| Created device <fakeIMU>. See C++ class fakeIMU for documentation.
[INFO] startup phase starting...
[INFO] Opening device prova_multipleanalogsensorsserver with parameters [("robotName" = "/prova"), ("name" = "/prova"), ("period" = "100")]
[DEBUG] |yarp.dev.PolyDriver|prova_multipleanalogsensorsserver| Parameters are (device multipleanalogsensorsserver) (id prova_multipleanalogsensorsserver) (name "/prova") (period 100) (robotName "/prova")
[INFO] |yarp.dev.PolyDriver|prova_multipleanalogsensorsserver| Created wrapper <multipleanalogsensorsserver>. See C++ class MultipleAnalogSensorsServer for documentation.
[INFO] Entering action level 5 of phase startup
[INFO] Executing attach action, level 5 on device prova_multipleanalogsensorsserver with parameters [("device" = "fakeIMU")]
[INFO] prova_multipleanalogsensorsserver is not an IWrapper. Trying IMultipleWrapper
[INFO] |yarp.os.Port|/prova/measures:o| Port /prova/measures:o active at tcp://172.29.38.79:10002/
[INFO] |yarp.os.Port|/prova/rpc:o| Port /prova/rpc:o active at tcp://172.29.38.79:10003/
[INFO] All actions for action level 5 of startup phase started. Waiting for unfinished actions.
[INFO] All actions for action level 5 of startup phase finished.
[INFO] startup phase finished.
libEGL warning: Not allowed to force software rendering when API explicitly selects a hardware device.
libEGL warning: failed to open /dev/dri/renderD128: Permission denied

Stack trace (most recent call last) in thread 22764:
#6    Object "[0xffffffffffffffff]", at 0xffffffffffffffff, in 
#5    Object "/lib/x86_64-linux-gnu/libc.so.6", at 0x7f3d7fd379ff, in 
#4    Object "/lib/x86_64-linux-gnu/libc.so.6", at 0x7f3d7fca5b42, in 
#3    Object "/lib/x86_64-linux-gnu/libstdc++.so.6", at 0x7f3d7c26b2b2, in 
#2    Object "/usr/local/lib/libYARP_os.so.3", at 0x7f3d4ccb2d1c, in 
#1    Object "/usr/local/lib/libYARP_os.so.3", at 0x7f3d4cbe48af, in 
#0    Object "/usr/local/lib/yarp/yarp_multipleanalogsensorsserver.so", at 0x7f3d47d3aa16, in 
Segmentation fault (Address not mapped to object [0x18])
traversaro commented 1 year ago

If you are sure that the /usr/local/lib/libYARP_os.so.3 library is indeed YARP 3.8.1, then I suggest to compile YARP and your code in Debug mode and then collect a detailed stacktrace with details on the line of code at which this segfaults happens.

lucapa17 commented 1 year ago

If you are sure that the /usr/local/lib/libYARP_os.so.3 library is indeed YARP 3.8.1, then I suggest to compile YARP and your code in Debug mode and then collect a detailed stacktrace with details on the line of code at which this segfaults happens.

After having compiled both YARP and our code in debug mode, we used gdb to collect information about the segmentation fault:

(gdb) r /usr/bin/gz sim -s model.sdf
Starting program: /usr/bin/ruby /usr/bin/gz sim -s model.sdf
[Thread debugging using libthread_db enabled]
Using host libthread_db library "/lib/x86_64-linux-gnu/libthread_db.so.1".
[New Thread 0x7fffecdbf640 (LWP 18157)]
[New Thread 0x7fffec5be640 (LWP 18158)]
[New Thread 0x7fffebdbd640 (LWP 18159)]
[New Thread 0x7fffeb5bc640 (LWP 18160)]
[New Thread 0x7fffeadbb640 (LWP 18161)]
[New Thread 0x7fffea5ba640 (LWP 18162)]
[New Thread 0x7fffe9db9640 (LWP 18163)]
[New Thread 0x7fffe95b8640 (LWP 18164)]
[New Thread 0x7fffe8db7640 (LWP 18165)]
[New Thread 0x7fffe85b6640 (LWP 18166)]
[New Thread 0x7fffe7db5640 (LWP 18167)]
[New Thread 0x7fffe75b4640 (LWP 18168)]
[New Thread 0x7fffe6db3640 (LWP 18169)]
[New Thread 0x7fffe65b2640 (LWP 18170)]
[New Thread 0x7fffe5db1640 (LWP 18171)]
[New Thread 0x7fffe55b0640 (LWP 18172)]
[New Thread 0x7fffe4daf640 (LWP 18173)]
[New Thread 0x7fffd7fff640 (LWP 18174)]
[New Thread 0x7fffd77fe640 (LWP 18175)]
[New Thread 0x7fffd6ffd640 (LWP 18176)]
[New Thread 0x7fffd67fc640 (LWP 18177)]
[New Thread 0x7fffd5ffb640 (LWP 18178)]
[New Thread 0x7fffd57fa640 (LWP 18179)]
[New Thread 0x7fffd4ff9640 (LWP 18180)]
[New Thread 0x7fffcffff640 (LWP 18181)]
[New Thread 0x7fffcf7fe640 (LWP 18182)]
[New Thread 0x7fffceffd640 (LWP 18183)]
[New Thread 0x7fffce7fc640 (LWP 18184)]
[New Thread 0x7fffcdffb640 (LWP 18185)]
[New Thread 0x7fffcd7fa640 (LWP 18186)]
[DEBUG] Reading file prova.xml
[DEBUG] yarprobotinterface: using xml parser for DTD v3.x
[DEBUG] Reading file prova.xml
[INFO] Yarprobotinterface was started using the following enable_tags: 
[INFO] Yarprobotinterface was started using the following disable_tags: 
[DEBUG] List of all enable attributes found in the include tags: 
[DEBUG] List of all disable attributes found in the include tags: 
[DEBUG] Preprocessor complete in:  7.62939e-06 s
[DEBUG] |yarp.dev.PolyDriver|fakeIMU| Parameters are (device fakeIMU)
[INFO] |yarp.device.fakeIMU| Using default period of  0.01  s
[INFO] |yarp.device.fakeIMU| sensorName:  sensorName
[New Thread 0x7fffc0167640 (LWP 18206)]
[INFO] |yarp.dev.PolyDriver|fakeIMU| Created device <fakeIMU>. See C++ class fakeIMU for documentation.
[INFO] startup phase starting...
[INFO] Opening device prova_multipleanalogsensorsserver with parameters [("robotName" = "/prova"), ("name" = "/prova"), ("period" = "100")]
[DEBUG] |yarp.dev.PolyDriver|prova_multipleanalogsensorsserver| Parameters are (device multipleanalogsensorsserver) (id prova_multipleanalogsensorsserver) (name "/prova") (period 100) (robotName "/prova")
[INFO] |yarp.dev.PolyDriver|prova_multipleanalogsensorsserver| Created wrapper <multipleanalogsensorsserver>. See C++ class MultipleAnalogSensorsServer for documentation.
[INFO] Entering action level 5 of phase startup
[INFO] Executing attach action, level 5 on device prova_multipleanalogsensorsserver with parameters [("device" = "fakeIMU")]
[INFO] prova_multipleanalogsensorsserver is not an IWrapper. Trying IMultipleWrapper
[New Thread 0x7fffbf759640 (LWP 18213)]
[INFO] |yarp.os.Port|/prova/measures:o| Port /prova/measures:o active at tcp://172.29.38.79:10002/
[New Thread 0x7fffbef58640 (LWP 18220)]
[INFO] |yarp.os.Port|/prova/rpc:o| Port /prova/rpc:o active at tcp://172.29.38.79:10003/
[New Thread 0x7fffbe757640 (LWP 18221)]
[INFO] All actions for action level 5 of startup phase started. Waiting for unfinished actions.
[INFO] All actions for action level 5 of startup phase finished.
[INFO] startup phase finished.
[Thread 0x7fffc0167640 (LWP 18206) exited]
[New Thread 0x7fffc0167640 (LWP 18222)]

Thread 35 "ruby" received signal SIGSEGV, Segmentation fault.
[Switching to Thread 0x7fffbe757640 (LWP 18221)]
0x00007fffbf944943 in MultipleAnalogSensorsServer::genericStreamData<yarp::dev::IThreeAxisGyroscopes> (this=0x555555958e30, wrappedDeviceInterface=0x555556fbfa00, metadataVector=std::vector of length 1, capacity 1 = {...}, streamingDataVector=std::vector of length 1, capacity 1 = {...}, getStatusMethodPtr=&virtual yarp::dev::IThreeAxisGyroscopes::getThreeAxisGyroscopeStatus(unsigned long) const, getMeasureMethodPtr=&virtual yarp::dev::IThreeAxisGyroscopes::getThreeAxisGyroscopeMeasure(unsigned long, yarp::sig::VectorOf<double>&, double&) const) at /mnt/c/Users/lucap/OneDrive - unige.it/Documenti/Università/Magistrale/Software Engineering/SE_Project/yarp/src/devices/multipleanalogsensorsserver/MultipleAnalogSensorsServer.cpp:496
496                 yarp::dev::MAS_status status = MAS_CALL_MEMBER_FN(wrappedDeviceInterface, getStatusMethodPtr)(i);

(gdb) bt
#0  0x00007fffbf944943 in MultipleAnalogSensorsServer::genericStreamData<yarp::dev::IThreeAxisGyroscopes>(yarp::dev::IThreeAxisGyroscopes*, std::vector<SensorMetadata, std::allocator<SensorMetadata> > const&, std::vector<SensorMeasurement, std::allocator<SensorMeasurement> >&, yarp::dev::MAS_status (yarp::dev::IThreeAxisGyroscopes::*)(unsigned long) const, bool (yarp::dev::IThreeAxisGyroscopes::*)(unsigned long, yarp::sig::VectorOf<double>&, double&) const)

     (this=0x555555958e30, wrappedDeviceInterface=0x555556fbfa00, metadataVector=std::vector of length 1, capacity 1 = {...}, streamingDataVector=std::vector of length 1, capacity 1 = {...}, getStatusMethodPtr=&virtual yarp::dev::IThreeAxisGyroscopes::getThreeAxisGyroscopeStatus(unsigned long) const, getMeasureMethodPtr=&virtual yarp::dev::IThreeAxisGyroscopes::getThreeAxisGyroscopeMeasure(unsigned long, yarp::sig::VectorOf<double>&, double&) const)
    at /mnt/c/Users/lucap/OneDrive - unige.it/Documenti/Università/Magistrale/Software Engineering/SE_Project/yarp/src/devices/multipleanalogsensorsserver/MultipleAnalogSensorsServer.cpp:496
#1  0x00007fffbf93ee33 in MultipleAnalogSensorsServer::run() (this=0x555555958e30)
    at /mnt/c/Users/lucap/OneDrive - unige.it/Documenti/Università/Magistrale/Software Engineering/SE_Project/yarp/src/devices/multipleanalogsensorsserver/MultipleAnalogSensorsServer.cpp:523
#2  0x00007fffc494ae8b in yarp::os::PeriodicThread::Private::step() (this=0x5555570334c0)
    at /mnt/c/Users/lucap/OneDrive - unige.it/Documenti/Università/Magistrale/Software Engineering/SE_Project/yarp/src/libYARP_os/src/yarp/os/PeriodicThread.cpp:246
#3  0x00007fffc494afa6 in yarp::os::PeriodicThread::Private::run() (this=0x5555570334c0)
    at /mnt/c/Users/lucap/OneDrive - unige.it/Documenti/Università/Magistrale/Software Engineering/SE_Project/yarp/src/libYARP_os/src/yarp/os/PeriodicThread.cpp:272
#4  0x00007fffc4a374e1 in theExecutiveBranch(void*) (args=0x5555570334c0)
    at /mnt/c/Users/lucap/OneDrive - unige.it/Documenti/Università/Magistrale/Software Engineering/SE_Project/yarp/src/libYARP_os/src/yarp/os/impl/ThreadImpl.cpp:81
#5  0x00007fffc4a38d30 in std::__invoke_impl<void, void (*)(void*), void*>(std::__invoke_other, void (*&&)(void*), void*&&) (__f=@0x5555570a1930: 0x7fffc4a3738d <theExecutiveBranch(void*)>)
    at /usr/include/c++/11/bits/invoke.h:61
#6  0x00007fffc4a38ca3 in std::__invoke<void (*)(void*), void*>(void (*&&)(void*), void*&&) (__fn=@0x5555570a1930: 0x7fffc4a3738d <theExecutiveBranch(void*)>) at /usr/include/c++/11/bits/invoke.h:96
#7  0x00007fffc4a38c03 in std::thread::_Invoker<std::tuple<void (*)(void*), void*> >::_M_invoke<0ul, 1ul>(std::_Index_tuple<0ul, 1ul>) (this=0x5555570a1928) at /usr/include/c++/11/bits/std_thread.h:253
#8  0x00007fffc4a38bb8 in std::thread::_Invoker<std::tuple<void (*)(void*), void*> >::operator()() (this=0x5555570a1928) at /usr/include/c++/11/bits/std_thread.h:260
#9  0x00007fffc4a38b98 in std::thread::_State_impl<std::thread::_Invoker<std::tuple<void (*)(void*), void*> > >::_M_run() (this=0x5555570a1920) at /usr/include/c++/11/bits/std_thread.h:211
#10 0x00007ffff40592b3 in  () at /lib/x86_64-linux-gnu/libstdc++.so.6
#11 0x00007ffff7a93b43 in start_thread (arg=<optimized out>) at ./nptl/pthread_create.c:442
#12 0x00007ffff7b25a00 in clone3 () at ../sysdeps/unix/sysv/linux/x86_64/clone3.S:81
GiacomoBisio commented 1 year ago

We don’t know if we can consider the previous steps as completed. In that case, which might be the next steps to follow? Thank you very much.

traversaro commented 1 year ago

We don’t know if we can consider the previous steps as completed. In that case, which might be the next steps to follow? Thank you very much.

If a new problem arises, we need to fix it even if it was not planned, I am afraid we need to fix the problem with Step 4, otherwise it will make it impossible to go forward.

Anyhow, thanks for pinging, I forgot we still had this problem.

GiacomoBisio commented 1 year ago

Ok thanks, we wait for news!

traversaro commented 1 year ago

Ok thanks, we wait for news!

I will look into this as it seems quite a tricky problem, but even if a problem is hard if I was in you I would avoid to just wait for somebody to just provide the solution: tryng to solve an hard problem you do not know how to solve may be frustrating, but it is a great way to learn new skills.

traversaro commented 1 year ago

Thanks for the nice README in https://github.com/robotology/study-gazebo-garden-yarp-plugins/blob/main/robotinterface/step4/README.md, it was quite easy to reproduce the problem. The output on my system is:

(/home/traversaro/yarpgzmodern/env) traversaro@IITICUBLAP257:~/yarpgzmodern/study-gazebo-garden-yarp-plugins/robotinterface/step4$ gz sim model.sdf
QStandardPaths: wrong permissions on runtime directory /run/user/1000/, 0755 instead of 0700
[DEBUG] Reading file prova.xml
[DEBUG] yarprobotinterface: using xml parser for DTD v3.x
[DEBUG] Reading file prova.xml
[INFO] Yarprobotinterface was started using the following enable_tags:
[INFO] Yarprobotinterface was started using the following disable_tags:
[DEBUG] List of all enable attributes found in the include tags:
[DEBUG] List of all disable attributes found in the include tags:
[DEBUG] Preprocessor complete in:  2.14577e-06 s
[DEBUG] |yarp.dev.PolyDriver|fakeIMU| Parameters are (device fakeIMU)
[INFO] |yarp.device.fakeIMU| Using default period of  0.01  s
[INFO] |yarp.device.fakeIMU| sensorName:  sensorName
[INFO] |yarp.dev.PolyDriver|fakeIMU| Created device <fakeIMU>. See C++ class fakeIMU for documentation.
[INFO] startup phase starting...
[INFO] Opening device prova_multipleanalogsensorsserver with parameters [("robotName" = "/prova"), ("name" = "/prova"), ("period" = "100")]
[DEBUG] |yarp.dev.PolyDriver|prova_multipleanalogsensorsserver| Parameters are (device multipleanalogsensorsserver) (id prova_multipleanalogsensorsserver) (name "/prova") (period 100) (robotName "/prova")
[INFO] |yarp.dev.PolyDriver|prova_multipleanalogsensorsserver| Created wrapper <multipleanalogsensorsserver>. See C++ class MultipleAnalogSensorsServer for documentation.
[INFO] Entering action level 5 of phase startup
[INFO] Executing attach action, level 5 on device prova_multipleanalogsensorsserver with parameters [("device" = "fakeIMU")]
[INFO] prova_multipleanalogsensorsserver is not an IWrapper. Trying IMultipleWrapper
[INFO] |yarp.os.Port|/prova/measures:o| Port /prova/measures:o active at tcp://172.23.21.248:10002/
[INFO] |yarp.os.Port|/prova/rpc:o| Port /prova/rpc:o active at tcp://172.23.21.248:10003/
[INFO] All actions for action level 5 of startup phase started. Waiting for unfinished actions.
[INFO] All actions for action level 5 of startup phase finished.
[INFO] startup phase finished.
Stack trace (most recent call last) in thread 20804:
#7    Object "", at 0xffffffffffffffff, in
#6    Object "/usr/lib/x86_64-linux-gnu/libc.so.6", at 0x7f98172b69ff, in
#5    Object "/usr/lib/x86_64-linux-gnu/libc.so.6", at 0x7f9817224b42, in
#4    Source "../../../../../libstdc++-v3/src/c++11/thread.cc", line 104, in execute_native_thread_routine [0x7f981272ae78]
#3    Source "/home/traversaro/yarpgzmodern/yarp/src/libYARP_os/src/yarp/os/impl/ThreadImpl.cpp", line 81, in theExecutiveBranch [0x7f97e439d2ac]
         78: #endif
         79:
         80:         thread->setPriority();
      >  81:         thread->run();
         82:         thread->threadRelease();
         83:     }
#2    Source "/home/traversaro/yarpgzmodern/yarp/src/libYARP_os/src/yarp/os/PeriodicThread.cpp", line 272, in run [0x7f97e42e79b7]
        269:     void run() override
        270:     {
        271:         while (!isClosing()) {
      > 272:             step();
        273:         }
        274:     }
#1    Source "/home/traversaro/yarpgzmodern/yarp/src/libYARP_os/src/yarp/os/PeriodicThread.cpp", line 246, in step [0x7f97e42e7951]
        243:         unlock();
        244:
        245:         if (!suspended) {
      > 246:             owner->run();
        247:         }
        248:
        249:         // At the end of each run of updateModule function, the thread is supposed
#0  | Source "/home/traversaro/yarpgzmodern/yarp/src/devices/multipleanalogsensorsserver/MultipleAnalogSensorsServer.cpp", line 523, in genericStreamData<yarp::dev::IThreeAxisGyroscopes>
    |   522:     // Populate buffers
    | > 523:     ok = ok && genericStreamData(m_iThreeAxisGyroscopes, m_sensorMetadata.ThreeAxisGyroscopes,
    |   524:                                  streamingData.ThreeAxisGyroscopes.measurements,
    |   525:                                  &yarp::dev::IThreeAxisGyroscopes::getThreeAxisGyroscopeStatus,
      Source "/home/traversaro/yarpgzmodern/yarp/src/devices/multipleanalogsensorsserver/MultipleAnalogSensorsServer.cpp", line 496, in run [0x7f97df79bd5d]
        493:         {
        494:             yarp::sig::Vector& outputBuffer = streamingDataVector[i].measurement;
        495:             double& outputTimestamp = streamingDataVector[i].timestamp;
      > 496:             yarp::dev::MAS_status status = MAS_CALL_MEMBER_FN(wrappedDeviceInterface, getStatusMethodPtr)(i);
        497:             if (status == yarp::dev::MAS_OK)
        498:             {
        499:                 MAS_CALL_MEMBER_FN(wrappedDeviceInterface, getMeasureMethodPtr)(i, outputBuffer, outputTimestamp);
Segmentation fault (Address not mapped to object [0x18])
Escalating to SIGKILL on [Gazebo Sim GUI]
traversaro commented 1 year ago

Ok, I understood the problem. The key thing that you need to understand is lifetime and ownership of objects (including YARP devices). In your example, you define the yarp::os::PolyDriver fakeImu device as a local variable in the Configure method. This means that at the end of the Configure method, the fakeImu object is deleted. Instead, the yarp::robotinterface::XMLReaderResult m_xmlRobotInterfaceResult variable is an attribute of the class GazeboYarpRobotInterface . This means that it exists as long as the instance of the GazeboYarpRobotInterface class exists. What is happening is that the periodic thread created by multipleanalogsensorsserver tries to access a method of a class (fakeImu) that does not exist anymore, as Configure has returned. How do you think you can solve this problem?

lucapa17 commented 1 year ago

Okay we understood the problem and solved it (be3aefc28ce7b324c28b948ca17f346f5b559131). Thank you very much and sorry for the incovenience.

traversaro commented 1 year ago

Okay we understood the problem and solved it (be3aefc). Thank you very much and sorry for the incovenience.

No problem, feel free to ask for any problem!

GiacomoBisio commented 1 year ago

Ok thanks! So, at this point what do you suggest to do?

traversaro commented 1 year ago

I think we can go back to the original goal! We need to:

lucapa17 commented 1 year ago

We are a bit confused about how to proceed. For example we are not sure about how our YARP device should access data from Gazebo simulation. is it possible to have a call to clarify all our doubts? Thank you very much.

traversaro commented 1 year ago

This is indeed the tricky point of all the implementation, also on the gazebo classic plugins it was not obvious as it involved the use of a singleton. When do you prefer to have the call? I am quite available in this days, so please let me know when you are available.

lucapa17 commented 1 year ago

We are avaiable today from 3 pm, or every day of the next week (except Tuesday)

GiacomoBisio commented 1 year ago

Sorry but we forgot to precise that today we are available only from 3 to 5 pm

traversaro commented 1 year ago

Can we do at 15:30 ? Can you set up a meeting and send an invitation at silvio.traversaro@iit.it ? Thanks!

traversaro commented 1 year ago

Sorry, I can't make it at 15:30 due to unexpected events. Can we do on Monday at 11:00 in the morning?

GiacomoBisio commented 1 year ago

Sorry, I can't make it at 15:30 due to unexpected events. Can we do on Monday at 11:00 in the morning?

Ok no problem!

traversaro commented 1 year ago

I investigated the problem we discussed in the meeting, and I found that the gz::sensors::ForceTorqueSensor class is no contained in the gz-sensors7::gz-sensors7, but in gz-sensors7::force_torque, see:

So you just need to link gz-sensors7::force_torque in the CMake to make your example pass.

lucapa17 commented 1 year ago

So you just need to link gz-sensors7::force_torque in the CMake to make your example pass.

Thank you very much. We were wondering if there is a way to retrieve sensor data directly through this, as you have already done in Gazebo Classic. So far, we haven't found anything, so we will continue with the approach we were using. If we find a way, we will replace it in the code.

lucapa17 commented 1 year ago

We managed to implement the YARP device using also the concept of the singleton that you explained to us in the last meeting. We have some problems related to the use of the callback mechanism in ForceTorqueDriver, because we don't find anything similiar to what you used in Gazebo Classic. In particular in Gazebo Classic you could use:

// Register callback to be called at every iteration
this->updateConnection = event::Events::ConnectWorldUpdateBegin(
        std::bind(&MyPlugin::OnUpdate, this));

In Gazebo Garden in our first simple plugin we used PostUpdate callback, provided by ISystemPostUpdate, and we didn't handle directly the callback registration. Now we are not sure about what is the best way to proceed, maybe we are approaching this problem from a wrong perspective.

You can find here what we have already done for the moment (20fbaeaf3a60d742eae0fe752538f86b1bb8946f)

traversaro commented 1 year ago

Cool, nice job! Probably if there is no callback equivalent, want we can do is to define a structure that contains the information that need to be passed between the YARP device and the Gazebo plugin in a structure, like:

struct ForceTorqueData
{
    std::mutex m_mutex;
    std::array<double, 6> m_data;

}

Then, you can allocate this object in the Gazebo plugin, and in the singleton pass a pointer to this object. Then, you can populate the data of this object in the PostUpdate, and then read it in the getSixAxisForceTorqueSensorMeasure method. Let me know if this is clear, otherwise we can discuss about this tomorrow.

lucapa17 commented 1 year ago

Okay, thank you very much for the suggestion.

I think we can go back to the original goal! We need to:

  • S2.1 Implement a YARP device that inherits from yarp::dev::ISixAxisForceTorqueSensors, that reads the values from the gazebo simulation
  • S2.2 Publish the data from the YARP device implemented in S2.1 to a YARP port by launching a multipleanalogsensorsserver via the yarprobotinterface C++ api

We should have done (robotinterface/step5).

traversaro commented 1 year ago

Great job! I will look more into this next week, and then we can decide what to do next!

lucapa17 commented 1 year ago

I have a simple question. In robotinterface/step5 I would like to put the ForceTorque plugin in the sdf inside the sensor, as we did in forcetorque, instead of putting it at the beginning of the model. In this way the starting _entity would be the sensor itself and I think it would be better. But if I try to do it, I have an error because the first plugin to be executed is the GazeboYarpRobotInterface, so I have the following error:

[ERROR] Target device forcetorque_plugin_device (network = ... ) does not exist.
[ERROR] Cannot run attach action on device forcetorque_nws_yarp

It happens also putting the GazeboYarpRobotInterface plugin at the end of the model, as you did in https://github.com/robotology/gazebo-yarp-plugins/blob/master/tutorial/model/forcetorque/model.sdf

traversaro commented 1 year ago

Interesting, I guess it boils down to the order used by Gazebo to launch plugins, in Gazebo Classic first the plugin in the nested entities were launched, while in Gz it seems the contrary, that is a bit problematic for us. I will look a bit in gz-sim code and let you know.

traversaro commented 1 year ago

Anyhow, probably it make sense to open a new issue to track this improvement? This issue has been already quite long and difficult to follow, as we reached an MVP we can consider it solved and open new issues for the improvements.