Implement a new plugin using Robot Interface #10

lucapa17 · 2023-05-29T06:37:02Z

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 · 2023-06-05T08:31:08Z

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

traversaro · 2023-06-05T08:33:51Z

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 · 2023-06-05T09:59:53Z

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

GiacomoBisio · 2023-06-05T10:03:47Z

Don’t worry! Thank you very much again.

traversaro · 2023-06-07T10:53:21Z

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

S1: To get use to the use of yarprobotinterface files, write a .xml file that launches two devices, and attach them.
- fakeIMU : a device that exposes a fake IMU that measures orientation, angular velocity and linear acceleration
- multipleanalogsensorsserver : a device that read the measures from the fakeIMU, and publish them on the YARP network
- You can test this file by launching the yarprobotinterface executable
S2: Once you tested successfully the .xml file with the yarprobotinterface, write a simple C++ program that uses the libYARP_robotinterface library ( https://www.yarp.it/latest/group__robointerface__all.html ) to load the .xml file and launch it, without passing through the yarprobotinterface executable (but you can check the yarprobotinterface executable code to get an inspirtation)
S3: Modify the code prepare in S2 to launch the fakeIMU not via the .xml, but via the code, and just launch the multipleanalogsensorsserver. This is a use case more similar to the one you will see in the gazebo
S4: Implement a simple Gazebo plugin that loads the .xml file via the libYARP_robotinterface library launches the multipleanalogsensorsserver device

traversaro · 2023-06-07T10:53:33Z

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

lucapa17 · 2023-06-15T14:35:13Z

Sorry but in the last days we were a bit busy.
We completed the first step (91dadcb) and the second step (ed87cc1) but we have some problems with the third one (e157172). 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 · 2023-06-16T02:17:24Z

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 · 2023-06-16T07:52:51Z

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 (4f6f455). Thank you!

lucapa17 · 2023-06-16T13:24:03Z

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 (5a52755). 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 · 2023-06-16T19:51:10Z

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

GiacomoBisio · 2023-06-16T22:57:09Z

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 · 2023-06-17T12:57:22Z

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.

GiacomoBisio · 2023-06-17T14:40:23Z

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 · 2023-06-17T15:01:51Z

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 · 2023-06-17T15:10:59Z

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 · 2023-06-18T10:51:01Z

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 · 2023-06-18T11:05:12Z

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 · 2023-06-19T10:16:42Z

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 · 2023-06-21T06:04:43Z

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 · 2023-06-21T07:01:21Z

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 · 2023-06-21T07:25:19Z

Ok thanks, we wait for news!

traversaro · 2023-06-21T07:56:12Z

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 · 2023-06-21T09:45:45Z

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 · 2023-06-21T09:54:24Z

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 · 2023-06-21T10:21:45Z

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

traversaro · 2023-06-21T10:51:57Z

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 · 2023-06-21T11:40:03Z

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

traversaro · 2023-06-21T11:44:31Z

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

lucapa17 · 2023-06-23T08:56:09Z

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 · 2023-06-23T09:20:03Z

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 · 2023-06-23T09:27:30Z

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

GiacomoBisio · 2023-06-23T12:40:40Z

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

traversaro · 2023-06-23T12:48:45Z

Can we do at 15:30 ? Can you set up a meeting and send an invitation at [email protected] ? Thanks!

traversaro · 2023-06-23T13:37:20Z

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 · 2023-06-23T13:38:49Z

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 · 2023-06-26T12:12:39Z

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:

https://github.com/gazebosim/gz-sensors/blob/gz-sensors7_7.2.0/examples/loop_sensor/CMakeLists.txt#L21 (from which I understood that each sensor has its library)
I realized this later, but we could have checked directly in which library the ForceTorqueSensor.cc file was added: https://github.com/gazebosim/gz-sensors/blob/gz-sensors7_7.2.0/src/CMakeLists.txt#L145C41-L145C52 .

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

lucapa17 · 2023-06-27T08:07:04Z

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 · 2023-06-29T14:37:25Z

S2.1 Implement a YARP device that inherits from yarp::dev::ISixAxisForceTorqueSensors, that reads the values from the gazebo simulation

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 (20fbaea)

traversaro · 2023-06-29T15:17:22Z

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 · 2023-06-30T15:48:24Z

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 · 2023-06-30T16:11:26Z

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

lucapa17 · 2023-07-01T10:57:51Z

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 · 2023-07-01T11:30:31Z

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 · 2023-07-03T09:06:09Z

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.

lucapa17 mentioned this issue May 29, 2023

Develop Gazebo Garden FT sensor YARP plugin #4

Closed

lucapa17 mentioned this issue Jul 3, 2023

Run ForceTorque and RobotInterface by specifying ForceTorque plugin as part of <sensor> sdf tag #11

Open

lucapa17 closed this as completed Jul 3, 2023

traversaro mentioned this issue Jul 13, 2023

[EPIC] Port gazebo-yarp-plugins functionality to Modern Gazebo #17

Closed

6 tasks

Implement a new plugin using Robot Interface #10

Implement a new plugin using Robot Interface #10

Comments

lucapa17 commented May 29, 2023

GiacomoBisio commented Jun 5, 2023

traversaro commented Jun 5, 2023

traversaro commented Jun 5, 2023

GiacomoBisio commented Jun 5, 2023

traversaro commented Jun 7, 2023

traversaro commented Jun 7, 2023

lucapa17 commented Jun 15, 2023

traversaro commented Jun 16, 2023

lucapa17 commented Jun 16, 2023 • edited Loading

lucapa17 commented Jun 16, 2023

traversaro commented Jun 16, 2023

GiacomoBisio commented Jun 16, 2023

traversaro commented Jun 17, 2023 • edited Loading

GiacomoBisio commented Jun 17, 2023

traversaro commented Jun 17, 2023

traversaro commented Jun 17, 2023

lucapa17 commented Jun 18, 2023

traversaro commented Jun 18, 2023

lucapa17 commented Jun 19, 2023

GiacomoBisio commented Jun 21, 2023

traversaro commented Jun 21, 2023

GiacomoBisio commented Jun 21, 2023

traversaro commented Jun 21, 2023

traversaro commented Jun 21, 2023 • edited Loading

traversaro commented Jun 21, 2023

lucapa17 commented Jun 21, 2023

traversaro commented Jun 21, 2023

GiacomoBisio commented Jun 21, 2023

traversaro commented Jun 21, 2023

lucapa17 commented Jun 23, 2023

traversaro commented Jun 23, 2023

lucapa17 commented Jun 23, 2023

GiacomoBisio commented Jun 23, 2023

traversaro commented Jun 23, 2023

traversaro commented Jun 23, 2023

GiacomoBisio commented Jun 23, 2023

traversaro commented Jun 26, 2023

lucapa17 commented Jun 27, 2023

lucapa17 commented Jun 29, 2023

traversaro commented Jun 29, 2023

lucapa17 commented Jun 30, 2023

traversaro commented Jun 30, 2023

lucapa17 commented Jul 1, 2023

traversaro commented Jul 1, 2023

traversaro commented Jul 3, 2023

lucapa17 commented Jun 16, 2023 •

edited

Loading

traversaro commented Jun 17, 2023 •

edited

Loading

traversaro commented Jun 21, 2023 •

edited

Loading