search

robotology / gz-sim-yarp-plugins

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

Add gz-sim-yarp-plugins-check-model executable tool to check if model loads correctly and add CLI11 dependency #179

Closed traversaro closed 3 months ago

traversaro commented 3 months ago

This should help ensuring that downstream models using gz-sim-yarp-plugins load correctly, to avoid regression such as https://github.com/icub-tech-iit/ergocub-software/pull/232 .

I still want to iterate a bit on the tool (at the moment it takes in input world files, but I guess it may be also useful to be able to take in input model uri in the form model://ergoCubGazeboV1 that is the format actually suggest to downstream users to include models in their world .sdf files, but the basic logic is already present.

traversaro commented 3 months ago

I still want to iterate a bit on the tool (at the moment it takes in input world files, but I guess it may be also useful to be able to take in input model uri in the form model://ergoCubGazeboV1 that is the format actually suggest to downstream users to include models in their world .sdf files, but the basic logic is already present.

Done in https://github.com/robotology/gz-sim-yarp-plugins/pull/179/commits/81d35101dd8e8c3449480ba803caf4cc27647147 . To handle the --world-file and --model-uri options I added a dependency on the cli11 library, that is available both in conda-forge and apt since Ubuntu 22.04 . If there is any user that do not want this dependency, the compilation of tools can be disabled with the GZ_SIM_YARP_PLUGINS_BUILD_TOOLS CMake option.

traversaro commented 3 months ago

The tests are failing as they require https://github.com/robotology/gz-sim-yarp-plugins/pull/176 to be merged to pass.

traversaro commented 3 months ago

Interesting, gz-sim-yarp-plugins-check-model-tutorial-camera-model only fails on Linux, it works fine on macOS.

traversaro commented 3 months ago

Wow cool, the tests are not passing in Debug, let's try locally.

traversaro commented 3 months ago

Only in CI is failing with:

      Start 17: gz-sim-yarp-plugins-check-model-tutorial-forcetorque-model-two-sensors
    Test #17: gz-sim-yarp-plugins-check-model-tutorial-forcetorque-model-two-sensors ....Subprocess aborted***Exception:   2.19 sec
[INFO] Yarp configuration string loaded:  (yarpDeviceName forcetorque_plugin_device1) (jointName joint_12) (sensorName force_torque_sensor1)
[INFO] gz-sim-yarp-forcetorque-system: configuration of sensor  force_torque_sensor1  loaded
[DEBUG] |yarp.dev.PolyDriver|gazebo_forcetorque| Parameters are (device gazebo_forcetorque) (jointName joint_12) (sensorName force_torque_sensor1) (sensorScopedName "model/force_torque_model/joint/joint_12/sensor/force_torque_sensor1") (sensor_name force_torque_sensor1) (yarpDeviceName forcetorque_plugin_device1)
[INFO] |yarp.dev.PolyDriver|gazebo_forcetorque| Created device <gazebo_forcetorque>. See C++ class ForceTorqueDriver for documentation.
[INFO] Registered YARP device with instance name: model/force_torque_model/forcetorque_plugin_device1
[INFO] Yarp configuration string loaded:  (yarpDeviceName forcetorque_plugin_device2) (jointName joint_23) (sensorName force_torque_sensor2)
[INFO] gz-sim-yarp-forcetorque-system: configuration of sensor  force_torque_sensor2  loaded
[DEBUG] |yarp.dev.PolyDriver|gazebo_forcetorque| Parameters are (device gazebo_forcetorque) (jointName joint_23) (sensorName force_torque_sensor2) (sensorScopedName "model/force_torque_model/joint/joint_23/sensor/force_torque_sensor2") (sensor_name force_torque_sensor2) (yarpDeviceName forcetorque_plugin_device2)
[INFO] |yarp.dev.PolyDriver|gazebo_forcetorque| Created device <gazebo_forcetorque>. See C++ class ForceTorqueDriver for documentation.
[INFO] Registered YARP device with instance name: model/force_torque_model/forcetorque_plugin_device2
[INFO] File is a URI:  model://forcetorque/model_two_sensors/forcetorque2sensors_nws.xml
[DEBUG] Reading file /home/runner/work/gz-sim-yarp-plugins/gz-sim-yarp-plugins/tutorial/forcetorque/model_two_sensors/forcetorque2sensors_nws.xml
[DEBUG] yarprobotinterface: using xml parser for DTD v3.x
[DEBUG] Reading file /home/runner/work/gz-sim-yarp-plugins/gz-sim-yarp-plugins/tutorial/forcetorque/model_two_sensors/forcetorque2sensors_nws.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:  6.91414e-06 s
[INFO] startup phase starting...
[INFO] Opening device forcetorque1_nws_yarp with parameters [("robotName" = "forcetorque"), ("name" = "/forcetorque1"), ("period" = "100")]
[DEBUG] |yarp.dev.PolyDriver|forcetorque1_nws_yarp| Parameters are (device multipleanalogsensorsserver) (id forcetorque1_nws_yarp) (name "/forcetorque1") (period 100) (robotName forcetorque)
[DEBUG] |yarp.device.multipleanalogsensorsserver| Open complete
[INFO] |yarp.dev.PolyDriver|forcetorque1_nws_yarp| Created wrapper <multipleanalogsensorsserver>. See C++ class MultipleAnalogSensorsServer for documentation.
[INFO] Opening device forcetorque2_nws_yarp with parameters [("robotName" = "forcetorque"), ("name" = "/forcetorque2"), ("period" = "100")]
[DEBUG] |yarp.dev.PolyDriver|forcetorque2_nws_yarp| Parameters are (device multipleanalogsensorsserver) (id forcetorque2_nws_yarp) (name "/forcetorque2") (period 100) (robotName forcetorque)
[DEBUG] |yarp.device.multipleanalogsensorsserver| Open complete
[INFO] |yarp.dev.PolyDriver|forcetorque2_nws_yarp| 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 forcetorque1_nws_yarp with parameters [("device" = "forcetorque_plugin_device1")]
[INFO] forcetorque1_nws_yarp is not an IWrapper. Trying IMultipleWrapper
[INFO] |yarp.os.Port|/forcetorque1/measures:o| Port /forcetorque1/measures:o active at tcp://127.0.0.1:10002/
[INFO] |yarp.os.Port|/forcetorque1/rpc:o| Port /forcetorque1/rpc:o active at tcp://127.0.0.1:10003/
[DEBUG] |yarp.device.multipleanalogsensorsserver| Attach complete
[INFO] Executing attach action, level 5 on device forcetorque2_nws_yarp with parameters [("device" = "forcetorque_plugin_device2")]
[INFO] forcetorque2_nws_yarp is not an IWrapper. Trying IMultipleWrapper
[INFO] |yarp.os.Port|/forcetorque2/measures:o| Port /forcetorque2/measures:o active at tcp://127.0.0.1:10004/
[INFO] |yarp.os.Port|/forcetorque2/rpc:o| Port /forcetorque2/rpc:o active at tcp://127.0.0.1:10005/
[DEBUG] |yarp.device.multipleanalogsensorsserver| Attach complete
[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.
[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 forcetorque1_nws_yarp with parameters []
pure virtual method called
terminate called without an active exception

This does not happens locally with conda.

traversaro commented 3 months ago

Actually the problem is triggered by the --coverage or -O0 flag, I reproduce it locally.

traversaro commented 3 months ago

Actually the problem is triggered by the --coverage or -O0 flag, I reproduce it locally.

backtrace of the failure:

(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  0x00007ffff72ed26e in __GI_raise (sig=sig@entry=6) at ../sysdeps/posix/raise.c:26
#4  0x00007ffff72d08ff in __GI_abort () at ./stdlib/abort.c:79
#5  0x00007ffff7590f9e in __gnu_cxx::__verbose_terminate_handler () at ../../../../libstdc++-v3/libsupc++/vterminate.cc:95
#6  0x00007ffff758f4e2 in __cxxabiv1::__terminate (handler=<optimized out>) at ../../../../libstdc++-v3/libsupc++/eh_terminate.cc:48
#7  0x00007ffff75892e3 in std::terminate () at ../../../../libstdc++-v3/libsupc++/eh_terminate.cc:58
#8  0x00007ffff758fff3 in __cxxabiv1::__cxa_pure_virtual () at ../../../../libstdc++-v3/libsupc++/pure.cc:50
#9  0x00007fffc00886ea in MultipleAnalogSensorsServer::run() () from /home/traversaro/gz-sim-yarp-plugins/.pixi/envs/default/lib/yarp/yarp_multipleanalogsensorsserver.so
#10 0x00007ffff77698de in yarp::os::PeriodicThread::Private::run() () from /home/traversaro/gz-sim-yarp-plugins/.pixi/envs/default/lib/libYARP_os.so.3
#11 0x00007ffff78449d5 in theExecutiveBranch(void*) () from /home/traversaro/gz-sim-yarp-plugins/.pixi/envs/default/lib/libYARP_os.so.3
#12 0x00007ffff75acb55 in std::execute_native_thread_routine (__p=<optimized out>) at ../../../../../libstdc++-v3/src/c++11/thread.cc:104
#13 0x00007ffff7344a94 in start_thread (arg=<optimized out>) at ./nptl/pthread_create.c:447
#14 0x00007ffff73d1c3c in clone3 () at ../sysdeps/unix/sysv/linux/x86_64/clone3.S:78
traversaro commented 3 months ago

As the error was in YARP, I quickly reproduced the error in the robotology-superbuild compiling also YARP in debug, the updated bt is:

Download failed: Invalid argument.  Continuing without source file ./nptl/./nptl/pthread_kill.c.
__pthread_kill_implementation (no_tid=0, signo=6, threadid=<optimized out>) at ./nptl/pthread_kill.c:44
warning: 44     ./nptl/pthread_kill.c: No such file or directory
(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  0x00007ffff733f26e in __GI_raise (sig=sig@entry=6) at ../sysdeps/posix/raise.c:26
#4  0x00007ffff73228ff in __GI_abort () at ./stdlib/abort.c:79
#5  0x00007ffff75e2f9e in __gnu_cxx::__verbose_terminate_handler () at ../../../../libstdc++-v3/libsupc++/vterminate.cc:95
#6  0x00007ffff75e14e2 in __cxxabiv1::__terminate (handler=<optimized out>) at ../../../../libstdc++-v3/libsupc++/eh_terminate.cc:48
#7  0x00007ffff75db2e3 in std::terminate () at ../../../../libstdc++-v3/libsupc++/eh_terminate.cc:58
#8  0x00007ffff75e1ff3 in __cxxabiv1::__cxa_pure_virtual () at ../../../../libstdc++-v3/libsupc++/pure.cc:50
#9  0x00007fffbd2e6b2a in MultipleAnalogSensorsServer::genericStreamData<yarp::dev::ITemperatureSensors> (this=0x555555854130, sensorType=0x7fffbd2efbb8 "TemperatureSensor",
    getMeasureMethodPtr=&virtual yarp::dev::ITemperatureSensors::getTemperatureSensorMeasure(unsigned long, yarp::sig::VectorOf<double>&, double&) const,
    getStatusMethodPtr=&virtual yarp::dev::ITemperatureSensors::getTemperatureSensorStatus(unsigned long) const, streamingDataVector=..., metadataVector=..., wrappedDeviceInterface=0x5555576d2bc8)
    at /home/traversaro/robotology-superbuild/src/YARP/src/devices/multipleanalogsensorsserver/MultipleAnalogSensorsServer.cpp:532
#10 MultipleAnalogSensorsServer::run (this=0x555555854130) at /home/traversaro/robotology-superbuild/src/YARP/src/devices/multipleanalogsensorsserver/MultipleAnalogSensorsServer.cpp:589
#11 0x00007ffff77b60c2 in yarp::os::PeriodicThread::Private::step (this=this@entry=0x5555576e1100) at /home/traversaro/robotology-superbuild/src/YARP/src/libYARP_os/src/yarp/os/PeriodicThread.cpp:246
#12 0x00007ffff77b6128 in yarp::os::PeriodicThread::Private::run (this=0x5555576e1100) at /home/traversaro/robotology-superbuild/src/YARP/src/libYARP_os/src/yarp/os/PeriodicThread.cpp:272
#13 0x00007ffff787df55 in theExecutiveBranch (args=0x5555576e1100) at /home/traversaro/robotology-superbuild/src/YARP/src/libYARP_os/src/yarp/os/impl/ThreadImpl.cpp:81
#14 0x00007ffff75feb55 in std::execute_native_thread_routine (__p=<optimized out>) at ../../../../../libstdc++-v3/src/c++11/thread.cc:104
#15 0x00007ffff7396a94 in start_thread (arg=<optimized out>) at ./nptl/pthread_create.c:447
#16 0x00007ffff7423c3c in clone3 () at ../sysdeps/unix/sysv/linux/x86_64/clone3.S:78
traversaro commented 3 months ago

As the error was in YARP, I quickly reproduced the error in the robotology-superbuild compiling also YARP in debug, the updated bt is:

Download failed: Invalid argument.  Continuing without source file ./nptl/./nptl/pthread_kill.c.
__pthread_kill_implementation (no_tid=0, signo=6, threadid=<optimized out>) at ./nptl/pthread_kill.c:44
warning: 44     ./nptl/pthread_kill.c: No such file or directory
(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  0x00007ffff733f26e in __GI_raise (sig=sig@entry=6) at ../sysdeps/posix/raise.c:26
#4  0x00007ffff73228ff in __GI_abort () at ./stdlib/abort.c:79
#5  0x00007ffff75e2f9e in __gnu_cxx::__verbose_terminate_handler () at ../../../../libstdc++-v3/libsupc++/vterminate.cc:95
#6  0x00007ffff75e14e2 in __cxxabiv1::__terminate (handler=<optimized out>) at ../../../../libstdc++-v3/libsupc++/eh_terminate.cc:48
#7  0x00007ffff75db2e3 in std::terminate () at ../../../../libstdc++-v3/libsupc++/eh_terminate.cc:58
#8  0x00007ffff75e1ff3 in __cxxabiv1::__cxa_pure_virtual () at ../../../../libstdc++-v3/libsupc++/pure.cc:50
#9  0x00007fffbd2e6b2a in MultipleAnalogSensorsServer::genericStreamData<yarp::dev::ITemperatureSensors> (this=0x555555854130, sensorType=0x7fffbd2efbb8 "TemperatureSensor",
    getMeasureMethodPtr=&virtual yarp::dev::ITemperatureSensors::getTemperatureSensorMeasure(unsigned long, yarp::sig::VectorOf<double>&, double&) const,
    getStatusMethodPtr=&virtual yarp::dev::ITemperatureSensors::getTemperatureSensorStatus(unsigned long) const, streamingDataVector=..., metadataVector=..., wrappedDeviceInterface=0x5555576d2bc8)
    at /home/traversaro/robotology-superbuild/src/YARP/src/devices/multipleanalogsensorsserver/MultipleAnalogSensorsServer.cpp:532
#10 MultipleAnalogSensorsServer::run (this=0x555555854130) at /home/traversaro/robotology-superbuild/src/YARP/src/devices/multipleanalogsensorsserver/MultipleAnalogSensorsServer.cpp:589
#11 0x00007ffff77b60c2 in yarp::os::PeriodicThread::Private::step (this=this@entry=0x5555576e1100) at /home/traversaro/robotology-superbuild/src/YARP/src/libYARP_os/src/yarp/os/PeriodicThread.cpp:246
#12 0x00007ffff77b6128 in yarp::os::PeriodicThread::Private::run (this=0x5555576e1100) at /home/traversaro/robotology-superbuild/src/YARP/src/libYARP_os/src/yarp/os/PeriodicThread.cpp:272
#13 0x00007ffff787df55 in theExecutiveBranch (args=0x5555576e1100) at /home/traversaro/robotology-superbuild/src/YARP/src/libYARP_os/src/yarp/os/impl/ThreadImpl.cpp:81
#14 0x00007ffff75feb55 in std::execute_native_thread_routine (__p=<optimized out>) at ../../../../../libstdc++-v3/src/c++11/thread.cc:104
#15 0x00007ffff7396a94 in start_thread (arg=<optimized out>) at ./nptl/pthread_create.c:447
#16 0x00007ffff7423c3c in clone3 () at ../sysdeps/unix/sysv/linux/x86_64/clone3.S:78

Ok, I found the issue. The problem is that the FT devices are destroyed before the yarprobotinterface is closed, because gz-sim does not destroy plugins/link/something else in reverse order w.r.t. to the order with which they are built. It is the same problem described in https://github.com/robotology/gazebo-yarp-plugins/issues/582 and fixed in https://github.com/robotology/gazebo-yarp-plugins/pull/619 .

traversaro commented 3 months ago

As fixing the issue is out of scope for this PR, for now I just suppressed the failed test, and I am tracking the crash issue in https://github.com/robotology/gz-sim-yarp-plugins/issues/185 . This PR is now ready for review.

xela-95 commented 3 months ago

Thanks @traversaro this is really cool (and it will save us a lot of troubles)!! I tested it locally both with a world file and a model URI and it worked as expected.

xela-95 commented 3 months ago

I'll try to rebase this on main in order to exploit the changes done in #185

xela-95 commented 3 months ago

Rebase and squashed.

xela-95 commented 3 months ago

Merging 🚀