Several improvements:

- Update DeviceRegistry::getDevicesAsPolyDriverList method - Add pragma once direcgtive to DeviceRegistry - Remove check to BaseStateDriver raising exeption with new data management - Move initialization of Camera buffers after Driver has been opened - Add test-helpers library and TestHelpers class - Fix Imu test - Update ControlBoardPositionDirectTest (still failing)
robotology · Apr 17, 2024 · a901e0f · a901e0f
1 parent c345e4c
commit a901e0f
Show file tree

Hide file tree

Showing 18 changed files with 326 additions and 147 deletions.
diff --git a/libraries/device-registry/DeviceRegistry.cc b/libraries/device-registry/DeviceRegistry.cc
@@ -8,6 +8,7 @@
 #include <sstream>
 #include <string>
 #include <unordered_map>
+#include <utility>
 #include <vector>
 
 #include <yarp/dev/PolyDriver.h>
@@ -51,8 +52,7 @@ bool DeviceRegistry::getDevicesAsPolyDriverList(
 
         // If the deviceDatabaseKey starts with the modelScopedName (device spawned by model
         // plugins), then it is eligible for insertion in the returned list
-        if ((deviceDatabaseKey.rfind(modelScopedName, 0)
-             == 0) /*|| (deviceDatabaseKey.rfind(worldName + "/" + modelScopedName, 0) == 0)*/)
+        if ((deviceDatabaseKey.find(modelScopedName, 0) != std::string::npos))
         {
             // Extract yarpDeviceName from deviceDatabaseKey
             yarpDeviceName = deviceDatabaseKey.substr(deviceDatabaseKey.find_last_of("/") + 1);
@@ -153,7 +153,6 @@ DeviceRegistry::getDevicesKeys(const gz::sim::EntityComponentManager& ecm) const
 
     {
         std::lock_guard<std::mutex> lock(mutex());
-        std::cerr << "==================== ecm address: " << EcmPtrSs.str() << std::endl;
         std::vector<std::string> keys;
         for (auto&& [key, value] : m_devicesMap)
         {

diff --git a/libraries/device-registry/DeviceRegistry.hh b/libraries/device-registry/DeviceRegistry.hh
@@ -1,3 +1,5 @@
+#pragma once
+
 #include <map>
 #include <mutex>
 #include <string>

diff --git a/plugins/basestate/BaseState.cc b/plugins/basestate/BaseState.cc
@@ -2,6 +2,7 @@
 #include <BaseStateShared.hh>
 #include <ConfigurationHelpers.hh>
 #include <DeviceIdGenerator.hh>
+#include <DeviceRegistry.hh>
 
 #include <memory>
 #include <mutex>

diff --git a/plugins/basestate/BaseStateDriver.cpp b/plugins/basestate/BaseStateDriver.cpp
@@ -59,12 +59,6 @@ class yarp::dev::gzyarp::BaseStateDriver : public yarp::dev::DeviceDriver,
             m_baseLinkName = config.find("baseLink").asString().substr(pos + separator.size() - 1);
         }
 
-        if (!m_baseLinkData)
-        {
-            yError() << "Error, BaseState sensor was not found";
-            return false;
-        }
-
         return true;
     }
 

diff --git a/plugins/camera/Camera.cc b/plugins/camera/Camera.cc
@@ -2,6 +2,7 @@
 #include <CameraShared.hh>
 #include <ConfigurationHelpers.hh>
 #include <DeviceIdGenerator.hh>
+#include <DeviceRegistry.hh>
 
 #include <cstddef>
 #include <cstring>

diff --git a/plugins/camera/CameraDriver.cpp b/plugins/camera/CameraDriver.cpp
@@ -3,6 +3,7 @@
 
 #include <cstdio>
 #include <cstring>
+#include <iostream>
 #include <mutex>
 
 #include <string>
@@ -62,12 +63,6 @@ class yarp::dev::gzyarp::CameraDriver : public yarp::dev::DeviceDriver,
     {
         std::string sensorScopedName(config.find(YarpCameraScopedName.c_str()).asString().c_str());
 
-        {
-            std::lock_guard<std::mutex> lock(m_sensorData->m_mutex);
-            m_sensorData->m_imageBuffer = new unsigned char[getRawBufferSize()];
-            memset(m_sensorData->m_imageBuffer, 0x00, getRawBufferSize());
-        }
-
         if (config.check("vertical_flip"))
             m_vertical_flip = true;
         if (config.check("horizontal_flip"))
@@ -275,6 +270,18 @@ class yarp::dev::gzyarp::CameraDriver : public yarp::dev::DeviceDriver,
     void setCameraData(::gzyarp::CameraData* dataPtr) override
     {
         m_sensorData = dataPtr;
+
+        // Now that we have a pointer to CameraData we can initialize the camera buffers
+        initializeCamera();
+    }
+
+    void initializeCamera()
+    {
+        {
+            std::lock_guard<std::mutex> lock(m_sensorData->m_mutex);
+            m_sensorData->m_imageBuffer = new unsigned char[getRawBufferSize()];
+            memset(m_sensorData->m_imageBuffer, 0x00, getRawBufferSize());
+        }
     }
 
 private:

diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt
@@ -7,3 +7,4 @@ add_subdirectory(camera)
 add_subdirectory(clock)
 add_subdirectory(controlboard)
 add_subdirectory(commons)
+add_subdirectory(test-helpers)
diff --git a/tests/commons/CMakeLists.txt b/tests/commons/CMakeLists.txt
@@ -1,24 +1,54 @@
-add_executable(ConfigurationParsingTest ConfigurationParsingTest.cc)
+add_executable(ConfigurationParsingFromFileTest ConfigurationParsingFromFileTest.cc)
+add_executable(ConfigurationParsingFromStringTest ConfigurationParsingFromStringTest.cc)
 
-target_link_libraries(ConfigurationParsingTest
+target_link_libraries(ConfigurationParsingFromFileTest
 PRIVATE
   GTest::gtest_main
+  test-helpers
   gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER}
   gz-plugin${GZ_PLUGIN_VER}::gz-plugin${GZ_PLUGIN_VER}
   YARP::YARP_dev
   YARP::YARP_os
   YARP::YARP_init
   gz-sim-yarp-device-registry
+  gz-sim-yarp-basestate-system
+  gz-sim-yarp-camera-system
+  gz-sim-yarp-controlboard-system
+  gz-sim-yarp-forcetorque-system
+  gz-sim-yarp-imu-system
+  gz-sim-yarp-laser-system
+)
+
+target_link_libraries(ConfigurationParsingFromStringTest
+PRIVATE
+  GTest::gtest_main
+  test-helpers
+  gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER}
+  gz-plugin${GZ_PLUGIN_VER}::gz-plugin${GZ_PLUGIN_VER}
+  YARP::YARP_dev
+  YARP::YARP_os
+  YARP::YARP_init
+  gz-sim-yarp-device-registry
+  gz-sim-yarp-basestate-system
+  gz-sim-yarp-camera-system
+  gz-sim-yarp-controlboard-system
+  gz-sim-yarp-forcetorque-system
+  gz-sim-yarp-imu-system
+  gz-sim-yarp-laser-system
 )
 
-add_test(NAME ConfigurationParsingTest COMMAND ConfigurationParsingTest)
+
+# add_test(NAME ConfigurationParsingFromFileTest COMMAND ConfigurationParsingFromFileTest)
+add_test(NAME ConfigurationParsingFromStringTest COMMAND ConfigurationParsingFromStringTest)
 
 set(_env_vars)
-# list(APPEND _env_vars "GZ_SIM_SYSTEM_PLUGIN_PATH=$<TARGET_FILE_DIR:gz-sim-yarp-camera-system>")
 list(APPEND _env_vars "LIBGL_ALWAYS_SOFTWARE=1"
   "GZ_SIM_SYSTEM_PLUGIN_PATH=$<TARGET_FILE_DIR:gz-sim-yarp-device-registry>"
-  "GZ_SIM_RESOURCE_PATH=${CMAKE_CURRENT_SOURCE_DIR}/.."
-  )
+  "GZ_SIM_RESOURCE_PATH=${GZ_SIM_RESOURCE_PATH}:${CMAKE_CURRENT_SOURCE_DIR}/.."
+)
+
+# set_tests_properties(ConfigurationParsingFromFileTest PROPERTIES
+#   ENVIRONMENT "${_env_vars}")
 
-set_tests_properties(ConfigurationParsingTest PROPERTIES
+set_tests_properties(ConfigurationParsingFromStringTest PROPERTIES
   ENVIRONMENT "${_env_vars}")
diff --git a/tests/commons/ConfigurationParsingFromFileTest.cc b/tests/commons/ConfigurationParsingFromFileTest.cc
@@ -0,0 +1,84 @@
+#include <gtest/gtest.h>
+
+#include <BaseStateDriver.cpp>
+#include <CameraDriver.cpp>
+#include <ControlBoardDriver.hh>
+#include <DeviceRegistry.hh>
+#include <ForceTorqueDriver.cpp>
+#include <ImuDriver.cpp>
+#include <LaserDriver.cpp>
+#include <TestHelpers.hh>
+
+#include <gz/common/Console.hh>
+#include <gz/sim/Entity.hh>
+#include <gz/sim/EntityComponentManager.hh>
+#include <gz/sim/EventManager.hh>
+#include <gz/sim/TestFixture.hh>
+#include <sdf/Element.hh>
+
+#include <iostream>
+#include <memory>
+#include <string>
+
+#include <yarp/dev/IFrameGrabberImage.h>
+#include <yarp/dev/PolyDriver.h>
+#include <yarp/os/Network.h>
+
+TEST(ConfigurationParsingTest, LoadPluginsWithYarpConfigurationFile)
+{
+    std::string modelSdfName = "model_configuration_file.sdf";
+    std::string sdfPath = std::string("../../../tests/commons/") + modelSdfName;
+    yarp::os::NetworkBase::setLocalMode(true);
+    gz::common::Console::SetVerbosity(4);
+
+    gz::sim::TestFixture testFixture{sdfPath};
+
+    testFixture.OnConfigure([&](const gz::sim::Entity& _worldEntity,
+                                const std::shared_ptr<const sdf::Element>& /*_sdf*/,
+                                gz::sim::EntityComponentManager& _ecm,
+                                gz::sim::EventManager& /*_eventMgr*/) {
+        // Test Camera
+        // std::cerr << "Testing Camera configuration" << std::endl;
+        // auto cameraDrivers
+        //     = gzyarp::testing::TestHelpers::getDevicesOfType<yarp::dev::gzyarp::CameraDriver>();
+        // EXPECT_EQ(cameraDrivers.size(), 1);
+        // EXPECT_TRUE(cameraDrivers[0] != nullptr);
+
+        // Test ForceTorque
+        std::cerr << "Testing FT configuration" << std::endl;
+        auto ftDrivers
+            = gzyarp::testing::TestHelpers::getDevicesOfType<yarp::dev::gzyarp::ForceTorqueDriver>();
+        EXPECT_EQ(ftDrivers.size(), 1);
+        EXPECT_TRUE(ftDrivers[0] != nullptr);
+
+        // Test Imu
+        std::cerr << "Testing Imu configuration" << std::endl;
+        auto imuDrivers
+            = gzyarp::testing::TestHelpers::getDevicesOfType<yarp::dev::gzyarp::ImuDriver>();
+        EXPECT_EQ(imuDrivers.size(), 1);
+        EXPECT_TRUE(imuDrivers[0] != nullptr);
+
+        // Test Laser
+        std::cerr << "Testing Laser configuration" << std::endl;
+        auto laserDrivers
+            = gzyarp::testing::TestHelpers::getDevicesOfType<yarp::dev::gzyarp::LaserDriver>();
+        EXPECT_EQ(laserDrivers.size(), 1);
+        EXPECT_TRUE(laserDrivers[0] != nullptr);
+
+        // Test basestate
+        std::cerr << "Testing BaseState configuration" << std::endl;
+        auto bsDrivers
+            = gzyarp::testing::TestHelpers::getDevicesOfType<yarp::dev::gzyarp::BaseStateDriver>();
+        EXPECT_EQ(bsDrivers.size(), 1);
+        EXPECT_TRUE(bsDrivers[0] != nullptr);
+
+        // Test controlboard
+        std::cerr << "Testing ControlBoard configuration" << std::endl;
+        auto cbDrivers
+            = gzyarp::testing::TestHelpers::getDevicesOfType<yarp::dev::gzyarp::ControlBoardDriver>();
+        EXPECT_EQ(cbDrivers.size(), 1);
+        EXPECT_TRUE(cbDrivers[0] != nullptr);
+    });
+
+    testFixture.Finalize();
+}
diff --git a/tests/commons/ConfigurationParsingFromStringTest.cc b/tests/commons/ConfigurationParsingFromStringTest.cc
@@ -0,0 +1,79 @@
+#include <gtest/gtest.h>
+
+#include <BaseStateDriver.cpp>
+#include <CameraDriver.cpp>
+#include <ControlBoardDriver.hh>
+#include <DeviceRegistry.hh>
+#include <ForceTorqueDriver.cpp>
+#include <ImuDriver.cpp>
+#include <LaserDriver.cpp>
+#include <TestHelpers.hh>
+
+#include <gz/common/Console.hh>
+#include <gz/sim/Entity.hh>
+#include <gz/sim/EntityComponentManager.hh>
+#include <gz/sim/EventManager.hh>
+#include <gz/sim/TestFixture.hh>
+#include <sdf/Element.hh>
+
+#include <iostream>
+#include <memory>
+#include <string>
+
+#include <yarp/dev/IFrameGrabberImage.h>
+#include <yarp/dev/PolyDriver.h>
+#include <yarp/os/Network.h>
+
+TEST(ConfigurationParsingTest, LoadPluginsWithYarpConfigurationString)
+{
+    std::string modelSdfName = "model_configuration_string.sdf";
+    std::string sdfPath = std::string("../../../tests/commons/") + modelSdfName;
+    yarp::os::NetworkBase::setLocalMode(true);
+
+    gz::sim::TestFixture testFixture{sdfPath};
+    gz::common::Console::SetVerbosity(4);
+
+    testFixture.OnConfigure([&](const gz::sim::Entity& _worldEntity,
+                                const std::shared_ptr<const sdf::Element>& /*_sdf*/,
+                                gz::sim::EntityComponentManager& _ecm,
+                                gz::sim::EventManager& /*_eventMgr*/) {
+        // Test Camera
+        std::cerr << "Testing Camera configuration" << std::endl;
+        auto cameraDrivers
+            = gzyarp::testing::TestHelpers::getDevicesOfType<yarp::dev::gzyarp::CameraDriver>();
+        EXPECT_EQ(cameraDrivers.size(), 1);
+        EXPECT_TRUE(cameraDrivers[0] != nullptr);
+
+        // Test ForceTorque
+        std::cerr << "Testing FT configuration" << std::endl;
+        auto ftDrivers
+            = gzyarp::testing::TestHelpers::getDevicesOfType<yarp::dev::gzyarp::ForceTorqueDriver>();
+        EXPECT_EQ(ftDrivers.size(), 1);
+        EXPECT_TRUE(ftDrivers[0] != nullptr);
+
+        // Test Imu
+        std::cerr << "Testing Imu configuration" << std::endl;
+        auto imuDrivers
+            = gzyarp::testing::TestHelpers::getDevicesOfType<yarp::dev::gzyarp::ImuDriver>();
+        EXPECT_EQ(imuDrivers.size(), 1);
+        EXPECT_TRUE(imuDrivers[0] != nullptr);
+
+        // Test Laser
+        std::cerr << "Testing Laser configuration" << std::endl;
+        auto laserDrivers
+            = gzyarp::testing::TestHelpers::getDevicesOfType<yarp::dev::gzyarp::LaserDriver>();
+        EXPECT_EQ(laserDrivers.size(), 1);
+        EXPECT_TRUE(laserDrivers[0] != nullptr);
+
+        // Test basestate
+        std::cerr << "Testing BaseState configuration" << std::endl;
+        auto bsDrivers
+            = gzyarp::testing::TestHelpers::getDevicesOfType<yarp::dev::gzyarp::BaseStateDriver>();
+        EXPECT_EQ(bsDrivers.size(), 1);
+        EXPECT_TRUE(bsDrivers[0] != nullptr);
+
+        // Controlboard test skipped since configuration too complex to pass as string
+    });
+
+    testFixture.Finalize();
+}