From a901e0f938f7a051df3e819bbe4575059346d826 Mon Sep 17 00:00:00 2001 From: Alessandro Croci Date: Wed, 17 Apr 2024 10:15:55 +0200 Subject: [PATCH] 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) --- libraries/device-registry/DeviceRegistry.cc | 5 +- libraries/device-registry/DeviceRegistry.hh | 2 + plugins/basestate/BaseState.cc | 1 + plugins/basestate/BaseStateDriver.cpp | 6 - plugins/camera/Camera.cc | 1 + plugins/camera/CameraDriver.cpp | 19 ++- tests/CMakeLists.txt | 1 + tests/commons/CMakeLists.txt | 44 +++++-- .../ConfigurationParsingFromFileTest.cc | 84 +++++++++++++ .../ConfigurationParsingFromStringTest.cc | 79 ++++++++++++ tests/commons/ConfigurationParsingTest.cc | 115 ------------------ tests/commons/model_configuration_file.sdf | 5 +- tests/controlboard/CMakeLists.txt | 1 + .../ControlBoardPositionDirectControlTest.cc | 37 +++++- tests/imu/CMakeLists.txt | 2 + tests/imu/ImuTest.cc | 23 +++- tests/test-helpers/CMakeLists.txt | 13 ++ tests/test-helpers/TestHelpers.hh | 35 ++++++ 18 files changed, 326 insertions(+), 147 deletions(-) create mode 100644 tests/commons/ConfigurationParsingFromFileTest.cc create mode 100644 tests/commons/ConfigurationParsingFromStringTest.cc delete mode 100644 tests/commons/ConfigurationParsingTest.cc create mode 100644 tests/test-helpers/CMakeLists.txt create mode 100644 tests/test-helpers/TestHelpers.hh diff --git a/libraries/device-registry/DeviceRegistry.cc b/libraries/device-registry/DeviceRegistry.cc index 912dab5..7af6f09 100644 --- a/libraries/device-registry/DeviceRegistry.cc +++ b/libraries/device-registry/DeviceRegistry.cc @@ -8,6 +8,7 @@ #include #include #include +#include #include #include @@ -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 lock(mutex()); - std::cerr << "==================== ecm address: " << EcmPtrSs.str() << std::endl; std::vector keys; for (auto&& [key, value] : m_devicesMap) { diff --git a/libraries/device-registry/DeviceRegistry.hh b/libraries/device-registry/DeviceRegistry.hh index 9bbc352..b931dc5 100644 --- a/libraries/device-registry/DeviceRegistry.hh +++ b/libraries/device-registry/DeviceRegistry.hh @@ -1,3 +1,5 @@ +#pragma once + #include #include #include diff --git a/plugins/basestate/BaseState.cc b/plugins/basestate/BaseState.cc index 06bb97f..9e9d90e 100644 --- a/plugins/basestate/BaseState.cc +++ b/plugins/basestate/BaseState.cc @@ -2,6 +2,7 @@ #include #include #include +#include #include #include diff --git a/plugins/basestate/BaseStateDriver.cpp b/plugins/basestate/BaseStateDriver.cpp index 90ecb12..c1a0e4b 100644 --- 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 index ade66c8..c213ac8 100644 --- a/plugins/camera/Camera.cc +++ b/plugins/camera/Camera.cc @@ -2,6 +2,7 @@ #include #include #include +#include #include #include diff --git a/plugins/camera/CameraDriver.cpp b/plugins/camera/CameraDriver.cpp index c5fa184..307024a 100644 --- a/plugins/camera/CameraDriver.cpp +++ b/plugins/camera/CameraDriver.cpp @@ -3,6 +3,7 @@ #include #include +#include #include #include @@ -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 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 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 index 28234a7..de327b2 100644 --- 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 index b0be7b9..74772d1 100644 --- 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=$") list(APPEND _env_vars "LIBGL_ALWAYS_SOFTWARE=1" "GZ_SIM_SYSTEM_PLUGIN_PATH=$" - "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 new file mode 100644 index 0000000..b490317 --- /dev/null +++ b/tests/commons/ConfigurationParsingFromFileTest.cc @@ -0,0 +1,84 @@ +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +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& /*_sdf*/, + gz::sim::EntityComponentManager& _ecm, + gz::sim::EventManager& /*_eventMgr*/) { + // Test Camera + // std::cerr << "Testing Camera configuration" << std::endl; + // auto cameraDrivers + // = gzyarp::testing::TestHelpers::getDevicesOfType(); + // 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(); + 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(); + 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(); + 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(); + 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(); + EXPECT_EQ(cbDrivers.size(), 1); + EXPECT_TRUE(cbDrivers[0] != nullptr); + }); + + testFixture.Finalize(); +} diff --git a/tests/commons/ConfigurationParsingFromStringTest.cc b/tests/commons/ConfigurationParsingFromStringTest.cc new file mode 100644 index 0000000..c5894de --- /dev/null +++ b/tests/commons/ConfigurationParsingFromStringTest.cc @@ -0,0 +1,79 @@ +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +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& /*_sdf*/, + gz::sim::EntityComponentManager& _ecm, + gz::sim::EventManager& /*_eventMgr*/) { + // Test Camera + std::cerr << "Testing Camera configuration" << std::endl; + auto cameraDrivers + = gzyarp::testing::TestHelpers::getDevicesOfType(); + 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(); + 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(); + 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(); + 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(); + EXPECT_EQ(bsDrivers.size(), 1); + EXPECT_TRUE(bsDrivers[0] != nullptr); + + // Controlboard test skipped since configuration too complex to pass as string + }); + + testFixture.Finalize(); +} diff --git a/tests/commons/ConfigurationParsingTest.cc b/tests/commons/ConfigurationParsingTest.cc deleted file mode 100644 index 1a8ce86..0000000 --- a/tests/commons/ConfigurationParsingTest.cc +++ /dev/null @@ -1,115 +0,0 @@ -#include - -#include - -#include - -#include -#include -#include -#include - -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.Finalize(); - - // Test Camera - std::cerr << "Testing Camera configuration" << std::endl; - auto cameraDeviceName = "model/model_with_plugins/link/link_1/sensor/camera_sensor/" - "camera_plugin_device"; // sensorScopedName / yarpDeviceName - auto driver = gzyarp::DeviceRegistry::getHandler()->getDevice(cameraDeviceName); - EXPECT_TRUE(driver != nullptr); - - // Test ForceTorque - std::cerr << "Testing FT configuration" << std::endl; - auto ftDeviceName = "model/model_with_plugins/joint/joint_12/sensor/" - "force_torque_sensor/" - "forcetorque_plugin_device"; - driver = gzyarp::DeviceRegistry::getHandler()->getDevice(ftDeviceName); - EXPECT_TRUE(driver != nullptr); - - // Test Imu - std::cerr << "Testing Imu configuration" << std::endl; - auto imuDeviceName = "model/model_with_plugins/link/link_1/sensor/imu_sensor/" - "imu_plugin_device"; - driver = gzyarp::DeviceRegistry::getHandler()->getDevice(imuDeviceName); - EXPECT_TRUE(driver != nullptr); - - // Test Laser - std::cerr << "Testing Laser configuration" << std::endl; - auto laserDeviceName = "model/model_with_plugins/link/link_1/sensor/laser_sensor/" - "laser_plugin_device"; - driver = gzyarp::DeviceRegistry::getHandler()->getDevice(laserDeviceName); - EXPECT_TRUE(driver != nullptr); - - // Test basestate - std::cerr << "Testing BaseState configuration" << std::endl; - auto baseStateDeviceName = "model/model_with_plugins/link/link_1/" - "basestate_plugin_device"; - driver = gzyarp::DeviceRegistry::getHandler()->getDevice(baseStateDeviceName); - EXPECT_TRUE(driver != nullptr); - - // Controlboard test skipped since configuration too complex to pass as string -} - -TEST(ConfigurationParsingTest, LoadPluginsWithYarpConfigurationFile) -{ - std::string modelSdfName = "model_configuration_file.sdf"; - std::string sdfPath = std::string("../../../tests/commons/") + modelSdfName; - yarp::os::NetworkBase::setLocalMode(true); - - gz::sim::TestFixture testFixture{sdfPath}; - gz::common::Console::SetVerbosity(4); - yarp::dev::PolyDriver* driver = nullptr; - - testFixture.Finalize(); - - // Test Camera - std::cerr << "Testing Camera configuration" << std::endl; - auto cameraDeviceName = "model/model_with_plugins/link/link_1/sensor/camera_sensor/" - "camera_plugin_device"; // sensorScopedName / yarpDeviceName - driver = gzyarp::DeviceRegistry::getHandler()->getDevice(cameraDeviceName); - EXPECT_TRUE(driver != nullptr); - - // Test ForceTorque - std::cerr << "Testing FT configuration" << std::endl; - auto ftDeviceName = "model/model_with_plugins/joint/joint_12/sensor/" - "force_torque_sensor/" - "forcetorque_plugin_device"; - driver = gzyarp::DeviceRegistry::getHandler()->getDevice(ftDeviceName); - EXPECT_TRUE(driver != nullptr); - - // Test Imu - std::cerr << "Testing Imu configuration" << std::endl; - auto imuDeviceName = "model/model_with_plugins/link/link_1/sensor/imu_sensor/" - "imu_plugin_device"; - driver = gzyarp::DeviceRegistry::getHandler()->getDevice(imuDeviceName); - EXPECT_TRUE(driver != nullptr); - - // Test Laser - std::cerr << "Testing Laser configuration" << std::endl; - auto laserDeviceName = "model/model_with_plugins/link/link_1/sensor/laser_sensor/" - "laser_plugin_device"; - driver = gzyarp::DeviceRegistry::getHandler()->getDevice(laserDeviceName); - EXPECT_TRUE(driver != nullptr); - - // Test basestate - std::cerr << "Testing BaseState configuration" << std::endl; - auto baseStateDeviceName = "model/model_with_plugins/link/link_1/" - "basestate_plugin_device"; - driver = gzyarp::DeviceRegistry::getHandler()->getDevice(baseStateDeviceName); - EXPECT_TRUE(driver != nullptr); - - // Test controlboard - std::cerr << "Testing ControlBoard configuration" << std::endl; - auto controlboardDeviceName = "model/model_with_plugins/controlboard_plugin_device"; - driver = gzyarp::DeviceRegistry::getHandler()->getDevice(controlboardDeviceName); - EXPECT_TRUE(driver != nullptr); -} diff --git a/tests/commons/model_configuration_file.sdf b/tests/commons/model_configuration_file.sdf index 9f943db..8c2103d 100644 --- a/tests/commons/model_configuration_file.sdf +++ b/tests/commons/model_configuration_file.sdf @@ -103,9 +103,9 @@ - + @@ -177,7 +177,6 @@ true true 30 - diff --git a/tests/controlboard/CMakeLists.txt b/tests/controlboard/CMakeLists.txt index 1e17830..b78f38f 100644 --- a/tests/controlboard/CMakeLists.txt +++ b/tests/controlboard/CMakeLists.txt @@ -11,6 +11,7 @@ foreach(TEST ${TESTS}) target_link_libraries(${TEST} PRIVATE GTest::gtest_main + test-helpers PRIVATE gz-plugin${GZ_PLUGIN_VER}::gz-plugin${GZ_PLUGIN_VER} PRIVATE gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER} PRIVATE diff --git a/tests/controlboard/ControlBoardPositionDirectControlTest.cc b/tests/controlboard/ControlBoardPositionDirectControlTest.cc index 34c6b99..c21003b 100644 --- a/tests/controlboard/ControlBoardPositionDirectControlTest.cc +++ b/tests/controlboard/ControlBoardPositionDirectControlTest.cc @@ -1,12 +1,25 @@ #include +#include #include +#include #include #include #include #include +#include +#include #include +#include + +#include +#include +#include +#include +#include +#include +#include #include #include @@ -30,7 +43,7 @@ namespace gzyarp namespace test { -class ControlBoardPositionDirectFixture : public testing::Test +class ControlBoardPositionDirectFixture : public ::testing::Test { protected: // void SetUp() override @@ -55,7 +68,15 @@ class ControlBoardPositionDirectFixture : public testing::Test EXPECT_NE(gz::sim::kNullEntity, modelEntity); model = gz::sim::Model(modelEntity); - driver = gzyarp::DeviceRegistry::getHandler()->getDevice(deviceScopedName); + std::vector cbDrivers + = gzyarp::testing::TestHelpers::getDevicesOfType< + yarp::dev::gzyarp::ControlBoardDriver>(); + ASSERT_EQ(cbDrivers.size(), 1); + auto devicesKeys = DeviceRegistry::getHandler()->getDevicesKeys(); + std::cerr << "Number of Devices: " << devicesKeys.size() << std::endl; + auto cbKey = devicesKeys.at(0); + driver = DeviceRegistry::getHandler()->getDevice(cbKey); + std::cerr << "Driver key: " << cbKey << std::endl; ASSERT_TRUE(driver != nullptr); iPositionDirectControl = nullptr; ASSERT_TRUE(driver->view(iPositionDirectControl)); @@ -84,7 +105,6 @@ class ControlBoardPositionDirectFixture : public testing::Test // Get SDF model name from test parameter gz::sim::TestFixture testFixture; - std::string deviceScopedName = "model/single_pendulum/controlboard_plugin_device"; double linkMass{1}; double linkLength{1.0}; double linkInertiaAtLinkEnd{0.3352}; // Computed with parallel axis theorem @@ -142,18 +162,25 @@ TEST_F(ControlBoardPositionDirectFixture, CheckPositionTrackingUsingPendulumMode Finalize(); int modeSet{}; - iControlMode->getControlMode(0, &modeSet); + + int nJoints{}; + iPositionDirectControl->getAxes(&nJoints); + std::cerr << "Number of joints: " << nJoints << std::endl; + + ASSERT_TRUE(iControlMode->getControlMode(0, &modeSet)); + std::cerr << "Control mode set: " << modeSet << std::endl; ASSERT_TRUE(modeSet == VOCAB_CM_POSITION_DIRECT); // Setup simulation server, this will call the post-update callbacks. // It also calls pre-update and update callbacks if those are being used. testFixture.Server()->Run(true, plannedIterations, false); - + std::cerr << "Simulation completed" << std::endl; // Final assertions EXPECT_TRUE(configured); // Verify that the post update function was called 1000 times EXPECT_EQ(plannedIterations, iterations); // Verify that the average tracking error is within the accepted tolerance + std::cerr << "Tracking error vector size: " << trackingErrors.size() << std::endl; auto avgTrackgingError = std::accumulate(trackingErrors.begin(), trackingErrors.end(), 0.0) / trackingErrors.size(); std::cerr << "Average tracking error: " << avgTrackgingError << std::endl; diff --git a/tests/imu/CMakeLists.txt b/tests/imu/CMakeLists.txt index 8a0d7d9..183a68c 100644 --- a/tests/imu/CMakeLists.txt +++ b/tests/imu/CMakeLists.txt @@ -1,6 +1,7 @@ add_executable(ImuTest ImuTest.cc) target_link_libraries(ImuTest PRIVATE GTest::gtest_main + test-helpers PRIVATE gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER} PRIVATE gz-plugin${GZ_PLUGIN_VER}::gz-plugin${GZ_PLUGIN_VER} PRIVATE @@ -8,6 +9,7 @@ target_link_libraries(ImuTest YARP::YARP_os YARP::YARP_init gz-sim-yarp-device-registry + gz-sim-yarp-imu-system ) add_test(NAME ImuTest COMMAND ImuTest) diff --git a/tests/imu/ImuTest.cc b/tests/imu/ImuTest.cc index 5d93bdf..4dce931 100644 --- a/tests/imu/ImuTest.cc +++ b/tests/imu/ImuTest.cc @@ -1,10 +1,26 @@ #include +#include +#include #include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include #include +#include + #include #include #include +#include class ImuFixture : public testing::Test { @@ -18,7 +34,10 @@ class ImuFixture : public testing::Test const std::shared_ptr& /*_sdf*/, gz::sim::EntityComponentManager& _ecm, gz::sim::EventManager& /*_eventMgr*/) { - driver = gzyarp::DeviceRegistry::getHandler()->getDevice(deviceScopedName); + auto imuDrivers + = gzyarp::testing::TestHelpers::getDevicesOfType(); + ASSERT_TRUE(imuDrivers.size() == 1); + driver = imuDrivers[0]; ASSERT_TRUE(driver != nullptr); ASSERT_TRUE(driver->view(igyroscope)); ASSERT_TRUE(driver->view(iorientation)); @@ -31,7 +50,7 @@ class ImuFixture : public testing::Test gz::sim::TestFixture testFixture; std::string deviceScopedName = "model/sensor_box/link/link_1/sensor/imu_sensor/" "imu_plugin_device"; - yarp::dev::PolyDriver* driver; + yarp::dev::gzyarp::ImuDriver* driver; yarp::dev::IThreeAxisGyroscopes* igyroscope = nullptr; yarp::dev::IOrientationSensors* iorientation = nullptr; yarp::dev::IThreeAxisLinearAccelerometers* iaccelerometer = nullptr; diff --git a/tests/test-helpers/CMakeLists.txt b/tests/test-helpers/CMakeLists.txt new file mode 100644 index 0000000..d1d36d0 --- /dev/null +++ b/tests/test-helpers/CMakeLists.txt @@ -0,0 +1,13 @@ +add_library(test-helpers INTERFACE TestHelpers.hh) + +target_include_directories(test-helpers INTERFACE "$") + +target_link_libraries(test-helpers + INTERFACE GTest::gtest_main + 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 +) diff --git a/tests/test-helpers/TestHelpers.hh b/tests/test-helpers/TestHelpers.hh new file mode 100644 index 0000000..3295c89 --- /dev/null +++ b/tests/test-helpers/TestHelpers.hh @@ -0,0 +1,35 @@ +#pragma once + +#include +#include + +#include + +namespace gzyarp::testing +{ + +class TestHelpers +{ +public: + template std::vector static getDevicesOfType() + { + auto deviceIds = DeviceRegistry::getHandler()->getDevicesKeys(); + std::vector devices{}; + // Get the controlboard devices + for (auto& deviceId : deviceIds) + { + auto polyDriver = DeviceRegistry::getHandler()->getDevice(deviceId); + T* driver = nullptr; + auto viewOk = polyDriver->view(driver); + if (viewOk && driver) + { + devices.push_back(driver); + } + } + std::cerr << "Total devices registered: " << deviceIds.size() + << ", of which of selected type: " << devices.size() << std::endl; + return devices; + } +}; + +} // namespace gzyarp::testing