Add gz-sim-yarp-plugins-check-model executable tool to check if a .wo…

…rld file containing gz-sim-yarp-plugins plugin is able to load correctly

.github/workflows/apt.yml

@@ -42,7 +42,7 @@ jobs:
         # YARP dependencies
         sudo apt-get install git build-essential cmake ninja-build libace-dev libeigen3-dev libopencv-dev libtinyxml-dev
         # gz-sim-yarp-plugins dependencies
-        sudo apt-get install gz-${{ matrix.gazebo_distro }}
+        sudo apt-get install gz-${{ matrix.gazebo_distro }} libcli11-dev
         # Test dependencies
         sudo apt-get install libgtest-dev
 

CMakeLists.txt

@@ -14,6 +14,12 @@ set(GZ_PLUGIN_VER ${gz-plugin2_VERSION_MAJOR})
 gz_find_package(gz-sim8 REQUIRED)
 set(GZ_SIM_VER ${gz-sim8_VERSION_MAJOR})
 
+option(GZ_SIM_YARP_PLUGINS_BUILD_TOOLS "If enabled, build command line helper tools" ON)
+
+if(GZ_SIM_YARP_PLUGINS_BUILD_TOOLS)
+    find_package(CLI11 REQUIRED)
+endif()
+
 # Defines the CMAKE_INSTALL_LIBDIR, CMAKE_INSTALL_BINDIR and many other useful macros.
 # See https://cmake.org/cmake/help/latest/module/GNUInstallDirs.html
 include(GNUInstallDirs)
@@ -48,6 +54,10 @@ set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON)
 add_subdirectory(libraries)
 add_subdirectory(plugins)
 
+if(GZ_SIM_YARP_PLUGINS_BUILD_TOOLS)
+    add_subdirectory(tools)
+endif()
+
 # Create and install CMake configuration files for this project that are
 # necessary for other projects to call find_package(gz-sim-yarp-plugins).
 include(InstallBasicPackageFiles)

ci_env.yml

@@ -11,3 +11,4 @@ dependencies:
   - yarp
   - gtest
   - cmake-package-check
+  - cli11

docs/build-from-source.md

@@ -60,7 +60,7 @@ If you are using an apt-based distribution such as Ubuntu and you want to use ap
 
 ```bash
 sudo apt-get update
-sudo apt-get install lsb-release wget gnupg cmake pkg-config ninja-build build-essential libgtest-dev
+sudo apt-get install lsb-release wget gnupg cmake pkg-config ninja-build build-essential libcli11-dev libgtest-dev
 ```
 
 and then install Gazebo Harmonic:

libraries/common/Common.hh

@@ -5,6 +5,7 @@
 
 #include <yarp/os/LogStream.h>
 #include <yarp/os/Value.h>
+
 namespace gzyarp
 {
 constexpr double pi = 3.1415926535897932384626433;

libraries/device-registry/CMakeLists.txt

@@ -17,5 +17,5 @@ target_link_libraries(gz-sim-yarp-device-registry
 install(TARGETS gz-sim-yarp-device-registry
         EXPORT ${PROJECT_NAME})
 
-install(FILES DeviceRegistry.hh 
+install(FILES DeviceRegistry.hh
         DESTINATION "${CMAKE_INSTALL_INCLUDEDIR}/gzyarp")

libraries/device-registry/DeviceRegistry.cc

@@ -21,6 +21,20 @@
 namespace gzyarp
 {
 
+PluginConfigureHelper::~PluginConfigureHelper()
+{
+    if (!m_configureSuccessful)
+    {
+        DeviceRegistry::getHandler()->incrementNrOfGzSimYARPPluginsNotSuccessfullyLoaded(*m_pecm);
+    }
+}
+
+void PluginConfigureHelper::setConfigureIsSuccessful(bool success)
+{
+    m_configureSuccessful = success;
+    return;
+}
+
 DeviceRegistry* DeviceRegistry::getHandler()
 {
     std::lock_guard<std::mutex> lock(mutex());
@@ -294,4 +308,41 @@ std::mutex& DeviceRegistry::mutex()
     return s_mutex;
 }
 
+std::size_t DeviceRegistry::getNrOfGzSimYARPPluginsNotSuccessfullyLoaded(const gz::sim::EntityComponentManager& ecm) const
+{
+    auto it = m_nrOfGzSimYARPPluginsNotSuccessfullyLoaded.find(getGzInstanceId(ecm));
+
+    if (it == m_nrOfGzSimYARPPluginsNotSuccessfullyLoaded.end())
+    {
+        return 0;
+    } else
+    {
+        return it->second;
+    }
+}
+
+std::size_t DeviceRegistry::getTotalNrOfGzSimYARPPluginsNotSuccessfullyLoaded() const
+{
+    std::size_t ret = 0;
+    for (const auto& pair : m_nrOfGzSimYARPPluginsNotSuccessfullyLoaded) {
+        ret += pair.second;
+    }
+    return ret;
+}
+
+void DeviceRegistry::incrementNrOfGzSimYARPPluginsNotSuccessfullyLoaded(const gz::sim::EntityComponentManager& ecm)
+{
+    std::string ecmid = getGzInstanceId(ecm);
+    auto it = m_nrOfGzSimYARPPluginsNotSuccessfullyLoaded.find(getGzInstanceId(ecm));
+
+    if (it == m_nrOfGzSimYARPPluginsNotSuccessfullyLoaded.end())
+    {
+        m_nrOfGzSimYARPPluginsNotSuccessfullyLoaded[ecmid] = 1;
+    }
+    else
+    {
+        m_nrOfGzSimYARPPluginsNotSuccessfullyLoaded[ecmid] = m_nrOfGzSimYARPPluginsNotSuccessfullyLoaded[ecmid] + 1;
+    }
+}
+
 } // namespace gzyarp

libraries/device-registry/DeviceRegistry.hh

@@ -14,6 +14,23 @@
 namespace gzyarp
 {
 
+/**
+ * Class instantiated by all gz-sim-yarp-plugins in the plugin Configure.
+ *
+ * The class destructor calls DeviceRegistry::getHandler()->incrementNrOfGzSimYARPPluginsNotSuccessfullyLoaded(ecm),
+ * unless the setConfigureIsSuccessful(true) method is called to signal that the configure of the plugin has been successful.
+ */
+class PluginConfigureHelper {
+public:
+    PluginConfigureHelper(const gz::sim::EntityComponentManager& ecm) : m_configureSuccessful(false), m_pecm(&ecm) {}
+    ~PluginConfigureHelper();
+    void setConfigureIsSuccessful(bool success);
+
+private:
+    bool m_configureSuccessful;
+    const gz::sim::EntityComponentManager* m_pecm;
+};
+
 class DeviceRegistry
 {
 public:
@@ -39,6 +56,22 @@ public:
 
     std::vector<std::string> getDevicesKeys(const gz::sim::EntityComponentManager& ecm) const;
 
+    /**
+     * Get number of gz-sim-yarp-plugins not successfully loaded for a specific simulation server.
+     */
+    std::size_t getNrOfGzSimYARPPluginsNotSuccessfullyLoaded(const gz::sim::EntityComponentManager& ecm) const;
+
+    /**
+     * Get number of gz-sim-yarp-plugins not successfully loaded for all simulation in the process.
+     */
+    std::size_t getTotalNrOfGzSimYARPPluginsNotSuccessfullyLoaded() const;
+
+    /**
+     * Increment number of gz-sim-yarp-plugins not successfully loaded. This function is only meant to be called by
+     * gz-sim-yarp-plugins gz-sim plugins.
+     */
+    void incrementNrOfGzSimYARPPluginsNotSuccessfullyLoaded(const gz::sim::EntityComponentManager& ecm);
+
 private:
     static std::string generateDeviceId(const gz::sim::Entity& entity,
                                         const gz::sim::EntityComponentManager& ecm,
@@ -55,6 +88,9 @@ private:
     static std::mutex& mutex();
     std::unordered_map<std::string, std::unordered_map<std::string, yarp::dev::PolyDriver*>>
         m_devicesMap; // map of known yarp devices
+
+    // Number of gz-sim-yarp-plugins YARP devices not loaded correctly for a given ecm
+    std::unordered_map<std::string, std::size_t> m_nrOfGzSimYARPPluginsNotSuccessfullyLoaded;
 };
 
 } // namespace gzyarp

plugins/basestate/BaseState.cc

@@ -69,6 +69,8 @@ class BaseState : public System, public ISystemConfigure, public ISystemPostUpda
 
         std::string netWrapper = "analogServer";
 
+        gzyarp::PluginConfigureHelper configureHelper(_ecm);
+
         m_ecm = &_ecm;
 
         using BaseStateDriverCreator
@@ -156,6 +158,8 @@ class BaseState : public System, public ISystemConfigure, public ISystemPostUpda
                      << m_deviceId << ")";
             return;
         }
+
+        configureHelper.setConfigureIsSuccessful(true);
         m_deviceRegistered = true;
         yInfo() << "Registered YARP device with instance name:" << m_deviceId;
     }

plugins/camera/Camera.cc

@@ -77,6 +77,8 @@ class Camera : public System,
     {
         yarp::os::Network::init();
 
+        gzyarp::PluginConfigureHelper configureHelper(_ecm);
+
         ecm = &_ecm;
 
         ::yarp::dev::Drivers::factory().add(
@@ -169,6 +171,7 @@ class Camera : public System,
         }
         this->m_deviceRegistered = true;
         this->cameraInitialized = false;
+        configureHelper.setConfigureIsSuccessful(true);
         yInfo() << "gz-sim-yarp-camera-system: Registered YARP device with instance name:"
                 << m_deviceId;
     }

plugins/clock/CMakeLists.txt

@@ -8,7 +8,8 @@ target_link_libraries(gz-sim-yarp-clock-system
   PRIVATE
     YARP::YARP_dev
     YARP::YARP_os
-    YARP::YARP_init)
+    YARP::YARP_init
+    gz-sim-yarp-device-registry)
 
 # Add install target
 install(TARGETS gz-sim-yarp-clock-system)

plugins/clock/Clock.cc

@@ -1,3 +1,5 @@
+#include <DeviceRegistry.hh>
+
 #include <chrono>
 #include <memory>
 #include <string>
@@ -49,12 +51,16 @@ class Clock : public System, public ISystemConfigure, public ISystemPostUpdate,
     {
         if (!m_initialized)
         {
+            gzyarp::PluginConfigureHelper configureHelper(_ecm);
+
             m_initialized = true;
             if (!m_clockPort.open(m_portName))
             {
                 yError() << "Failed to open port" << m_portName;
                 return;
             }
+
+            configureHelper.setConfigureIsSuccessful(true);
             yInfo() << "gz-sim-yarp-clock-system plugin initialized.";
         }
     }

plugins/controlboard/src/ControlBoard.cpp

@@ -84,6 +84,8 @@ void ControlBoard::Configure(const Entity& _entity,
 
     bool wipe = false;
 
+    gzyarp::PluginConfigureHelper configureHelper(_ecm);
+
     m_ecm = &_ecm;
 
     if (ConfigurationHelpers::loadPluginConfiguration(_sdf, m_pluginParameters))
@@ -157,6 +159,7 @@ void ControlBoard::Configure(const Entity& _entity,
 
     resetPositionsAndTrajectoryGenerators(_ecm);
 
+    configureHelper.setConfigureIsSuccessful(true);
     yInfo() << "Registered YARP device with instance name:" << m_deviceId;
     m_deviceRegistered = true;
 }

plugins/forcetorque/ForceTorque.cc

@@ -68,6 +68,8 @@ class ForceTorque : public System,
     {
         yarp::os::Network::init();
 
+        gzyarp::PluginConfigureHelper configureHelper(_ecm);
+
         ecm = &_ecm;
 
         std::string netWrapper = "analogServer";
@@ -147,6 +149,8 @@ class ForceTorque : public System,
                      << m_deviceId << ") into DeviceRegistry";
             return;
         }
+
+        configureHelper.setConfigureIsSuccessful(true);
         m_deviceRegistered = true;
         yInfo() << "Registered YARP device with instance name:" << m_deviceId;
     }

plugins/imu/Imu.cc

@@ -78,6 +78,8 @@ class Imu : public System,
 
         ::yarp::os::Property driver_properties;
 
+        gzyarp::PluginConfigureHelper configureHelper(_ecm);
+
         ecm = &_ecm;
 
         if (ConfigurationHelpers::loadPluginConfiguration(_sdf, driver_properties))
@@ -146,6 +148,7 @@ class Imu : public System,
             return;
         }
 
+        configureHelper.setConfigureIsSuccessful(true);
         m_deviceRegistered = true;
         yInfo() << "Registered YARP device with instance name:" << m_deviceId;
     }

plugins/laser/Laser.cc

@@ -80,6 +80,8 @@ class Laser : public System,
                                                                                "LaserDriver"));
         ::yarp::os::Property driver_properties;
 
+        gzyarp::PluginConfigureHelper configureHelper(_ecm);
+
         ecm = &_ecm;
 
         if (ConfigurationHelpers::loadPluginConfiguration(_sdf, driver_properties))
@@ -147,6 +149,8 @@ class Laser : public System,
                      << ")";
             return;
         }
+
+        configureHelper.setConfigureIsSuccessful(true);
         m_deviceRegistered = true;
         yInfo() << "Registered YARP device with instance name:" << m_deviceId;
     }

plugins/robotinterface/RobotInterface.cc

@@ -74,6 +74,8 @@ class RobotInterface : public System, public ISystemConfigure
         yarp::os::Network::init();
         auto model = Model(_entity);
 
+        gzyarp::PluginConfigureHelper configureHelper(_ecm);
+
         if (!loadYarpRobotInterfaceConfigurationFile(_sdf, _ecm, model))
         {
             yError("gz-sim-yarp-robotinterface-system : Error loading robotinterface configuration "
@@ -104,6 +106,8 @@ class RobotInterface : public System, public ISystemConfigure
             m_xmlRobotInterfaceResult.robot.enterPhase(yarp::robotinterface::ActionPhaseShutdown);
             return;
         }
+
+        configureHelper.setConfigureIsSuccessful(true);
         m_robotInterfaceCorrectlyStarted = true;
     }
 

tests/CMakeLists.txt

@@ -8,3 +8,7 @@ add_subdirectory(clock)
 add_subdirectory(controlboard)
 add_subdirectory(commons)
 add_subdirectory(test-helpers)
+
+if(GZ_SIM_YARP_PLUGINS_BUILD_TOOLS)
+    add_subdirectory(gz-sim-yarp-plugins-check-model)
+endif()

tests/gz-sim-yarp-plugins-check-model/CMakeLists.txt

@@ -0,0 +1,40 @@
+macro(gsyp_add_tutorial_model_check tutorialname worldrelpath)
+    add_test(NAME gz-sim-yarp-plugins-check-model-tutorial-${tutorialname}
+             COMMAND gz-sim-yarp-plugins-check-model --world-file ${PROJECT_SOURCE_DIR}/tutorial/${worldrelpath})
+    set(_env_vars)
+    list(APPEND _env_vars
+        "GZ_SIM_SYSTEM_PLUGIN_PATH=$<TARGET_FILE_DIR:gz-sim-yarp-device-registry>"
+        "GZ_SIM_RESOURCE_PATH=${GZ_SIM_RESOURCE_PATH}:${PROJECT_SOURCE_DIR}/tutorial"
+    )
+    set_tests_properties(gz-sim-yarp-plugins-check-model-tutorial-${tutorialname} PROPERTIES
+        ENVIRONMENT "${_env_vars}")
+endmacro()
+
+# Check that the tutorial starts correctly
+gsyp_add_tutorial_model_check(basestate basestate/model.sdf)
+# TODO(traversaro): test that are comment are segfaulting on pixi+WSL2, we should understand why and uncomment them
+gsyp_add_tutorial_model_check(camera-model camera/model/model.sdf)
+# gsyp_add_tutorial_model_check(camera-model-horizontal-flip camera/model_horizontal_flip/model.sdf)
+# gsyp_add_tutorial_model_check(camera-model-vertical-flip camera/model_vertical_flip/model.sdf)
+# gsyp_add_tutorial_model_check(laser laser/model.sdf)
+gsyp_add_tutorial_model_check(clock clock/model.sdf)
+gsyp_add_tutorial_model_check(forcetorque-model-one-sensor forcetorque/model_one_sensor/model.sdf)
+gsyp_add_tutorial_model_check(forcetorque-model-two-sensors forcetorque/model_two_sensors/model2sensors.sdf)
+gsyp_add_tutorial_model_check(single-pendulum single_pendulum/model.sdf)
+
+# Then check that the command correctly exit a failure if we pass it a invalid model
+add_test(NAME gz-sim-yarp-plugins-check-model-fails-as-expected-with-wrong-world-file
+         COMMAND gz-sim-yarp-plugins-check-model --world-file  ./world_with_robotinterface_with_non_existing_filename.sdf
+         WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR})
+set(_env_vars)
+list(APPEND _env_vars
+    "GZ_SIM_SYSTEM_PLUGIN_PATH=$<TARGET_FILE_DIR:gz-sim-yarp-device-registry>"
+)
+set_tests_properties(gz-sim-yarp-plugins-check-model-fails-as-expected-with-wrong-world-file PROPERTIES
+    ENVIRONMENT "${_env_vars}")
+set_property(TEST gz-sim-yarp-plugins-check-model-fails-as-expected-with-wrong-world-file PROPERTY WILL_FAIL true)
+
+# Check that the command fails if not argument is passed
+add_test(NAME gz-sim-yarp-plugins-check-model-fails-as-expected-with-no-options
+         COMMAND gz-sim-yarp-plugins-check-model)
+set_property(TEST gz-sim-yarp-plugins-check-model-fails-as-expected-with-no-options PROPERTY WILL_FAIL true)