From 331251799a62eec9329c17f3a6d2669b65383d57 Mon Sep 17 00:00:00 2001 From: Alessandro Croci Date: Fri, 12 Apr 2024 16:47:30 +0200 Subject: [PATCH 01/25] Add unit test launching multiple gazebo instances of the same world concurrently In this first version no gz-sim-yarp-plugin is used --- tests/controlboard/CMakeLists.txt | 1 + .../ControlBoardOnMultipleGazeboInstances.cc | 72 +++++++ .../coupled_pendulum_no_plugins.sdf | 181 ++++++++++++++++++ 3 files changed, 254 insertions(+) create mode 100644 tests/controlboard/ControlBoardOnMultipleGazeboInstances.cc create mode 100644 tests/controlboard/coupled_pendulum_no_plugins.sdf diff --git a/tests/controlboard/CMakeLists.txt b/tests/controlboard/CMakeLists.txt index dff7777..60e0c0c 100644 --- a/tests/controlboard/CMakeLists.txt +++ b/tests/controlboard/CMakeLists.txt @@ -3,6 +3,7 @@ set(TESTS ControlBoardPositionDirectControlTest ControlBoardPositionControlTest ControlBoardCommonsTest + ControlBoardOnMultipleGazeboInstances ) foreach(TEST ${TESTS}) diff --git a/tests/controlboard/ControlBoardOnMultipleGazeboInstances.cc b/tests/controlboard/ControlBoardOnMultipleGazeboInstances.cc new file mode 100644 index 0000000..3c1cd9d --- /dev/null +++ b/tests/controlboard/ControlBoardOnMultipleGazeboInstances.cc @@ -0,0 +1,72 @@ +#include +#include + +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace gzyarp +{ +namespace test +{ + +TEST(ControlBoardOnMultipleGazeboInstances, StartConcurrentGazeboInstances) +{ + auto plannedIterations = 100; + gz::sim::TestFixture fixture1("../../../tests/controlboard/coupled_pendulum_no_plugins.sdf"); + gz::sim::TestFixture fixture2("../../../tests/controlboard/coupled_pendulum_no_plugins.sdf"); + gz::common::Console::SetVerbosity(4); + + fixture1 + .OnPreUpdate( + [&](const gz::sim::UpdateInfo& _info, gz::sim::EntityComponentManager& _ecm) {}) + .OnPostUpdate( + [&](const gz::sim::UpdateInfo& _info, const gz::sim::EntityComponentManager& _ecm) { + std::cerr << _info.iterations << std::endl; + }) + .Finalize(); + + fixture2 + .OnPreUpdate( + [&](const gz::sim::UpdateInfo& _info, gz::sim::EntityComponentManager& _ecm) {}) + .OnPostUpdate( + [&](const gz::sim::UpdateInfo& _info, const gz::sim::EntityComponentManager& _ecm) { + std::cerr << _info.iterations << std::endl; + }) + .Finalize(); + + ASSERT_TRUE(fixture1.Server()->Run(false, plannedIterations, false)); + ASSERT_TRUE(fixture2.Server()->Run(false, plannedIterations, false)); + + while (fixture1.Server()->Running() || fixture2.Server()->Running()) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + + ASSERT_EQ(fixture1.Server()->IterationCount(), plannedIterations); + ASSERT_EQ(fixture2.Server()->IterationCount(), plannedIterations); +} + +} // namespace test +} // namespace gzyarp diff --git a/tests/controlboard/coupled_pendulum_no_plugins.sdf b/tests/controlboard/coupled_pendulum_no_plugins.sdf new file mode 100644 index 0000000..48f2d1f --- /dev/null +++ b/tests/controlboard/coupled_pendulum_no_plugins.sdf @@ -0,0 +1,181 @@ + + + + + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + + + 0 0 0 0 0 0 + 1000 + + 1 + 0 + 0 + 1 + 0 + 1 + + + + 0 0 1 0 0 0 + + + 0.14999999999999999 0.14999999999999999 2.1499999999999999 + + + + + 0 0 1 0 0 0 + + + 0.14999999999999999 0.14999999999999999 2.1499999999999999 + + + + + + 0.14999999999999999 0 1.75 3.1415000000000002 0 0 + base_link + link2 + + 1 0 0 + + -5 + 5 + 100 + 100 + + + 0 + 0 + + + + + 0 0 0 0 0 0 + + 0 0 -0.90000000000000002 -0.30000000000000004 0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + + 0 0 -0.5 0 0 0 + + + 0.14999999999999999 0.14999999999999999 1 + + + + + + -0.14999999999999999 0 1.75 3.1415000000000002 0 0 + base_link + link1 + + 1 0 0 + + -5 + 5 + 100 + 100 + + + 0 + 0 + + + + + 0 0 0 0 0 0 + + 0 0 -0.90000000000000002 0 0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + + 0 0 -0.5 0 0 0 + + + 0.14999999999999999 0.14999999999999999 1 + + + + + + + + + + 0 0 0 0 0 0 + + + 0 0 0 0 0 0 + + + + + From c0914d287353edfb59c5b5a1002803f39c04192f Mon Sep 17 00:00:00 2001 From: Alessandro Croci Date: Fri, 12 Apr 2024 16:47:30 +0200 Subject: [PATCH 02/25] Fix logs in DeviceRegistry.cc --- libraries/device-registry/DeviceRegistry.cc | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/libraries/device-registry/DeviceRegistry.cc b/libraries/device-registry/DeviceRegistry.cc index 50f1fba..9de12b5 100644 --- a/libraries/device-registry/DeviceRegistry.cc +++ b/libraries/device-registry/DeviceRegistry.cc @@ -21,7 +21,7 @@ DeviceRegistry* DeviceRegistry::getHandler() { s_handle = new DeviceRegistry(); if (!s_handle) - yError() << "Error while calling gzyarp::Handler constructor"; + yError() << "Error while calling gzyarp::DeviceRegistry constructor"; } return s_handle; @@ -67,7 +67,8 @@ bool DeviceRegistry::getDevicesAsPolyDriverList( } else { // If a name collision is found, print a clear error and return - yError() << "gzyarp::Handler robotinterface getDevicesAsPolyDriverList error: "; + yError() << "gzyarp::DeviceRegistry robotinterface getDevicesAsPolyDriverList " + "error: "; yError() << "two YARP devices with yarpDeviceName " << yarpDeviceName << " found in model " << modelScopedName; yError() << "First instance: " << got->second; @@ -92,7 +93,8 @@ bool DeviceRegistry::setDevice(std::string deviceDatabaseKey, yarp::dev::PolyDri ret = true; else { - yError() << " Error in gzyarp::Handler while inserting a new yarp device pointer!"; + yError() << " Error in gzyarp::DeviceRegistry while inserting a new yarp device " + "pointer!"; yError() << " The name of the device is already present but the pointer does not match " "with the one already registered!"; yError() << " This should not happen, check the names are correct in your config file. " @@ -106,7 +108,7 @@ bool DeviceRegistry::setDevice(std::string deviceDatabaseKey, yarp::dev::PolyDri std::pair(deviceDatabaseKey, device2add)) .second) { - yError() << " Error in gzyarp::Handler while inserting a new device pointer!"; + yError() << " Error in gzyarp::DeviceRegistry while inserting a new device pointer!"; ret = false; } else ret = true; From f3e524b56464cefb9e4531fdfcff80e9170832a4 Mon Sep 17 00:00:00 2001 From: Alessandro Croci Date: Fri, 12 Apr 2024 16:47:30 +0200 Subject: [PATCH 03/25] Add getter method to DeviceRegistry to obtain a vector of device keys --- libraries/device-registry/DeviceRegistry.cc | 10 ++++++++++ libraries/device-registry/DeviceRegistry.hh | 2 ++ 2 files changed, 12 insertions(+) diff --git a/libraries/device-registry/DeviceRegistry.cc b/libraries/device-registry/DeviceRegistry.cc index 9de12b5..42af8b7 100644 --- a/libraries/device-registry/DeviceRegistry.cc +++ b/libraries/device-registry/DeviceRegistry.cc @@ -129,6 +129,16 @@ yarp::dev::PolyDriver* DeviceRegistry::getDevice(const std::string& deviceDataba return tmp; } +std::vector DeviceRegistry::getDevicesKeys() const +{ + std::vector keys; + for (auto&& [key, value] : m_devicesMap) + keys.push_back(key); + return keys; +} + +// Private methods + void DeviceRegistry::removeDevice(const std::string& deviceDatabaseKey) { DevicesMap::iterator device = m_devicesMap.find(deviceDatabaseKey); diff --git a/libraries/device-registry/DeviceRegistry.hh b/libraries/device-registry/DeviceRegistry.hh index b4666b2..b4edd78 100644 --- a/libraries/device-registry/DeviceRegistry.hh +++ b/libraries/device-registry/DeviceRegistry.hh @@ -25,6 +25,8 @@ public: void removeDevice(const std::string& deviceDatabaseKey); + std::vector getDevicesKeys() const; + private: DeviceRegistry(); static DeviceRegistry* s_handle; From eeed3e79a5d96a09e920d3a186477a5142f7891d Mon Sep 17 00:00:00 2001 From: Alessandro Croci Date: Fri, 12 Apr 2024 16:47:30 +0200 Subject: [PATCH 04/25] Update ControlBoardOnMultipleGazeboInstances unit test - introduced controlboard plugin - check that DeviceRegistry has the correct number of device keys --> FAILING --- .../ControlBoardOnMultipleGazeboInstances.cc | 43 ++++++++++++++++--- .../gazebo_controlboard_multiple_joints.ini | 4 +- ...oupled_pendulum_multiple_gz_instances.sdf} | 4 +- 3 files changed, 42 insertions(+), 9 deletions(-) rename tests/controlboard/{coupled_pendulum_no_plugins.sdf => coupled_pendulum_multiple_gz_instances.sdf} (97%) diff --git a/tests/controlboard/ControlBoardOnMultipleGazeboInstances.cc b/tests/controlboard/ControlBoardOnMultipleGazeboInstances.cc index 3c1cd9d..733010f 100644 --- a/tests/controlboard/ControlBoardOnMultipleGazeboInstances.cc +++ b/tests/controlboard/ControlBoardOnMultipleGazeboInstances.cc @@ -1,5 +1,5 @@ #include -#include +#include #include @@ -33,26 +33,56 @@ namespace test TEST(ControlBoardOnMultipleGazeboInstances, StartConcurrentGazeboInstances) { + std::string deviceScopedName = "model/coupled_pendulum/controlboard_plugin_device"; auto plannedIterations = 100; - gz::sim::TestFixture fixture1("../../../tests/controlboard/coupled_pendulum_no_plugins.sdf"); - gz::sim::TestFixture fixture2("../../../tests/controlboard/coupled_pendulum_no_plugins.sdf"); + yarp::dev::PolyDriver* driver1; + yarp::dev::PolyDriver* driver2; + yarp::dev::IEncoders* iEncoders1 = nullptr; + yarp::dev::IEncoders* iEncoders2 = nullptr; + double jointPosition1{}, jointPosition2{}; + gz::sim::TestFixture fixture1("../../../tests/controlboard/" + "coupled_pendulum_multiple_gz_instances.sdf"); + gz::sim::TestFixture fixture2("../../../tests/controlboard/" + "coupled_pendulum_multiple_gz_instances.sdf"); gz::common::Console::SetVerbosity(4); fixture1 + .OnConfigure([&](const gz::sim::Entity& _worldEntity, + const std::shared_ptr& /*_sdf*/, + gz::sim::EntityComponentManager& _ecm, + gz::sim::EventManager& /*_eventMgr*/) { + driver1 = gzyarp::DeviceRegistry::getHandler()->getDevice(deviceScopedName); + ASSERT_TRUE(driver1 != nullptr); + iEncoders1 = nullptr; + ASSERT_TRUE(driver1->view(iEncoders1)); + }) .OnPreUpdate( [&](const gz::sim::UpdateInfo& _info, gz::sim::EntityComponentManager& _ecm) {}) .OnPostUpdate( [&](const gz::sim::UpdateInfo& _info, const gz::sim::EntityComponentManager& _ecm) { - std::cerr << _info.iterations << std::endl; + std::cerr << "Iteration: " << _info.iterations << std::endl; + iEncoders1->getEncoder(0, &jointPosition1); + std::cerr << "Joint position 1: " << jointPosition1 << std::endl; }) .Finalize(); fixture2 + .OnConfigure([&](const gz::sim::Entity& _worldEntity, + const std::shared_ptr& /*_sdf*/, + gz::sim::EntityComponentManager& _ecm, + gz::sim::EventManager& /*_eventMgr*/) { + driver2 = gzyarp::DeviceRegistry::getHandler()->getDevice(deviceScopedName); + ASSERT_TRUE(driver2 != nullptr); + iEncoders2 = nullptr; + ASSERT_TRUE(driver2->view(iEncoders2)); + }) .OnPreUpdate( [&](const gz::sim::UpdateInfo& _info, gz::sim::EntityComponentManager& _ecm) {}) .OnPostUpdate( [&](const gz::sim::UpdateInfo& _info, const gz::sim::EntityComponentManager& _ecm) { - std::cerr << _info.iterations << std::endl; + std::cerr << "Iteration: " << _info.iterations << std::endl; + iEncoders2->getEncoder(0, &jointPosition2); + std::cerr << "Joint position 2: " << jointPosition2 << std::endl; }) .Finalize(); @@ -66,6 +96,9 @@ TEST(ControlBoardOnMultipleGazeboInstances, StartConcurrentGazeboInstances) ASSERT_EQ(fixture1.Server()->IterationCount(), plannedIterations); ASSERT_EQ(fixture2.Server()->IterationCount(), plannedIterations); + + // Check that DeviceRegistry has two devices, one for each Gazebo instance + ASSERT_EQ(gzyarp::DeviceRegistry::getHandler()->getDevicesKeys().size(), 2); } } // namespace test diff --git a/tests/controlboard/conf/gazebo_controlboard_multiple_joints.ini b/tests/controlboard/conf/gazebo_controlboard_multiple_joints.ini index 99a888f..63878b4 100644 --- a/tests/controlboard/conf/gazebo_controlboard_multiple_joints.ini +++ b/tests/controlboard/conf/gazebo_controlboard_multiple_joints.ini @@ -31,5 +31,5 @@ stictionUp (0.0) stictionDwn (0.0) [LIMITS] -jntPosMax 200.0 10.0 -jntPosMin -200.0 -10.0 +jntPosMax (200.0 10.0) +jntPosMin (-200.0 -10.0) diff --git a/tests/controlboard/coupled_pendulum_no_plugins.sdf b/tests/controlboard/coupled_pendulum_multiple_gz_instances.sdf similarity index 97% rename from tests/controlboard/coupled_pendulum_no_plugins.sdf rename to tests/controlboard/coupled_pendulum_multiple_gz_instances.sdf index 48f2d1f..fa155d4 100644 --- a/tests/controlboard/coupled_pendulum_no_plugins.sdf +++ b/tests/controlboard/coupled_pendulum_multiple_gz_instances.sdf @@ -159,12 +159,12 @@ - + From 0fa62682c5d81a5b2e1a142dea3364a2285fcf18 Mon Sep 17 00:00:00 2001 From: Alessandro Croci Date: Fri, 12 Apr 2024 16:47:30 +0200 Subject: [PATCH 05/25] Add DeviceIdGenerator class --- libraries/common/CMakeLists.txt | 2 +- libraries/common/DeviceIdGenerator.cpp | 20 ++++++++++++++++++++ libraries/common/DeviceIdGenerator.hh | 19 +++++++++++++++++++ 3 files changed, 40 insertions(+), 1 deletion(-) create mode 100644 libraries/common/DeviceIdGenerator.cpp create mode 100644 libraries/common/DeviceIdGenerator.hh diff --git a/libraries/common/CMakeLists.txt b/libraries/common/CMakeLists.txt index cdc0c0f..cd4f6e6 100644 --- a/libraries/common/CMakeLists.txt +++ b/libraries/common/CMakeLists.txt @@ -1,4 +1,4 @@ -add_library(gz-sim-yarp-commons SHARED Common.hh ConfigurationHelpers.hh ConfigurationHelpers.cpp) +add_library(gz-sim-yarp-commons SHARED Common.hh ConfigurationHelpers.hh ConfigurationHelpers.cpp DeviceIdGenerator.hh DeviceIdGenerator.cpp) target_include_directories(gz-sim-yarp-commons PUBLIC "$") diff --git a/libraries/common/DeviceIdGenerator.cpp b/libraries/common/DeviceIdGenerator.cpp new file mode 100644 index 0000000..bb1a3a0 --- /dev/null +++ b/libraries/common/DeviceIdGenerator.cpp @@ -0,0 +1,20 @@ +#include + +#include +#include +#include + +namespace gzyarp +{ +using gz::sim::Entity; +using gz::sim::EntityComponentManager; + +std::string DeviceIdGenerator::generateDeviceId(const Entity& entity, + const EntityComponentManager& ecm, + const std::string& yarpDeviceName) +{ + auto scopedName = gz::sim::scopedName(entity, ecm, "/"); + return scopedName + "/" + yarpDeviceName; +} + +} // namespace gzyarp diff --git a/libraries/common/DeviceIdGenerator.hh b/libraries/common/DeviceIdGenerator.hh new file mode 100644 index 0000000..f89e080 --- /dev/null +++ b/libraries/common/DeviceIdGenerator.hh @@ -0,0 +1,19 @@ +#pragma once + +#include + +#include +#include + +namespace gzyarp +{ + +class DeviceIdGenerator +{ +public: + static std::string generateDeviceId(const gz::sim::Entity& entity, + const gz::sim::EntityComponentManager& ecm, + const std::string& yarpDeviceName); +}; + +} // namespace gzyarp From 47010f7d3be95508c2e651a9bc0f2d3c900f913e Mon Sep 17 00:00:00 2001 From: Alessandro Croci Date: Fri, 12 Apr 2024 16:47:30 +0200 Subject: [PATCH 06/25] Update plugins to use DeviceIdGenerator to generate the device id to set on DeviceRegistry Plugins modified: - basestate - camera - controlboard - forcetorque - imu - laser --- plugins/basestate/BaseState.cc | 22 +++++---------- plugins/camera/Camera.cc | 28 +++++++++---------- plugins/controlboard/include/ControlBoard.hh | 2 +- plugins/controlboard/src/ControlBoard.cpp | 20 +++++++------- plugins/forcetorque/ForceTorque.cc | 29 ++++++++++---------- plugins/imu/Imu.cc | 28 +++++++++---------- plugins/laser/Laser.cc | 28 +++++++++---------- 7 files changed, 74 insertions(+), 83 deletions(-) diff --git a/plugins/basestate/BaseState.cc b/plugins/basestate/BaseState.cc index c8d309c..8a33d67 100644 --- a/plugins/basestate/BaseState.cc +++ b/plugins/basestate/BaseState.cc @@ -1,5 +1,6 @@ #include #include +#include #include #include @@ -35,7 +36,7 @@ class BaseState : public System, public ISystemConfigure, public ISystemPostUpda { if (m_deviceRegistered) { - DeviceRegistry::getHandler()->removeDevice(m_deviceScopedName); + DeviceRegistry::getHandler()->removeDevice(m_deviceId); m_deviceRegistered = false; } @@ -111,14 +112,6 @@ class BaseState : public System, public ISystemConfigure, public ISystemPostUpda driver_properties.put(YarpBaseStateScopedName.c_str(), m_baseLinkScopedName.c_str()); - if (!driver_properties.check("yarpDeviceName")) - { - yError() << "gz-sim-yarp-basestate-system : missing yarpDeviceName parameter for " - "device " - << m_baseLinkScopedName; - return; - } - // Insert the pointer in the singleton handler for retrieving it in the yarp driver BaseStateDataSingleton::getBaseStateDataHandler()->setBaseStateData(&(m_baseStateData)); @@ -132,17 +125,16 @@ class BaseState : public System, public ISystemConfigure, public ISystemPostUpda return; } - m_deviceScopedName - = m_baseLinkScopedName + "/" + driver_properties.find("yarpDeviceName").asString(); + m_deviceId = DeviceIdGenerator::generateDeviceId(m_baseLinkEntity, _ecm, deviceName); - if (!DeviceRegistry::getHandler()->setDevice(m_deviceScopedName, &m_baseStateDriver)) + if (!DeviceRegistry::getHandler()->setDevice(m_deviceId, &m_baseStateDriver)) { yError() << "gz-sim-yarp-basestate-system: failed setting scopedDeviceName(=" - << m_deviceScopedName << ")"; + << m_deviceId << ")"; return; } m_deviceRegistered = true; - yInfo() << "Registered YARP device with instance name:" << m_deviceScopedName; + yInfo() << "Registered YARP device with instance name:" << m_deviceId; } virtual void PostUpdate(const UpdateInfo& _info, const EntityComponentManager& _ecm) override @@ -235,7 +227,7 @@ class BaseState : public System, public ISystemConfigure, public ISystemPostUpda private: bool m_deviceRegistered; std::string m_baseLinkScopedName; - std::string m_deviceScopedName; + std::string m_deviceId; Entity m_baseLinkEntity; Link m_baseLink; yarp::dev::PolyDriver m_baseStateDriver; diff --git a/plugins/camera/Camera.cc b/plugins/camera/Camera.cc index ab3248d..63d5875 100644 --- a/plugins/camera/Camera.cc +++ b/plugins/camera/Camera.cc @@ -1,5 +1,6 @@ #include #include +#include #include #include @@ -41,7 +42,7 @@ class Camera : public System, { if (m_deviceRegistered) { - DeviceRegistry::getHandler()->removeDevice(m_deviceScopedName); + DeviceRegistry::getHandler()->removeDevice(m_deviceId); m_deviceRegistered = false; } @@ -83,6 +84,11 @@ class Camera : public System, yError() << "gz-sim-yarp-camera-system : missing parentLinkName parameter"; return; } + if (!driver_properties.check("yarpDeviceName")) + { + yError() << "gz-sim-yarp-camera-system : missing yarpDeviceName parameter"; + return; + } yInfo() << "gz-sim-yarp-camera-system: configuration of sensor " << driver_properties.find("sensorName").asString() << " loaded"; } else @@ -110,12 +116,6 @@ class Camera : public System, this->cameraData.sensorScopedName = sensorScopedName; driver_properties.put(YarpCameraScopedName.c_str(), sensorScopedName.c_str()); - if (!driver_properties.check("yarpDeviceName")) - { - yError() << "gz-sim-yarp-camera-system : missing yarpDeviceName parameter for device" - << sensorScopedName; - return; - } // Insert the pointer in the singleton handler for retriving it in the yarp driver CameraDataSingleton::getHandler()->setSensor(&(this->cameraData)); @@ -137,19 +137,19 @@ class Camera : public System, return; } - m_deviceScopedName - = sensorScopedName + "/" + driver_properties.find("yarpDeviceName").asString(); + auto deviceName = driver_properties.find("yarpDeviceName").asString(); + m_deviceId = DeviceIdGenerator::generateDeviceId(sensor, _ecm, deviceName); - if (!DeviceRegistry::getHandler()->setDevice(m_deviceScopedName, &m_cameraDriver)) + if (!DeviceRegistry::getHandler()->setDevice(m_deviceId, &m_cameraDriver)) { - yError() << "gz-sim-yarp-camera-system: failed setting scopedDeviceName(=" - << m_deviceScopedName << ")"; + yError() << "gz-sim-yarp-camera-system: failed setting scopedDeviceName(=" << m_deviceId + << ")"; return; } this->m_deviceRegistered = true; this->cameraInitialized = false; yInfo() << "gz-sim-yarp-camera-system: Registered YARP device with instance name:" - << m_deviceScopedName; + << m_deviceId; } virtual void PreUpdate(const UpdateInfo& _info, EntityComponentManager& _ecm) override @@ -188,7 +188,7 @@ class Camera : public System, private: Entity sensor; yarp::dev::PolyDriver m_cameraDriver; - std::string m_deviceScopedName; + std::string m_deviceId; std::string sensorScopedName; bool m_deviceRegistered; CameraData cameraData; diff --git a/plugins/controlboard/include/ControlBoard.hh b/plugins/controlboard/include/ControlBoard.hh index d3c945b..c451989 100644 --- a/plugins/controlboard/include/ControlBoard.hh +++ b/plugins/controlboard/include/ControlBoard.hh @@ -53,7 +53,7 @@ public: private: bool m_deviceRegistered; std::string m_robotScopedName; - std::string m_deviceScopedName; + std::string m_deviceId; gz::sim::Entity m_modelEntity; yarp::dev::PolyDriver m_controlBoardDriver; ControlBoardData m_controlBoardData; diff --git a/plugins/controlboard/src/ControlBoard.cpp b/plugins/controlboard/src/ControlBoard.cpp index 5b6598b..47989d5 100644 --- a/plugins/controlboard/src/ControlBoard.cpp +++ b/plugins/controlboard/src/ControlBoard.cpp @@ -4,6 +4,7 @@ #include #include #include +#include #include #include @@ -57,7 +58,7 @@ ControlBoard::~ControlBoard() { if (m_deviceRegistered) { - DeviceRegistry::getHandler()->removeDevice(m_deviceScopedName); + DeviceRegistry::getHandler()->removeDevice(m_deviceId); m_deviceRegistered = false; } @@ -102,13 +103,12 @@ void ControlBoard::Configure(const Entity& _entity, m_robotScopedName = gz::sim::scopedName(_entity, _ecm, "/"); yDebug() << "gz-sim-yarp-controlboard-system : robot scoped name: " << m_robotScopedName; - m_deviceScopedName - = m_robotScopedName + "/" + m_pluginParameters.find("yarpDeviceName").asString(); - yDebug() << "gz-sim-yarp-controlboard-system : device scoped name: " << m_deviceScopedName; + m_deviceId = DeviceIdGenerator::generateDeviceId(_entity, _ecm, deviceName); + yDebug() << "gz-sim-yarp-controlboard-system : deviceId: " << m_deviceId; m_modelEntity = _entity; - m_controlBoardData.controlBoardId = m_deviceScopedName; + m_controlBoardData.controlBoardId = m_deviceId; m_pluginParameters.put(yarp::dev::gzyarp::YarpControlBoardScopedName.c_str(), m_robotScopedName.c_str()); @@ -117,7 +117,7 @@ void ControlBoard::Configure(const Entity& _entity, ControlBoardDataSingleton::getControlBoardHandler()->setControlBoardData(&(m_controlBoardData)); m_pluginParameters.put("device", "gazebo_controlboard"); - m_pluginParameters.put("controlBoardId", m_deviceScopedName); + m_pluginParameters.put("controlBoardId", m_deviceId); if (_sdf->HasElement("initialConfiguration")) { @@ -134,10 +134,10 @@ void ControlBoard::Configure(const Entity& _entity, return; } - if (!DeviceRegistry::getHandler()->setDevice(m_deviceScopedName, &m_controlBoardDriver)) + if (!DeviceRegistry::getHandler()->setDevice(m_deviceId, &m_controlBoardDriver)) { - yError() << "gz-sim-yarp-basestate-system: failed setting scopedDeviceName(=" - << m_deviceScopedName << ")"; + yError() << "gz-sim-yarp-basestate-system: failed setting scopedDeviceName(=" << m_deviceId + << ")"; return; } @@ -149,7 +149,7 @@ void ControlBoard::Configure(const Entity& _entity, resetPositionsAndTrajectoryGenerators(_ecm); - yInfo() << "Registered YARP device with instance name:" << m_deviceScopedName; + yInfo() << "Registered YARP device with instance name:" << m_deviceId; m_deviceRegistered = true; } diff --git a/plugins/forcetorque/ForceTorque.cc b/plugins/forcetorque/ForceTorque.cc index 84e2e9b..e3652a4 100644 --- a/plugins/forcetorque/ForceTorque.cc +++ b/plugins/forcetorque/ForceTorque.cc @@ -1,4 +1,5 @@ #include +#include #include #include @@ -38,7 +39,7 @@ class ForceTorque : public System, { if (m_deviceRegistered) { - DeviceRegistry::getHandler()->removeDevice(m_deviceScopedName); + DeviceRegistry::getHandler()->removeDevice(m_deviceId); m_deviceRegistered = false; } @@ -83,6 +84,11 @@ class ForceTorque : public System, yError() << "gz-sim-yarp-forcetorque-system : missing jointName parameter"; return; } + if (!driver_properties.check("yarpDeviceName")) + { + yError() << "gz-sim-yarp-forcetorque-system : missing yarpDeviceName parameter"; + return; + } yInfo() << "gz-sim-yarp-forcetorque-system: configuration of sensor " << driver_properties.find("sensorName").asString() << " loaded"; } else @@ -101,13 +107,6 @@ class ForceTorque : public System, this->forceTorqueData.sensorScopedName = sensorScopedName; driver_properties.put(YarpForceTorqueScopedName.c_str(), sensorScopedName.c_str()); - if (!driver_properties.check("yarpDeviceName")) - { - yError() << "gz-sim-yarp-forcetorque-system : missing yarpDeviceName parameter for " - "device" - << sensorScopedName; - return; - } // Insert the pointer in the singleton handler for retriving it in the yarp driver ForceTorqueDataSingleton::getHandler()->setSensor(&(this->forceTorqueData)); @@ -121,17 +120,17 @@ class ForceTorque : public System, return; } - m_deviceScopedName - = sensorScopedName + "/" + driver_properties.find("yarpDeviceName").asString(); - - if (!DeviceRegistry::getHandler()->setDevice(m_deviceScopedName, &m_forceTorqueDriver)) + auto deviceName = driver_properties.find("yarpDeviceName").asString(); + m_deviceId = DeviceIdGenerator::generateDeviceId(sensor, _ecm, deviceName); + std::cerr << "============== DeviceId: " << m_deviceId << std::endl; + if (!DeviceRegistry::getHandler()->setDevice(m_deviceId, &m_forceTorqueDriver)) { yError() << "gz-sim-yarp-forcetorque-system: failed setting scopedDeviceName(=" - << m_deviceScopedName << ")"; + << m_deviceId << ")"; return; } m_deviceRegistered = true; - yInfo() << "Registered YARP device with instance name:" << m_deviceScopedName; + yInfo() << "Registered YARP device with instance name:" << m_deviceId; } virtual void PreUpdate(const UpdateInfo& _info, EntityComponentManager& _ecm) override @@ -171,7 +170,7 @@ class ForceTorque : public System, private: Entity sensor; yarp::dev::PolyDriver m_forceTorqueDriver; - std::string m_deviceScopedName; + std::string m_deviceId; std::string sensorScopedName; bool m_deviceRegistered; ForceTorqueData forceTorqueData; diff --git a/plugins/imu/Imu.cc b/plugins/imu/Imu.cc index b51521a..860774a 100644 --- a/plugins/imu/Imu.cc +++ b/plugins/imu/Imu.cc @@ -1,4 +1,5 @@ #include +#include #include #include @@ -42,7 +43,7 @@ class Imu : public System, { if (m_deviceRegistered) { - DeviceRegistry::getHandler()->removeDevice(m_deviceScopedName); + DeviceRegistry::getHandler()->removeDevice(m_deviceId); m_deviceRegistered = false; } @@ -77,6 +78,11 @@ class Imu : public System, yError() << "gz-sim-yarp-imu-system : missing parentLinkName parameter"; return; } + if (!driver_properties.check("yarpDeviceName")) + { + yError() << "gz-sim-yarp-imu-system : missing yarpDeviceName parameter"; + return; + } yInfo() << "gz-sim-yarp-imu-system: configuration of sensor " << driver_properties.find("sensorName").asString() << " loaded"; } else @@ -97,12 +103,6 @@ class Imu : public System, this->imuData.sensorScopedName = sensorScopedName; driver_properties.put(YarpIMUScopedName.c_str(), sensorScopedName.c_str()); - if (!driver_properties.check("yarpDeviceName")) - { - yError() << "gz-sim-yarp-imu-system : missing yarpDeviceName parameter for device" - << sensorScopedName; - return; - } // Insert the pointer in the singleton handler for retriving it in the yarp driver ImuDataSingleton::getHandler()->setSensor(&(this->imuData)); @@ -115,17 +115,17 @@ class Imu : public System, return; } - m_deviceScopedName - = sensorScopedName + "/" + driver_properties.find("yarpDeviceName").asString(); + auto deviceName = driver_properties.find("yarpDeviceName").asString(); + m_deviceId = DeviceIdGenerator::generateDeviceId(sensor, _ecm, deviceName); - if (!DeviceRegistry::getHandler()->setDevice(m_deviceScopedName, &m_imuDriver)) + if (!DeviceRegistry::getHandler()->setDevice(m_deviceId, &m_imuDriver)) { - yError() << "gz-sim-yarp-imu-system: failed setting scopedDeviceName(=" - << m_deviceScopedName << ")"; + yError() << "gz-sim-yarp-imu-system: failed setting scopedDeviceName(=" << m_deviceId + << ")"; return; } m_deviceRegistered = true; - yInfo() << "Registered YARP device with instance name:" << m_deviceScopedName; + yInfo() << "Registered YARP device with instance name:" << m_deviceId; } virtual void PreUpdate(const UpdateInfo& _info, EntityComponentManager& _ecm) override { @@ -173,7 +173,7 @@ class Imu : public System, private: Entity sensor; yarp::dev::PolyDriver m_imuDriver; - std::string m_deviceScopedName; + std::string m_deviceId; std::string sensorScopedName; bool m_deviceRegistered; ImuData imuData; diff --git a/plugins/laser/Laser.cc b/plugins/laser/Laser.cc index ea42da2..1741de3 100644 --- a/plugins/laser/Laser.cc +++ b/plugins/laser/Laser.cc @@ -1,4 +1,5 @@ #include +#include #include #include @@ -42,7 +43,7 @@ class Laser : public System, { if (m_deviceRegistered) { - DeviceRegistry::getHandler()->removeDevice(m_deviceScopedName); + DeviceRegistry::getHandler()->removeDevice(m_deviceId); m_deviceRegistered = false; } @@ -82,6 +83,11 @@ class Laser : public System, yError() << "gz-sim-yarp-laser-system : missing parentLinkName parameter"; return; } + if (!driver_properties.check("yarpDeviceName")) + { + yError() << "gz-sim-yarp-laser-system : missing yarpDeviceName parameter"; + return; + } yInfo() << "gz-sim-yarp-laser-system: configuration of sensor " << driver_properties.find("sensorName").asString() << " loaded"; } else @@ -101,12 +107,6 @@ class Laser : public System, this->laserData.sensorScopedName = sensorScopedName; driver_properties.put(YarpLaserScopedName.c_str(), sensorScopedName.c_str()); - if (!driver_properties.check("yarpDeviceName")) - { - yError() << "gz-sim-yarp-laser-system : missing yarpDeviceName parameter for device" - << sensorScopedName; - return; - } // Insert the pointer in the singleton handler for retriving it in the yarp driver LaserDataSingleton::getHandler()->setSensor(&(this->laserData)); @@ -119,17 +119,17 @@ class Laser : public System, return; } - m_deviceScopedName - = sensorScopedName + "/" + driver_properties.find("yarpDeviceName").asString(); + auto deviceName = driver_properties.find("yarpDeviceName").asString(); + m_deviceId = DeviceIdGenerator::generateDeviceId(sensor, _ecm, deviceName); - if (!DeviceRegistry::getHandler()->setDevice(m_deviceScopedName, &m_laserDriver)) + if (!DeviceRegistry::getHandler()->setDevice(m_deviceId, &m_laserDriver)) { - yError() << "gz-sim-yarp-laser-system: failed setting scopedDeviceName(=" - << m_deviceScopedName << ")"; + yError() << "gz-sim-yarp-laser-system: failed setting scopedDeviceName(=" << m_deviceId + << ")"; return; } m_deviceRegistered = true; - yInfo() << "Registered YARP device with instance name:" << m_deviceScopedName; + yInfo() << "Registered YARP device with instance name:" << m_deviceId; } virtual void PreUpdate(const UpdateInfo& _info, EntityComponentManager& _ecm) override { @@ -170,7 +170,7 @@ class Laser : public System, private: Entity sensor; yarp::dev::PolyDriver m_laserDriver; - std::string m_deviceScopedName; + std::string m_deviceId; std::string sensorScopedName; bool m_deviceRegistered; LaserData laserData; From a9e0afae48ba95f0d0235dc0e0abc7d161569097 Mon Sep 17 00:00:00 2001 From: Alessandro Croci Date: Fri, 12 Apr 2024 16:47:30 +0200 Subject: [PATCH 07/25] Update data structure management of forcetorque plugin This is done by: - removing the data singleton - adding a new header file containing the data structure and a custom interface to be implemented by force torque driver --- plugins/forcetorque/CMakeLists.txt | 3 - plugins/forcetorque/ForceTorque.cc | 29 +++++- plugins/forcetorque/ForceTorqueDriver.cpp | 43 +++++---- plugins/forcetorque/ForceTorqueShared.hh | 24 +++++ plugins/forcetorque/singleton/CMakeLists.txt | 17 ---- .../singleton/ForceTorqueDataSingleton.cc | 91 ------------------- .../singleton/ForceTorqueDataSingleton.hh | 40 -------- 7 files changed, 73 insertions(+), 174 deletions(-) create mode 100644 plugins/forcetorque/ForceTorqueShared.hh delete mode 100644 plugins/forcetorque/singleton/CMakeLists.txt delete mode 100644 plugins/forcetorque/singleton/ForceTorqueDataSingleton.cc delete mode 100644 plugins/forcetorque/singleton/ForceTorqueDataSingleton.hh diff --git a/plugins/forcetorque/CMakeLists.txt b/plugins/forcetorque/CMakeLists.txt index 3afdbfc..cc2cd4d 100644 --- a/plugins/forcetorque/CMakeLists.txt +++ b/plugins/forcetorque/CMakeLists.txt @@ -1,5 +1,3 @@ -add_subdirectory(singleton) - add_library(gz-sim-yarp-forcetorque-system SHARED ForceTorque.cc ForceTorqueDriver.cpp) target_include_directories(gz-sim-yarp-forcetorque-system @@ -15,7 +13,6 @@ target_link_libraries(gz-sim-yarp-forcetorque-system YARP::YARP_os YARP::YARP_init gz-sim-yarp-device-registry - gz-sim-yarp-forcetorque-singleton gz-sim-yarp-commons) # Add install target diff --git a/plugins/forcetorque/ForceTorque.cc b/plugins/forcetorque/ForceTorque.cc index e3652a4..d448027 100644 --- a/plugins/forcetorque/ForceTorque.cc +++ b/plugins/forcetorque/ForceTorque.cc @@ -2,20 +2,33 @@ #include #include +#include +#include +#include + +#include #include +#include +#include +#include #include #include #include #include +#include #include #include #include +#include #include +#include #include #include +#include #include #include +#include using namespace gz; using namespace sim; @@ -45,7 +58,6 @@ class ForceTorque : public System, if (m_forceTorqueDriver.isValid()) m_forceTorqueDriver.close(); - ForceTorqueDataSingleton::getHandler()->removeSensor(sensorScopedName); yarp::os::Network::fini(); } @@ -108,9 +120,6 @@ class ForceTorque : public System, driver_properties.put(YarpForceTorqueScopedName.c_str(), sensorScopedName.c_str()); - // Insert the pointer in the singleton handler for retriving it in the yarp driver - ForceTorqueDataSingleton::getHandler()->setSensor(&(this->forceTorqueData)); - driver_properties.put("device", "gazebo_forcetorque"); driver_properties.put("sensor_name", sensorName); if (!m_forceTorqueDriver.open(driver_properties)) @@ -120,9 +129,19 @@ class ForceTorque : public System, return; } + IForceTorqueData* ftData = nullptr; + auto viewOk = m_forceTorqueDriver.view(ftData); + + if (!viewOk || !ftData) + { + yError() << "gz-sim-yarp-forcetorque-system Plugin failed: error in getting " + "IForceTorqueData interface"; + return; + } + ftData->setForceTorqueData(&forceTorqueData); + auto deviceName = driver_properties.find("yarpDeviceName").asString(); m_deviceId = DeviceIdGenerator::generateDeviceId(sensor, _ecm, deviceName); - std::cerr << "============== DeviceId: " << m_deviceId << std::endl; if (!DeviceRegistry::getHandler()->setDevice(m_deviceId, &m_forceTorqueDriver)) { yError() << "gz-sim-yarp-forcetorque-system: failed setting scopedDeviceName(=" diff --git a/plugins/forcetorque/ForceTorqueDriver.cpp b/plugins/forcetorque/ForceTorqueDriver.cpp index 80dbb56..4226269 100644 --- a/plugins/forcetorque/ForceTorqueDriver.cpp +++ b/plugins/forcetorque/ForceTorqueDriver.cpp @@ -1,13 +1,17 @@ #include -#include +#include +#include #include +#include #include #include #include #include #include +#include +#include namespace yarp { @@ -28,7 +32,8 @@ const double fakeTemperatureValue = 25.0; class yarp::dev::gzyarp::ForceTorqueDriver : public yarp::dev::DeviceDriver, public yarp::dev::ISixAxisForceTorqueSensors, - public yarp::dev::ITemperatureSensors + public yarp::dev::ITemperatureSensors, + public ::gzyarp::IForceTorqueData { public: ForceTorqueDriver() @@ -39,7 +44,7 @@ class yarp::dev::gzyarp::ForceTorqueDriver : public yarp::dev::DeviceDriver, } // DEVICE DRIVER - virtual bool open(yarp::os::Searchable& config) + bool open(yarp::os::Searchable& config) override { std::string sensorScopedName( @@ -56,29 +61,22 @@ class yarp::dev::gzyarp::ForceTorqueDriver : public yarp::dev::DeviceDriver, } m_frameName = m_sensorName; - m_sensorData - = ::gzyarp::ForceTorqueDataSingleton::getHandler()->getSensor(sensorScopedName); - - if (!m_sensorData) - { - yError() << "Error, ForceTorque sensor was not found"; - return false; - } return true; } - virtual bool close() + bool close() override { return true; } // SIX AXIS FORCE TORQUE SENSORS - virtual size_t getNrOfSixAxisForceTorqueSensors() const + size_t getNrOfSixAxisForceTorqueSensors() const override { return 1; } - virtual yarp::dev::MAS_status getSixAxisForceTorqueSensorStatus(size_t sens_index) const + + yarp::dev::MAS_status getSixAxisForceTorqueSensorStatus(size_t sens_index) const override { if (sens_index >= 1) { @@ -87,7 +85,8 @@ class yarp::dev::gzyarp::ForceTorqueDriver : public yarp::dev::DeviceDriver, return MAS_OK; } - virtual bool getSixAxisForceTorqueSensorName(size_t sens_index, std::string& name) const + + bool getSixAxisForceTorqueSensorName(size_t sens_index, std::string& name) const override { if (sens_index >= 1) { @@ -97,8 +96,9 @@ class yarp::dev::gzyarp::ForceTorqueDriver : public yarp::dev::DeviceDriver, name = m_sensorName; return true; } - virtual bool - getSixAxisForceTorqueSensorFrameName(size_t sens_index, std::string& frameName) const + + bool + getSixAxisForceTorqueSensorFrameName(size_t sens_index, std::string& frameName) const override { if (sens_index >= 1) { @@ -212,8 +212,15 @@ class yarp::dev::gzyarp::ForceTorqueDriver : public yarp::dev::DeviceDriver, return true; } + // IForceTorqueData + + void setForceTorqueData(::gzyarp::ForceTorqueData* ftData) override + { + m_sensorData = ftData; + } + private: - ForceTorqueData* m_sensorData; + ::gzyarp::ForceTorqueData* m_sensorData = nullptr; std::string m_sensorName; std::string m_frameName; }; diff --git a/plugins/forcetorque/ForceTorqueShared.hh b/plugins/forcetorque/ForceTorqueShared.hh new file mode 100644 index 0000000..e1c7509 --- /dev/null +++ b/plugins/forcetorque/ForceTorqueShared.hh @@ -0,0 +1,24 @@ +#include +#include +#include + +namespace gzyarp +{ + +struct ForceTorqueData +{ + mutable std::mutex m_mutex; + std::array m_data; + std::string sensorScopedName; + double simTime; +}; + +class IForceTorqueData +{ +public: + virtual void setForceTorqueData(ForceTorqueData*) = 0; + + virtual ~IForceTorqueData(){}; +}; + +} // namespace gzyarp diff --git a/plugins/forcetorque/singleton/CMakeLists.txt b/plugins/forcetorque/singleton/CMakeLists.txt deleted file mode 100644 index ea0f60a..0000000 --- a/plugins/forcetorque/singleton/CMakeLists.txt +++ /dev/null @@ -1,17 +0,0 @@ - -add_library(gz-sim-yarp-forcetorque-singleton SHARED ForceTorqueDataSingleton.hh ForceTorqueDataSingleton.cc) - -target_include_directories(gz-sim-yarp-forcetorque-singleton - PUBLIC "$") - -target_compile_features(gz-sim-yarp-forcetorque-singleton PRIVATE cxx_std_17) - -target_link_libraries(gz-sim-yarp-forcetorque-singleton - PRIVATE gz-plugin${GZ_PLUGIN_VER}::gz-plugin${GZ_PLUGIN_VER} - PRIVATE gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER} - PRIVATE - YARP::YARP_dev - YARP::YARP_os - YARP::YARP_init) - -install(TARGETS gz-sim-yarp-forcetorque-singleton) diff --git a/plugins/forcetorque/singleton/ForceTorqueDataSingleton.cc b/plugins/forcetorque/singleton/ForceTorqueDataSingleton.cc deleted file mode 100644 index 2d9806c..0000000 --- a/plugins/forcetorque/singleton/ForceTorqueDataSingleton.cc +++ /dev/null @@ -1,91 +0,0 @@ -#include - -#include -#include - -namespace gzyarp -{ - -ForceTorqueDataSingleton* ForceTorqueDataSingleton::getHandler() -{ - std::lock_guard lock(mutex()); - if (!s_handle) - { - s_handle = new ForceTorqueDataSingleton(); - if (!s_handle) - yError() << "Error while calling gzyarp::HandlerForceTorque constructor"; - } - - return s_handle; -} - -bool ForceTorqueDataSingleton::setSensor(ForceTorqueData* _sensorDataPtr) -{ - bool ret = false; - std::string sensorScopedName = _sensorDataPtr->sensorScopedName; - SensorsMap::iterator sensor = m_sensorsMap.find(sensorScopedName); - - if (sensor != m_sensorsMap.end()) - ret = true; - else - { - // sensor does not exists. Add to map - if (!m_sensorsMap - .insert(std::pair(sensorScopedName, _sensorDataPtr)) - .second) - { - yError() << "Error in gzyarp::HandlerForceTorque while inserting a new sensor pointer!"; - yError() << " The name of the sensor is already present but the pointer does not match " - "with the one already registered!"; - yError() << " This should not happen, as the scoped name should be unique in Gazebo. " - "Fatal error."; - ret = false; - } else - ret = true; - } - return ret; -} - -ForceTorqueData* ForceTorqueDataSingleton::getSensor(const std::string& sensorScopedName) const -{ - ForceTorqueData* tmp; - - SensorsMap::const_iterator sensor = m_sensorsMap.find(sensorScopedName); - if (sensor != m_sensorsMap.end()) - { - tmp = sensor->second; - } else - { - yError() << "Sensor was not found: " << sensorScopedName; - tmp = NULL; - } - return tmp; -} - -void ForceTorqueDataSingleton::removeSensor(const std::string& sensorName) -{ - SensorsMap::iterator sensor = m_sensorsMap.find(sensorName); - if (sensor != m_sensorsMap.end()) - { - m_sensorsMap.erase(sensor); - } else - { - yError() << "Could not remove sensor " << sensorName << ". Sensor was not found"; - } -} - -ForceTorqueDataSingleton::ForceTorqueDataSingleton() - : m_sensorsMap() -{ - m_sensorsMap.clear(); -} - -ForceTorqueDataSingleton* ForceTorqueDataSingleton::s_handle = NULL; - -std::mutex& ForceTorqueDataSingleton::mutex() -{ - static std::mutex s_mutex; - return s_mutex; -} - -} // namespace gzyarp diff --git a/plugins/forcetorque/singleton/ForceTorqueDataSingleton.hh b/plugins/forcetorque/singleton/ForceTorqueDataSingleton.hh deleted file mode 100644 index f6232c8..0000000 --- a/plugins/forcetorque/singleton/ForceTorqueDataSingleton.hh +++ /dev/null @@ -1,40 +0,0 @@ -#include -#include -#include - -#include - -#include -#include - -struct ForceTorqueData -{ - std::mutex m_mutex; - std::array m_data; - std::string sensorScopedName; - double simTime; -}; - -namespace gzyarp -{ - -class ForceTorqueDataSingleton -{ -public: - static ForceTorqueDataSingleton* getHandler(); - - bool setSensor(ForceTorqueData* _sensorDataPtr); - - ForceTorqueData* getSensor(const std::string& sensorScopedName) const; - - void removeSensor(const std::string& sensorName); - -private: - ForceTorqueDataSingleton(); - static ForceTorqueDataSingleton* s_handle; - static std::mutex& mutex(); - typedef std::map SensorsMap; - SensorsMap m_sensorsMap; // map of known sensors -}; - -} // namespace gzyarp From 0a9a68852126a1d31573bb67bf305b5d2e57cfc5 Mon Sep 17 00:00:00 2001 From: Alessandro Croci Date: Fri, 12 Apr 2024 16:47:30 +0200 Subject: [PATCH 08/25] Refactor data management of imu plugin --- plugins/imu/CMakeLists.txt | 3 - plugins/imu/Imu.cc | 30 ++++++-- plugins/imu/ImuDriver.cpp | 25 ++++--- plugins/imu/ImuShared.hh | 26 +++++++ plugins/imu/singleton/CMakeLists.txt | 18 ----- plugins/imu/singleton/ImuDataSingleton.cc | 90 ----------------------- plugins/imu/singleton/ImuDataSingleton.hh | 40 ---------- 7 files changed, 66 insertions(+), 166 deletions(-) create mode 100644 plugins/imu/ImuShared.hh delete mode 100644 plugins/imu/singleton/CMakeLists.txt delete mode 100644 plugins/imu/singleton/ImuDataSingleton.cc delete mode 100644 plugins/imu/singleton/ImuDataSingleton.hh diff --git a/plugins/imu/CMakeLists.txt b/plugins/imu/CMakeLists.txt index 7c3e113..4a662d1 100644 --- a/plugins/imu/CMakeLists.txt +++ b/plugins/imu/CMakeLists.txt @@ -1,5 +1,3 @@ -add_subdirectory(singleton) - add_library(gz-sim-yarp-imu-system SHARED Imu.cc ImuDriver.cpp) target_include_directories(gz-sim-yarp-imu-system @@ -15,7 +13,6 @@ target_link_libraries(gz-sim-yarp-imu-system YARP::YARP_os YARP::YARP_init gz-sim-yarp-device-registry - gz-sim-yarp-imu-singleton gz-sim-yarp-commons) # Add install target diff --git a/plugins/imu/Imu.cc b/plugins/imu/Imu.cc index 860774a..b950463 100644 --- a/plugins/imu/Imu.cc +++ b/plugins/imu/Imu.cc @@ -1,24 +1,37 @@ #include #include #include +#include + +#include +#include +#include #include #include +#include +#include +#include #include #include #include #include +#include #include #include #include #include #include #include +#include +#include #include #include +#include #include #include +#include using namespace gz; using namespace sim; @@ -49,7 +62,6 @@ class Imu : public System, if (m_imuDriver.isValid()) m_imuDriver.close(); - ImuDataSingleton::getHandler()->removeSensor(sensorScopedName); } virtual void Configure(const Entity& _entity, @@ -104,9 +116,6 @@ class Imu : public System, driver_properties.put(YarpIMUScopedName.c_str(), sensorScopedName.c_str()); - // Insert the pointer in the singleton handler for retriving it in the yarp driver - ImuDataSingleton::getHandler()->setSensor(&(this->imuData)); - driver_properties.put("device", "gazebo_imu"); driver_properties.put("sensor_name", sensorName); if (!m_imuDriver.open(driver_properties)) @@ -115,6 +124,16 @@ class Imu : public System, return; } + IImuData* iImuDataPtr = nullptr; + auto viewOk = m_imuDriver.view(iImuDataPtr); + + if (!viewOk || !iImuDataPtr) + { + yError() << "gz-sim-yarp-imu-system: failed to get IImuData interface"; + return; + } + iImuDataPtr->setImuData(&imuData); + auto deviceName = driver_properties.find("yarpDeviceName").asString(); m_deviceId = DeviceIdGenerator::generateDeviceId(sensor, _ecm, deviceName); @@ -124,6 +143,7 @@ class Imu : public System, << ")"; return; } + m_deviceRegistered = true; yInfo() << "Registered YARP device with instance name:" << m_deviceId; } @@ -176,7 +196,7 @@ class Imu : public System, std::string m_deviceId; std::string sensorScopedName; bool m_deviceRegistered; - ImuData imuData; + ::gzyarp::ImuData imuData; bool imuInitialized; gz::transport::Node node; gz::msgs::IMU imuMsg; diff --git a/plugins/imu/ImuDriver.cpp b/plugins/imu/ImuDriver.cpp index b974d31..9c36666 100644 --- a/plugins/imu/ImuDriver.cpp +++ b/plugins/imu/ImuDriver.cpp @@ -1,12 +1,16 @@ #include -#include +#include +#include #include +#include #include #include #include #include +#include +#include namespace yarp { @@ -35,7 +39,8 @@ class yarp::dev::gzyarp::ImuDriver : public yarp::dev::DeviceDriver, public yarp::dev::IThreeAxisGyroscopes, public yarp::dev::IThreeAxisLinearAccelerometers, public yarp::dev::IOrientationSensors, - public yarp::dev::IThreeAxisMagnetometers + public yarp::dev::IThreeAxisMagnetometers, + public ::gzyarp::IImuData { public: @@ -63,13 +68,6 @@ class yarp::dev::gzyarp::ImuDriver : public yarp::dev::DeviceDriver, } m_frameName = m_sensorName; - m_sensorData = ::gzyarp::ImuDataSingleton::getHandler()->getSensor(sensorScopedName); - - if (!m_sensorData) - { - yError() << "Error, IMU sensor was not found"; - return false; - } return true; } @@ -193,8 +191,15 @@ class yarp::dev::gzyarp::ImuDriver : public yarp::dev::DeviceDriver, return genericGetMeasure(sens_index, out, timestamp, 0); } + // IIMUDATA + + void setImuData(::gzyarp::ImuData* dataPtr) override + { + m_sensorData = dataPtr; + } + private: - ImuData* m_sensorData; + ::gzyarp::ImuData* m_sensorData = nullptr; std::string m_sensorName; std::string m_frameName; diff --git a/plugins/imu/ImuShared.hh b/plugins/imu/ImuShared.hh new file mode 100644 index 0000000..367badc --- /dev/null +++ b/plugins/imu/ImuShared.hh @@ -0,0 +1,26 @@ +#pragma once + +#include +#include +#include + +namespace gzyarp +{ + +struct ImuData +{ + std::mutex m_mutex; + std::array m_data; + std::string sensorScopedName; + double simTime; +}; + +class IImuData +{ +public: + virtual void setImuData(ImuData* dataPtr) = 0; + + virtual ~IImuData(){}; +}; + +} // namespace gzyarp diff --git a/plugins/imu/singleton/CMakeLists.txt b/plugins/imu/singleton/CMakeLists.txt deleted file mode 100644 index a423d12..0000000 --- a/plugins/imu/singleton/CMakeLists.txt +++ /dev/null @@ -1,18 +0,0 @@ - -add_library(gz-sim-yarp-imu-singleton SHARED ImuDataSingleton.hh ImuDataSingleton.cc) - -target_include_directories(gz-sim-yarp-imu-singleton - PUBLIC "$") - -target_compile_features(gz-sim-yarp-imu-singleton PRIVATE cxx_std_17) - -target_link_libraries(gz-sim-yarp-imu-singleton - PRIVATE gz-plugin${GZ_PLUGIN_VER}::gz-plugin${GZ_PLUGIN_VER} - PRIVATE gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER} - PRIVATE - YARP::YARP_dev - YARP::YARP_os - YARP::YARP_init) - -# Add install target -install(TARGETS gz-sim-yarp-imu-singleton) diff --git a/plugins/imu/singleton/ImuDataSingleton.cc b/plugins/imu/singleton/ImuDataSingleton.cc deleted file mode 100644 index fa27e21..0000000 --- a/plugins/imu/singleton/ImuDataSingleton.cc +++ /dev/null @@ -1,90 +0,0 @@ -#include - -#include -#include - -namespace gzyarp -{ - -ImuDataSingleton* ImuDataSingleton::getHandler() -{ - std::lock_guard lock(mutex()); - if (!s_handle) - { - s_handle = new ImuDataSingleton(); - if (!s_handle) - yError() << "Error while calling gzyarp::HandlerIMU constructor"; - } - - return s_handle; -} - -bool ImuDataSingleton::setSensor(ImuData* _sensorDataPtr) -{ - bool ret = false; - std::string sensorScopedName = _sensorDataPtr->sensorScopedName; - SensorsMap::iterator sensor = m_sensorsMap.find(sensorScopedName); - - if (sensor != m_sensorsMap.end()) - ret = true; - else - { - // sensor does not exists. Add to map - if (!m_sensorsMap.insert(std::pair(sensorScopedName, _sensorDataPtr)) - .second) - { - yError() << "Error in gzyarp::HandlerIMU while inserting a new sensor pointer!"; - yError() << " The name of the sensor is already present but the pointer does not match " - "with the one already registered!"; - yError() << " This should not happen, as the scoped name should be unique in Gazebo. " - "Fatal error."; - ret = false; - } else - ret = true; - } - return ret; -} - -ImuData* ImuDataSingleton::getSensor(const std::string& sensorScopedName) const -{ - ImuData* tmp; - - SensorsMap::const_iterator sensor = m_sensorsMap.find(sensorScopedName); - if (sensor != m_sensorsMap.end()) - { - tmp = sensor->second; - } else - { - yError() << "Sensor was not found: " << sensorScopedName; - tmp = NULL; - } - return tmp; -} - -void ImuDataSingleton::removeSensor(const std::string& sensorName) -{ - SensorsMap::iterator sensor = m_sensorsMap.find(sensorName); - if (sensor != m_sensorsMap.end()) - { - m_sensorsMap.erase(sensor); - } else - { - yError() << "Could not remove sensor " << sensorName << ". Sensor was not found"; - } -} - -ImuDataSingleton::ImuDataSingleton() - : m_sensorsMap() -{ - m_sensorsMap.clear(); -} - -ImuDataSingleton* ImuDataSingleton::s_handle = NULL; - -std::mutex& ImuDataSingleton::mutex() -{ - static std::mutex s_mutex; - return s_mutex; -} - -} // namespace gzyarp diff --git a/plugins/imu/singleton/ImuDataSingleton.hh b/plugins/imu/singleton/ImuDataSingleton.hh deleted file mode 100644 index 13a03cd..0000000 --- a/plugins/imu/singleton/ImuDataSingleton.hh +++ /dev/null @@ -1,40 +0,0 @@ -#include -#include -#include - -#include - -#include -#include - -struct ImuData -{ - std::mutex m_mutex; - std::array m_data; - std::string sensorScopedName; - double simTime; -}; - -namespace gzyarp -{ - -class ImuDataSingleton -{ -public: - static ImuDataSingleton* getHandler(); - - bool setSensor(ImuData* _sensorDataPtr); - - ImuData* getSensor(const std::string& sensorScopedName) const; - - void removeSensor(const std::string& sensorName); - -private: - ImuDataSingleton(); - static ImuDataSingleton* s_handle; - static std::mutex& mutex(); - typedef std::map SensorsMap; - SensorsMap m_sensorsMap; // map of known sensors -}; - -} // namespace gzyarp From 2740f8e182e0889b9ead6c64d53b6750c4350453 Mon Sep 17 00:00:00 2001 From: Alessandro Croci Date: Fri, 12 Apr 2024 16:59:41 +0200 Subject: [PATCH 09/25] Refactor data management of laser plugin --- plugins/laser/CMakeLists.txt | 3 - plugins/laser/Laser.cc | 30 +++++- plugins/laser/LaserDriver.cpp | 40 ++++---- plugins/laser/LaserShared.hh | 26 ++++++ plugins/laser/singleton/CMakeLists.txt | 18 ---- plugins/laser/singleton/LaserDataSingleton.cc | 91 ------------------- plugins/laser/singleton/LaserDataSingleton.hh | 40 -------- 7 files changed, 75 insertions(+), 173 deletions(-) create mode 100644 plugins/laser/LaserShared.hh delete mode 100644 plugins/laser/singleton/CMakeLists.txt delete mode 100644 plugins/laser/singleton/LaserDataSingleton.cc delete mode 100644 plugins/laser/singleton/LaserDataSingleton.hh diff --git a/plugins/laser/CMakeLists.txt b/plugins/laser/CMakeLists.txt index 35a3933..2732904 100644 --- a/plugins/laser/CMakeLists.txt +++ b/plugins/laser/CMakeLists.txt @@ -1,5 +1,3 @@ -add_subdirectory(singleton) - add_library(gz-sim-yarp-laser-system SHARED Laser.cc LaserDriver.cpp) target_include_directories(gz-sim-yarp-laser-system @@ -15,7 +13,6 @@ target_link_libraries(gz-sim-yarp-laser-system YARP::YARP_os YARP::YARP_init gz-sim-yarp-device-registry - gz-sim-yarp-laser-singleton gz-sim-yarp-commons) # Add install target diff --git a/plugins/laser/Laser.cc b/plugins/laser/Laser.cc index 1741de3..046fc5e 100644 --- a/plugins/laser/Laser.cc +++ b/plugins/laser/Laser.cc @@ -1,24 +1,39 @@ #include #include #include +#include + +#include +#include +#include +#include #include +#include #include +#include +#include +#include #include #include #include #include +#include #include #include #include #include #include #include +#include +#include #include #include +#include #include #include +#include using namespace gz; using namespace sim; @@ -49,7 +64,6 @@ class Laser : public System, if (m_laserDriver.isValid()) m_laserDriver.close(); - LaserDataSingleton::getHandler()->removeSensor(sensorScopedName); yarp::os::Network::fini(); } @@ -108,9 +122,6 @@ class Laser : public System, driver_properties.put(YarpLaserScopedName.c_str(), sensorScopedName.c_str()); - // Insert the pointer in the singleton handler for retriving it in the yarp driver - LaserDataSingleton::getHandler()->setSensor(&(this->laserData)); - driver_properties.put("device", "gazebo_laser"); driver_properties.put("sensor_name", sensorName); if (!m_laserDriver.open(driver_properties)) @@ -119,6 +130,17 @@ class Laser : public System, return; } + ILaserData* iLaserData = nullptr; + auto viewOk = m_laserDriver.view(iLaserData); + + if (!viewOk || !iLaserData) + { + yError() << "gz-sim-yarp-laser-system Plugin failed: error in getting " + "ILaserData interface"; + return; + } + iLaserData->setLaserData(&laserData); + auto deviceName = driver_properties.find("yarpDeviceName").asString(); m_deviceId = DeviceIdGenerator::generateDeviceId(sensor, _ecm, deviceName); diff --git a/plugins/laser/LaserDriver.cpp b/plugins/laser/LaserDriver.cpp index 7fcf45a..b4c4f70 100644 --- a/plugins/laser/LaserDriver.cpp +++ b/plugins/laser/LaserDriver.cpp @@ -1,14 +1,18 @@ #include -#include +#include +#include #include +#include #include #include #include #include #include #include +#include +#include namespace yarp { @@ -24,7 +28,8 @@ class LaserDriver; const std::string YarpLaserScopedName = "sensorScopedName"; class yarp::dev::gzyarp::LaserDriver : public yarp::dev::Lidar2DDeviceBase, - public yarp::dev::DeviceDriver + public yarp::dev::DeviceDriver, + public ::gzyarp::ILaserData { public: LaserDriver() @@ -35,7 +40,7 @@ class yarp::dev::gzyarp::LaserDriver : public yarp::dev::Lidar2DDeviceBase, } // DEVICE DRIVER - virtual bool open(yarp::os::Searchable& config) + bool open(yarp::os::Searchable& config) override { std::string sensorScopedName(config.find(YarpLaserScopedName.c_str()).asString().c_str()); @@ -49,13 +54,7 @@ class yarp::dev::gzyarp::LaserDriver : public yarp::dev::Lidar2DDeviceBase, m_sensorName = config.find("sensor_name").asString().substr(pos + separator.size() - 1); } m_frameName = m_sensorName; - m_sensorData = ::gzyarp::LaserDataSingleton::getHandler()->getSensor(sensorScopedName); - if (!m_sensorData) - { - yError() << "Error, Laser sensor was not found"; - return false; - } if (this->parseConfiguration(config) == false) { yError() << "error parsing parameters"; @@ -66,18 +65,18 @@ class yarp::dev::gzyarp::LaserDriver : public yarp::dev::Lidar2DDeviceBase, return true; } - virtual bool close() + bool close() override { return true; } // Lidar2DDeviceBase - bool acquireDataFromHW() + bool acquireDataFromHW() override { return true; } - bool getRawData(yarp::sig::Vector& ranges, double* timestamp) + bool getRawData(yarp::sig::Vector& ranges, double* timestamp) override { std::lock_guard lock(m_sensorData->m_mutex); @@ -92,36 +91,43 @@ class yarp::dev::gzyarp::LaserDriver : public yarp::dev::Lidar2DDeviceBase, } // IRangefinder2D - virtual bool setDistanceRange(double min, double max) + bool setDistanceRange(double min, double max) override { std::lock_guard guard(m_mutex); yError() << "setDistanceRange() Not yet implemented"; return false; } - virtual bool setScanLimits(double min, double max) + bool setScanLimits(double min, double max) override { std::lock_guard guard(m_mutex); yError() << "setScanLimits() Not yet implemented"; return false; } - virtual bool setHorizontalResolution(double step) + bool setHorizontalResolution(double step) override { std::lock_guard guard(m_mutex); yError() << "setHorizontalResolution() Not yet implemented"; return false; } - virtual bool setScanRate(double rate) + bool setScanRate(double rate) override { std::lock_guard guard(m_mutex); yError() << "setScanRate() Not yet implemented"; return false; } + // ILaserData + + void setLaserData(::gzyarp::LaserData* dataPtr) override + { + m_sensorData = dataPtr; + } + private: - LaserData* m_sensorData; + ::gzyarp::LaserData* m_sensorData; std::string m_sensorName; std::string m_frameName; }; diff --git a/plugins/laser/LaserShared.hh b/plugins/laser/LaserShared.hh new file mode 100644 index 0000000..9e0c8a6 --- /dev/null +++ b/plugins/laser/LaserShared.hh @@ -0,0 +1,26 @@ +#pragma once + +#include +#include +#include + +namespace gzyarp +{ + +struct LaserData +{ + std::mutex m_mutex; + std::vector m_data; + std::string sensorScopedName; + double simTime; +}; + +class ILaserData +{ +public: + virtual void setLaserData(LaserData* dataPtr) = 0; + + virtual ~ILaserData(){}; +}; + +} // namespace gzyarp diff --git a/plugins/laser/singleton/CMakeLists.txt b/plugins/laser/singleton/CMakeLists.txt deleted file mode 100644 index 3178cd2..0000000 --- a/plugins/laser/singleton/CMakeLists.txt +++ /dev/null @@ -1,18 +0,0 @@ - -add_library(gz-sim-yarp-laser-singleton SHARED LaserDataSingleton.hh LaserDataSingleton.cc) - -target_include_directories(gz-sim-yarp-laser-singleton - PUBLIC "$") - -target_compile_features(gz-sim-yarp-laser-singleton PRIVATE cxx_std_17) - -target_link_libraries(gz-sim-yarp-laser-singleton - PRIVATE gz-plugin${GZ_PLUGIN_VER}::gz-plugin${GZ_PLUGIN_VER} - PRIVATE gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER} - PRIVATE - YARP::YARP_dev - YARP::YARP_os - YARP::YARP_init) - -# Add install target -install(TARGETS gz-sim-yarp-laser-singleton) diff --git a/plugins/laser/singleton/LaserDataSingleton.cc b/plugins/laser/singleton/LaserDataSingleton.cc deleted file mode 100644 index e6a952a..0000000 --- a/plugins/laser/singleton/LaserDataSingleton.cc +++ /dev/null @@ -1,91 +0,0 @@ -#include - -#include -#include - -namespace gzyarp -{ - -LaserDataSingleton* LaserDataSingleton::getHandler() -{ - std::lock_guard lock(mutex()); - if (!s_handle) - { - s_handle = new LaserDataSingleton(); - if (!s_handle) - yError() << "Error while calling gzyarp::HandlerLaser constructor"; - } - - return s_handle; -} - -bool LaserDataSingleton::setSensor(LaserData* _sensorDataPtr) -{ - bool ret = false; - std::string sensorScopedName = _sensorDataPtr->sensorScopedName; - SensorsMap::iterator sensor = m_sensorsMap.find(sensorScopedName); - - if (sensor != m_sensorsMap.end()) - ret = true; - else - { - // sensor does not exists. Add to map - if (!m_sensorsMap - .insert(std::pair(sensorScopedName, _sensorDataPtr)) - .second) - { - yError() << "Error in gzyarp::HandlerLaser while inserting a new sensor pointer!"; - yError() << " The name of the sensor is already present but the pointer does not match " - "with the one already registered!"; - yError() << " This should not happen, as the scoped name should be unique in Gazebo. " - "Fatal error."; - ret = false; - } else - ret = true; - } - return ret; -} - -LaserData* LaserDataSingleton::getSensor(const std::string& sensorScopedName) const -{ - LaserData* tmp; - - SensorsMap::const_iterator sensor = m_sensorsMap.find(sensorScopedName); - if (sensor != m_sensorsMap.end()) - { - tmp = sensor->second; - } else - { - yError() << "Sensor was not found: " << sensorScopedName; - tmp = NULL; - } - return tmp; -} - -void LaserDataSingleton::removeSensor(const std::string& sensorName) -{ - SensorsMap::iterator sensor = m_sensorsMap.find(sensorName); - if (sensor != m_sensorsMap.end()) - { - m_sensorsMap.erase(sensor); - } else - { - yError() << "Could not remove sensor " << sensorName << ". Sensor was not found"; - } -} - -LaserDataSingleton::LaserDataSingleton() - : m_sensorsMap() -{ - m_sensorsMap.clear(); -} - -LaserDataSingleton* LaserDataSingleton::s_handle = NULL; - -std::mutex& LaserDataSingleton::mutex() -{ - static std::mutex s_mutex; - return s_mutex; -} - -} // namespace gzyarp diff --git a/plugins/laser/singleton/LaserDataSingleton.hh b/plugins/laser/singleton/LaserDataSingleton.hh deleted file mode 100644 index 89a05d0..0000000 --- a/plugins/laser/singleton/LaserDataSingleton.hh +++ /dev/null @@ -1,40 +0,0 @@ -#include -#include -#include - -#include - -#include -#include - -struct LaserData -{ - std::mutex m_mutex; - std::vector m_data; - std::string sensorScopedName; - double simTime; -}; - -namespace gzyarp -{ - -class LaserDataSingleton -{ -public: - static LaserDataSingleton* getHandler(); - - bool setSensor(LaserData* _sensorDataPtr); - - LaserData* getSensor(const std::string& sensorScopedName) const; - - void removeSensor(const std::string& sensorName); - -private: - LaserDataSingleton(); - static LaserDataSingleton* s_handle; - static std::mutex& mutex(); - typedef std::map SensorsMap; - SensorsMap m_sensorsMap; // map of known sensors -}; - -} // namespace gzyarp From 4173112f6a62b0037c553b3b3b81e70be436a6e0 Mon Sep 17 00:00:00 2001 From: Alessandro Croci Date: Fri, 12 Apr 2024 17:14:18 +0200 Subject: [PATCH 10/25] Refactor data management of camera plugin --- plugins/camera/CMakeLists.txt | 3 - plugins/camera/Camera.cc | 32 ++++++- plugins/camera/CameraDriver.cpp | 39 ++++---- plugins/camera/CameraShared.hh | 28 ++++++ plugins/camera/singleton/CMakeLists.txt | 16 ---- .../camera/singleton/CameraDataSingleton.cc | 91 ------------------- .../camera/singleton/CameraDataSingleton.hh | 42 --------- 7 files changed, 79 insertions(+), 172 deletions(-) create mode 100644 plugins/camera/CameraShared.hh delete mode 100644 plugins/camera/singleton/CMakeLists.txt delete mode 100644 plugins/camera/singleton/CameraDataSingleton.cc delete mode 100644 plugins/camera/singleton/CameraDataSingleton.hh diff --git a/plugins/camera/CMakeLists.txt b/plugins/camera/CMakeLists.txt index a003389..eab84fc 100644 --- a/plugins/camera/CMakeLists.txt +++ b/plugins/camera/CMakeLists.txt @@ -1,5 +1,3 @@ -add_subdirectory(singleton) - add_library(gz-sim-yarp-camera-system SHARED Camera.cc CameraDriver.cpp) target_include_directories(gz-sim-yarp-camera-system PUBLIC "$") @@ -14,7 +12,6 @@ target_link_libraries(gz-sim-yarp-camera-system YARP::YARP_os YARP::YARP_init gz-sim-yarp-device-registry - gz-sim-yarp-camera-singleton gz-sim-yarp-commons) # Add install target diff --git a/plugins/camera/Camera.cc b/plugins/camera/Camera.cc index 63d5875..ade66c8 100644 --- a/plugins/camera/Camera.cc +++ b/plugins/camera/Camera.cc @@ -1,23 +1,40 @@ #include +#include #include #include +#include +#include +#include +#include +#include + +#include #include +#include +#include +#include #include #include #include #include +#include #include #include #include #include #include #include +#include +#include +#include #include #include +#include #include #include +#include using namespace gz; using namespace sim; @@ -50,7 +67,6 @@ class Camera : public System, { m_cameraDriver.close(); } - CameraDataSingleton::getHandler()->removeSensor(sensorScopedName); yarp::os::Network::fini(); } @@ -117,9 +133,6 @@ class Camera : public System, driver_properties.put(YarpCameraScopedName.c_str(), sensorScopedName.c_str()); - // Insert the pointer in the singleton handler for retriving it in the yarp driver - CameraDataSingleton::getHandler()->setSensor(&(this->cameraData)); - driver_properties.put("device", "gazebo_camera"); driver_properties.put("sensor_name", sensorName); @@ -130,6 +143,17 @@ class Camera : public System, return; } + ICameraData* iCameraData = nullptr; + auto viewOk = m_cameraDriver.view(iCameraData); + + if (!viewOk || !iCameraData) + { + yError() << "gz-sim-yarp-camera-system Plugin failed: error in getting " + "ICameraData interface"; + return; + } + iCameraData->setCameraData(&cameraData); + m_cameraDriver.view(iFrameGrabberImage); if (iFrameGrabberImage == NULL) { diff --git a/plugins/camera/CameraDriver.cpp b/plugins/camera/CameraDriver.cpp index 67768e1..c5fa184 100644 --- a/plugins/camera/CameraDriver.cpp +++ b/plugins/camera/CameraDriver.cpp @@ -1,12 +1,18 @@ -#include +#include #include +#include +#include #include +#include #include #include +#include #include #include +#include +#include namespace yarp { @@ -22,7 +28,8 @@ class CameraDriver; const std::string YarpCameraScopedName = "sensorScopedName"; class yarp::dev::gzyarp::CameraDriver : public yarp::dev::DeviceDriver, - public yarp::dev::IFrameGrabberImage + public yarp::dev::IFrameGrabberImage, + public ::gzyarp::ICameraData { public: CameraDriver() @@ -51,16 +58,9 @@ class yarp::dev::gzyarp::CameraDriver : public yarp::dev::DeviceDriver, } // DEVICE DRIVER - virtual bool open(yarp::os::Searchable& config) + bool open(yarp::os::Searchable& config) override { std::string sensorScopedName(config.find(YarpCameraScopedName.c_str()).asString().c_str()); - m_sensorData = ::gzyarp::CameraDataSingleton::getHandler()->getSensor(sensorScopedName); - - if (!m_sensorData) - { - yError() << "Error, Camera sensor was not found"; - return false; - } { std::lock_guard lock(m_sensorData->m_mutex); @@ -80,7 +80,7 @@ class yarp::dev::gzyarp::CameraDriver : public yarp::dev::DeviceDriver, return true; } - virtual bool close() + bool close() override { delete[] m_sensorData->m_imageBuffer; m_sensorData->m_imageBuffer = 0; @@ -88,7 +88,7 @@ class yarp::dev::gzyarp::CameraDriver : public yarp::dev::DeviceDriver, } // IFRAMEGRABBER IMAGE - virtual bool getImage(yarp::sig::ImageOf& _image) + bool getImage(yarp::sig::ImageOf& _image) override { std::lock_guard lock(m_sensorData->m_mutex); _image.resize(width(), height()); @@ -184,17 +184,17 @@ class yarp::dev::gzyarp::CameraDriver : public yarp::dev::DeviceDriver, return true; } - virtual int height() const + int height() const override { return m_sensorData->m_height; } - virtual int width() const + int width() const override { return m_sensorData->m_width; } - virtual int getRawBufferSize() + int getRawBufferSize() { return m_sensorData->m_bufferSize; } @@ -270,8 +270,15 @@ class yarp::dev::gzyarp::CameraDriver : public yarp::dev::DeviceDriver, } } + // ICameraData + + void setCameraData(::gzyarp::CameraData* dataPtr) override + { + m_sensorData = dataPtr; + } + private: - CameraData* m_sensorData; + ::gzyarp::CameraData* m_sensorData; int counter; bool m_vertical_flip; bool m_horizontal_flip; diff --git a/plugins/camera/CameraShared.hh b/plugins/camera/CameraShared.hh new file mode 100644 index 0000000..a468818 --- /dev/null +++ b/plugins/camera/CameraShared.hh @@ -0,0 +1,28 @@ +#pragma once + +#include +#include + +namespace gzyarp +{ + +struct CameraData +{ + std::mutex m_mutex; + int m_width; + int m_height; + int m_bufferSize; + unsigned char* m_imageBuffer; + std::string sensorScopedName; + double simTime; +}; + +class ICameraData +{ +public: + virtual void setCameraData(CameraData* dataPtr) = 0; + + virtual ~ICameraData(){}; +}; + +} // namespace gzyarp diff --git a/plugins/camera/singleton/CMakeLists.txt b/plugins/camera/singleton/CMakeLists.txt deleted file mode 100644 index 7ea8056..0000000 --- a/plugins/camera/singleton/CMakeLists.txt +++ /dev/null @@ -1,16 +0,0 @@ -add_library(gz-sim-yarp-camera-singleton SHARED CameraDataSingleton.hh CameraDataSingleton.cc) - -target_include_directories(gz-sim-yarp-camera-singleton PUBLIC "$") - -target_compile_features(gz-sim-yarp-camera-singleton PRIVATE cxx_std_17) - -target_link_libraries(gz-sim-yarp-camera-singleton - PRIVATE gz-plugin${GZ_PLUGIN_VER}::gz-plugin${GZ_PLUGIN_VER} - PRIVATE gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER} - PRIVATE - YARP::YARP_dev - YARP::YARP_os - YARP::YARP_init) - -# Add install target -install(TARGETS gz-sim-yarp-camera-singleton) diff --git a/plugins/camera/singleton/CameraDataSingleton.cc b/plugins/camera/singleton/CameraDataSingleton.cc deleted file mode 100644 index f984480..0000000 --- a/plugins/camera/singleton/CameraDataSingleton.cc +++ /dev/null @@ -1,91 +0,0 @@ -#include - -#include -#include - -namespace gzyarp -{ - -CameraDataSingleton* CameraDataSingleton::getHandler() -{ - std::lock_guard lock(mutex()); - if (!s_handle) - { - s_handle = new CameraDataSingleton(); - if (!s_handle) - yError() << "Error while calling gzyarp::HandlerCamera constructor"; - } - - return s_handle; -} - -bool CameraDataSingleton::setSensor(CameraData* _sensorDataPtr) -{ - bool ret = false; - std::string sensorScopedName = _sensorDataPtr->sensorScopedName; - SensorsMap::iterator sensor = m_sensorsMap.find(sensorScopedName); - - if (sensor != m_sensorsMap.end()) - ret = true; - else - { - // sensor does not exists. Add to map - if (!m_sensorsMap - .insert(std::pair(sensorScopedName, _sensorDataPtr)) - .second) - { - yError() << "Error in gzyarp::HandlerCamera while inserting a new sensor pointer!"; - yError() << " The name of the sensor is already present but the pointer does not match " - "with the one already registered!"; - yError() << " This should not happen, as the scoped name should be unique in Gazebo. " - "Fatal error."; - ret = false; - } else - ret = true; - } - return ret; -} - -CameraData* CameraDataSingleton::getSensor(const std::string& sensorScopedName) const -{ - CameraData* tmp; - - SensorsMap::const_iterator sensor = m_sensorsMap.find(sensorScopedName); - if (sensor != m_sensorsMap.end()) - { - tmp = sensor->second; - } else - { - yError() << "Sensor was not found: " << sensorScopedName; - tmp = NULL; - } - return tmp; -} - -void CameraDataSingleton::removeSensor(const std::string& sensorName) -{ - SensorsMap::iterator sensor = m_sensorsMap.find(sensorName); - if (sensor != m_sensorsMap.end()) - { - m_sensorsMap.erase(sensor); - } else - { - yError() << "Could not remove sensor " << sensorName << ". Sensor was not found"; - } -} - -CameraDataSingleton::CameraDataSingleton() - : m_sensorsMap() -{ - m_sensorsMap.clear(); -} - -CameraDataSingleton* CameraDataSingleton::s_handle = NULL; - -std::mutex& CameraDataSingleton::mutex() -{ - static std::mutex s_mutex; - return s_mutex; -} - -} // namespace gzyarp diff --git a/plugins/camera/singleton/CameraDataSingleton.hh b/plugins/camera/singleton/CameraDataSingleton.hh deleted file mode 100644 index 4e874e9..0000000 --- a/plugins/camera/singleton/CameraDataSingleton.hh +++ /dev/null @@ -1,42 +0,0 @@ -#include -#include - -#include - -#include -#include - -struct CameraData -{ - std::mutex m_mutex; - int m_width; - int m_height; - int m_bufferSize; - unsigned char* m_imageBuffer; - std::string sensorScopedName; - double simTime; -}; - -namespace gzyarp -{ - -class CameraDataSingleton -{ -public: - static CameraDataSingleton* getHandler(); - - bool setSensor(CameraData* _sensorDataPtr); - - CameraData* getSensor(const std::string& sensorScopedName) const; - - void removeSensor(const std::string& sensorName); - -private: - CameraDataSingleton(); - static CameraDataSingleton* s_handle; - static std::mutex& mutex(); - typedef std::map SensorsMap; - SensorsMap m_sensorsMap; -}; - -} // namespace gzyarp From 71589ef5597d88c1e5cbe4aa52d3f8f00c1e01a8 Mon Sep 17 00:00:00 2001 From: Alessandro Croci Date: Fri, 12 Apr 2024 17:35:31 +0200 Subject: [PATCH 11/25] Refactor data management of basestate plugin --- plugins/basestate/BaseState.cc | 38 +++++-- plugins/basestate/BaseStateDriver.cpp | 43 +++++--- plugins/basestate/BaseStateShared.hh | 28 +++++ plugins/basestate/CMakeLists.txt | 4 - .../singleton/BaseStateDataSingleton.cc | 102 ------------------ .../singleton/BaseStateDataSingleton.hh | 43 -------- plugins/basestate/singleton/CMakeLists.txt | 17 --- 7 files changed, 84 insertions(+), 191 deletions(-) create mode 100644 plugins/basestate/BaseStateShared.hh delete mode 100644 plugins/basestate/singleton/BaseStateDataSingleton.cc delete mode 100644 plugins/basestate/singleton/BaseStateDataSingleton.hh delete mode 100644 plugins/basestate/singleton/CMakeLists.txt diff --git a/plugins/basestate/BaseState.cc b/plugins/basestate/BaseState.cc index 8a33d67..06bb97f 100644 --- a/plugins/basestate/BaseState.cc +++ b/plugins/basestate/BaseState.cc @@ -1,21 +1,36 @@ #include +#include #include #include +#include +#include +#include + +#include +#include #include +#include +#include +#include #include #include #include #include #include +#include #include #include #include +#include +#include #include #include +#include #include #include +#include using namespace gz; using namespace sim; @@ -44,13 +59,12 @@ class BaseState : public System, public ISystemConfigure, public ISystemPostUpda { m_baseStateDriver.close(); } - BaseStateDataSingleton::getBaseStateDataHandler()->removeBaseLink(m_baseLinkScopedName); } - virtual void Configure(const Entity& _entity, - const std::shared_ptr& _sdf, - EntityComponentManager& _ecm, - EventManager& /*_eventMgr*/) override + void Configure(const Entity& _entity, + const std::shared_ptr& _sdf, + EntityComponentManager& _ecm, + EventManager& /*_eventMgr*/) override { std::string netWrapper = "analogServer"; @@ -112,9 +126,6 @@ class BaseState : public System, public ISystemConfigure, public ISystemPostUpda driver_properties.put(YarpBaseStateScopedName.c_str(), m_baseLinkScopedName.c_str()); - // Insert the pointer in the singleton handler for retrieving it in the yarp driver - BaseStateDataSingleton::getBaseStateDataHandler()->setBaseStateData(&(m_baseStateData)); - driver_properties.put("device", "gazebo_basestate"); driver_properties.put("robot", m_baseLinkScopedName); @@ -125,6 +136,17 @@ class BaseState : public System, public ISystemConfigure, public ISystemPostUpda return; } + IBaseStateData* iBaseStateData = nullptr; + auto viewOk = m_baseStateDriver.view(iBaseStateData); + + if (!viewOk || !iBaseStateData) + { + yError() << "gz-sim-yarp-basestate-system Plugin failed: error in getting " + "IBaseStateData interface"; + return; + } + iBaseStateData->setBaseStateData(&m_baseStateData); + m_deviceId = DeviceIdGenerator::generateDeviceId(m_baseLinkEntity, _ecm, deviceName); if (!DeviceRegistry::getHandler()->setDevice(m_deviceId, &m_baseStateDriver)) diff --git a/plugins/basestate/BaseStateDriver.cpp b/plugins/basestate/BaseStateDriver.cpp index d9f14b1..90ecb12 100644 --- a/plugins/basestate/BaseStateDriver.cpp +++ b/plugins/basestate/BaseStateDriver.cpp @@ -1,7 +1,9 @@ -#include +#include #include +#include #include +#include #include #include @@ -9,6 +11,9 @@ #include #include #include +#include +#include +#include namespace yarp { @@ -26,7 +31,8 @@ const unsigned YarpBaseStateChannelsNumber = 18; // The BaseState sensor has 18 class yarp::dev::gzyarp::BaseStateDriver : public yarp::dev::DeviceDriver, public yarp::dev::IAnalogSensor, - public yarp::dev::IPreciselyTimed + public yarp::dev::IPreciselyTimed, + public ::gzyarp::IBaseStateData { public: BaseStateDriver() @@ -37,7 +43,7 @@ class yarp::dev::gzyarp::BaseStateDriver : public yarp::dev::DeviceDriver, } // DeviceDriver - virtual bool open(yarp::os::Searchable& config) + bool open(yarp::os::Searchable& config) override { std::string m_modelScopedName( @@ -53,10 +59,6 @@ class yarp::dev::gzyarp::BaseStateDriver : public yarp::dev::DeviceDriver, m_baseLinkName = config.find("baseLink").asString().substr(pos + separator.size() - 1); } - m_baseLinkData - = ::gzyarp::BaseStateDataSingleton::getBaseStateDataHandler()->getBaseStateData( - m_modelScopedName); - if (!m_baseLinkData) { yError() << "Error, BaseState sensor was not found"; @@ -66,12 +68,12 @@ class yarp::dev::gzyarp::BaseStateDriver : public yarp::dev::DeviceDriver, return true; } - virtual bool close() + bool close() override { return true; } - int read(yarp::sig::Vector& out) + int read(yarp::sig::Vector& out) override { if (out.size() != YarpBaseStateChannelsNumber) { @@ -102,7 +104,7 @@ class yarp::dev::gzyarp::BaseStateDriver : public yarp::dev::DeviceDriver, return AS_OK; } - yarp::os::Stamp getLastInputStamp() + yarp::os::Stamp getLastInputStamp() override { std::lock_guard lock(m_baseLinkData->mutex); @@ -114,37 +116,44 @@ class yarp::dev::gzyarp::BaseStateDriver : public yarp::dev::DeviceDriver, return m_baseLinkData->simTimestamp; } - int calibrateChannel(int ch, double v) + int calibrateChannel(int ch, double v) override { return AS_OK; } - int calibrateChannel(int ch) + int calibrateChannel(int ch) override { return AS_OK; } - int calibrateSensor(const yarp::sig::Vector& value) + int calibrateSensor(const yarp::sig::Vector& value) override { return AS_OK; } - int calibrateSensor() + int calibrateSensor() override { return AS_OK; } - int getChannels() + int getChannels() override { return AS_OK; } - int getState(int ch) + int getState(int ch) override { return AS_OK; } + // IBaseStateData + + void setBaseStateData(::gzyarp::BaseStateData* baseLinkData) override + { + m_baseLinkData = baseLinkData; + } + private: - BaseStateData* m_baseLinkData; + ::gzyarp::BaseStateData* m_baseLinkData; std::string m_baseLinkName; }; diff --git a/plugins/basestate/BaseStateShared.hh b/plugins/basestate/BaseStateShared.hh new file mode 100644 index 0000000..e829208 --- /dev/null +++ b/plugins/basestate/BaseStateShared.hh @@ -0,0 +1,28 @@ +#pragma once + +#include +#include +#include +#include + +namespace gzyarp +{ + +struct BaseStateData +{ + std::mutex mutex; + std::string baseLinkScopedName; + bool dataAvailable; + std::array data; + yarp::os::Stamp simTimestamp; +}; + +class IBaseStateData +{ +public: + virtual void setBaseStateData(BaseStateData*) = 0; + + virtual ~IBaseStateData(){}; +}; + +} // namespace gzyarp diff --git a/plugins/basestate/CMakeLists.txt b/plugins/basestate/CMakeLists.txt index 2966a4f..5ccdca4 100644 --- a/plugins/basestate/CMakeLists.txt +++ b/plugins/basestate/CMakeLists.txt @@ -1,6 +1,3 @@ - -add_subdirectory(singleton) - add_library(gz-sim-yarp-basestate-system SHARED BaseState.cc BaseStateDriver.cpp) target_include_directories(gz-sim-yarp-basestate-system @@ -16,7 +13,6 @@ target_link_libraries(gz-sim-yarp-basestate-system YARP::YARP_os YARP::YARP_init gz-sim-yarp-device-registry - gz-sim-yarp-basestate-singleton gz-sim-yarp-commons) # Add install target diff --git a/plugins/basestate/singleton/BaseStateDataSingleton.cc b/plugins/basestate/singleton/BaseStateDataSingleton.cc deleted file mode 100644 index 114a8c0..0000000 --- a/plugins/basestate/singleton/BaseStateDataSingleton.cc +++ /dev/null @@ -1,102 +0,0 @@ -#include - -#include -#include - -namespace gzyarp -{ - -BaseStateDataSingleton* BaseStateDataSingleton::getBaseStateDataHandler() -{ - { - std::lock_guard lock(mutex()); - if (!s_handle) - { - s_handle = new BaseStateDataSingleton(); - if (!s_handle) - { - yError() << "Error while calling gzyarp::HandlerBaseState constructor"; - } - } - } - return s_handle; -} - -bool BaseStateDataSingleton::setBaseStateData(BaseStateData* _baseStateDataPtr) -{ - bool ret = false; - BaseLinksMap::iterator baseLink = m_baseLinksMap.find(_baseStateDataPtr->baseLinkScopedName); - - if (baseLink != m_baseLinksMap.end()) - { - ret = true; - } else - { - // baseLink does not exists. Add to map - if (!m_baseLinksMap - .insert( - std::pair(_baseStateDataPtr->baseLinkScopedName, - _baseStateDataPtr)) - .second) - { - yError() << "Error in gzyarp::HandlerBaseState while inserting a new base link " - "pointer!"; - yError() << "The name of the base link is already present but the pointer does not " - "match " - "with the one already registered!"; - yError() << "This should not happen, as the scoped name should be unique in Gazebo. " - "Fatal error."; - ret = false; - } else - { - ret = true; - } - } - return ret; -} - -BaseStateData* -BaseStateDataSingleton::getBaseStateData(const std::string& _baseLinkScopedName) const -{ - BaseStateData* dataPtr; - - BaseLinksMap::const_iterator baseLink = m_baseLinksMap.find(_baseLinkScopedName); - if (baseLink != m_baseLinksMap.end()) - { - dataPtr = baseLink->second; - } else - { - yError() << "Base link was not found: " << _baseLinkScopedName; - dataPtr = nullptr; - } - return dataPtr; -} - -void BaseStateDataSingleton::removeBaseLink(const std::string& _baseLinkScopedName) -{ - BaseLinksMap::iterator baseLink = m_baseLinksMap.find(_baseLinkScopedName); - if (baseLink != m_baseLinksMap.end()) - { - m_baseLinksMap.erase(baseLink); - } else - { - yError() << "Could not remove base link " << _baseLinkScopedName - << ". Base link was not found"; - } -} - -BaseStateDataSingleton::BaseStateDataSingleton() - : m_baseLinksMap() -{ - m_baseLinksMap.clear(); -} - -BaseStateDataSingleton* BaseStateDataSingleton::s_handle = NULL; - -std::mutex& BaseStateDataSingleton::mutex() -{ - static std::mutex s_mutex; - return s_mutex; -} - -} // namespace gzyarp diff --git a/plugins/basestate/singleton/BaseStateDataSingleton.hh b/plugins/basestate/singleton/BaseStateDataSingleton.hh deleted file mode 100644 index 0c9e5f4..0000000 --- a/plugins/basestate/singleton/BaseStateDataSingleton.hh +++ /dev/null @@ -1,43 +0,0 @@ -#include -#include -#include - -#include - -#include -#include -#include - -struct BaseStateData -{ - std::mutex mutex; - std::string baseLinkScopedName; - bool dataAvailable; - std::array data; - yarp::os::Stamp simTimestamp; -}; - -namespace gzyarp -{ - -class BaseStateDataSingleton -{ - -public: - static BaseStateDataSingleton* getBaseStateDataHandler(); - - bool setBaseStateData(BaseStateData* _baseStateDataPtr); - - BaseStateData* getBaseStateData(const std::string& _baseLinkScopedName) const; - - void removeBaseLink(const std::string& _baseLinkScopedName); - -private: - BaseStateDataSingleton(); - static BaseStateDataSingleton* s_handle; - static std::mutex& mutex(); - typedef std::map BaseLinksMap; - BaseLinksMap m_baseLinksMap; // map of base links with their scoped name as key -}; - -} // namespace gzyarp diff --git a/plugins/basestate/singleton/CMakeLists.txt b/plugins/basestate/singleton/CMakeLists.txt deleted file mode 100644 index 24885e7..0000000 --- a/plugins/basestate/singleton/CMakeLists.txt +++ /dev/null @@ -1,17 +0,0 @@ - -add_library(gz-sim-yarp-basestate-singleton SHARED BaseStateDataSingleton.hh BaseStateDataSingleton.cc) - -target_include_directories(gz-sim-yarp-basestate-singleton - PUBLIC "$") - -target_compile_features(gz-sim-yarp-basestate-singleton PRIVATE cxx_std_17) - -target_link_libraries(gz-sim-yarp-basestate-singleton - PRIVATE gz-plugin${GZ_PLUGIN_VER}::gz-plugin${GZ_PLUGIN_VER} - PRIVATE gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER} - PRIVATE - YARP::YARP_dev - YARP::YARP_os - YARP::YARP_init) - -install(TARGETS gz-sim-yarp-basestate-singleton) From f0d224658bd884cd8836a9eda798092ba7eea778 Mon Sep 17 00:00:00 2001 From: Alessandro Croci Date: Mon, 15 Apr 2024 12:09:22 +0200 Subject: [PATCH 12/25] Refactor data management of controlboard plugin Fix also `ControlBoardCommonsTest` to use `DeviceRegistry` in place of the singleton and remove expectations on device ids (since they are an internal implementation detail). --- plugins/controlboard/CMakeLists.txt | 6 +- plugins/controlboard/include/ControlBoard.hh | 4 + .../ControlBoardData.hh | 10 ++ .../include/ControlBoardDriver.hh | 7 +- .../ControlBoardTrajectory.hh | 1 + plugins/controlboard/singleton/CMakeLists.txt | 16 --- .../singleton/ControlBoardDataSingleton.cpp | 117 ------------------ .../singleton/ControlBoardDataSingleton.hh | 41 ------ plugins/controlboard/src/ControlBoard.cpp | 23 +++- .../controlboard/src/ControlBoardDriver.cpp | 14 ++- .../ControlBoardTrajectory.cpp | 0 tests/controlboard/CMakeLists.txt | 5 +- tests/controlboard/ControlBoardCommonsTest.cc | 84 ++++++++----- 13 files changed, 102 insertions(+), 226 deletions(-) rename plugins/controlboard/{singleton => include}/ControlBoardData.hh (93%) rename plugins/controlboard/{singleton => include}/ControlBoardTrajectory.hh (99%) delete mode 100644 plugins/controlboard/singleton/CMakeLists.txt delete mode 100644 plugins/controlboard/singleton/ControlBoardDataSingleton.cpp delete mode 100644 plugins/controlboard/singleton/ControlBoardDataSingleton.hh rename plugins/controlboard/{singleton => src}/ControlBoardTrajectory.cpp (100%) diff --git a/plugins/controlboard/CMakeLists.txt b/plugins/controlboard/CMakeLists.txt index 393575a..e37ac2e 100644 --- a/plugins/controlboard/CMakeLists.txt +++ b/plugins/controlboard/CMakeLists.txt @@ -1,11 +1,14 @@ set(HEADER_FILES include/ControlBoard.hh + include/ControlBoardData.hh include/ControlBoardDriver.hh + include/ControlBoardTrajectory.hh ) set(SRC_FILES src/ControlBoard.cpp src/ControlBoardDriver.cpp + src/ControlBoardTrajectory.cpp ) add_library(gz-sim-yarp-controlboard-system SHARED ${HEADER_FILES} ${SRC_FILES}) @@ -15,8 +18,6 @@ target_include_directories(gz-sim-yarp-controlboard-system target_compile_features(gz-sim-yarp-controlboard-system PRIVATE cxx_std_17) -add_subdirectory(singleton) - target_link_libraries(gz-sim-yarp-controlboard-system PRIVATE gz-plugin${GZ_PLUGIN_VER}::gz-plugin${GZ_PLUGIN_VER} PRIVATE gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER} @@ -26,7 +27,6 @@ target_link_libraries(gz-sim-yarp-controlboard-system YARP::YARP_init gz-sim-yarp-commons gz-sim-yarp-device-registry - gz-sim-yarp-controlboard-singleton gz-sim-yarp-commons ) diff --git a/plugins/controlboard/include/ControlBoard.hh b/plugins/controlboard/include/ControlBoard.hh index c451989..5c87788 100644 --- a/plugins/controlboard/include/ControlBoard.hh +++ b/plugins/controlboard/include/ControlBoard.hh @@ -2,6 +2,8 @@ #include +#include +#include #include #include @@ -14,8 +16,10 @@ #include #include +#include #include #include +#include #include #include diff --git a/plugins/controlboard/singleton/ControlBoardData.hh b/plugins/controlboard/include/ControlBoardData.hh similarity index 93% rename from plugins/controlboard/singleton/ControlBoardData.hh rename to plugins/controlboard/include/ControlBoardData.hh index c8229dc..483fd39 100644 --- a/plugins/controlboard/singleton/ControlBoardData.hh +++ b/plugins/controlboard/include/ControlBoardData.hh @@ -3,6 +3,8 @@ #include #include +#include +#include #include #include #include @@ -72,4 +74,12 @@ public: std::chrono::milliseconds controlUpdatePeriod = std::chrono::milliseconds(1); }; +class IControlBoardData +{ +public: + virtual void setControlBoardData(ControlBoardData*) = 0; + + virtual ~IControlBoardData(){}; +}; + } // namespace gzyarp diff --git a/plugins/controlboard/include/ControlBoardDriver.hh b/plugins/controlboard/include/ControlBoardDriver.hh index 2b8c3e7..6636949 100644 --- a/plugins/controlboard/include/ControlBoardDriver.hh +++ b/plugins/controlboard/include/ControlBoardDriver.hh @@ -41,7 +41,8 @@ class ControlBoardDriver : public DeviceDriver, public IPositionControl, public IVelocityControl, public ICurrentControl, - public IPidControl + public IPidControl, + public ::gzyarp::IControlBoardData { public: // DeviceDriver @@ -202,6 +203,10 @@ public: bool setPidOffset(const PidControlTypeEnum& pidtype, int j, double v) override; bool isPidEnabled(const PidControlTypeEnum& pidtype, int j, bool* enabled) override; + // IControlBoardData + + void setControlBoardData(::gzyarp::ControlBoardData* controlBoardData) override; + private: std::string m_controlBoardId; ::gzyarp::ControlBoardData* m_controlBoardData; diff --git a/plugins/controlboard/singleton/ControlBoardTrajectory.hh b/plugins/controlboard/include/ControlBoardTrajectory.hh similarity index 99% rename from plugins/controlboard/singleton/ControlBoardTrajectory.hh rename to plugins/controlboard/include/ControlBoardTrajectory.hh index d06f416..52a734a 100644 --- a/plugins/controlboard/singleton/ControlBoardTrajectory.hh +++ b/plugins/controlboard/include/ControlBoardTrajectory.hh @@ -1,6 +1,7 @@ #pragma once #include +#include #include #include diff --git a/plugins/controlboard/singleton/CMakeLists.txt b/plugins/controlboard/singleton/CMakeLists.txt deleted file mode 100644 index 5d0f36a..0000000 --- a/plugins/controlboard/singleton/CMakeLists.txt +++ /dev/null @@ -1,16 +0,0 @@ - -add_library(gz-sim-yarp-controlboard-singleton SHARED ControlBoardData.hh ControlBoardDataSingleton.hh ControlBoardTrajectory.hh ControlBoardDataSingleton.cpp ControlBoardTrajectory.cpp) - -target_include_directories(gz-sim-yarp-controlboard-singleton PUBLIC "$") - -target_compile_features(gz-sim-yarp-controlboard-singleton PRIVATE cxx_std_17) - -target_link_libraries(gz-sim-yarp-controlboard-singleton - PRIVATE gz-plugin${GZ_PLUGIN_VER}::gz-plugin${GZ_PLUGIN_VER} - PRIVATE gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER} - PRIVATE - YARP::YARP_dev - YARP::YARP_os - YARP::YARP_init) - -install(TARGETS gz-sim-yarp-controlboard-singleton) diff --git a/plugins/controlboard/singleton/ControlBoardDataSingleton.cpp b/plugins/controlboard/singleton/ControlBoardDataSingleton.cpp deleted file mode 100644 index 92c86a4..0000000 --- a/plugins/controlboard/singleton/ControlBoardDataSingleton.cpp +++ /dev/null @@ -1,117 +0,0 @@ -#include - -#include - -#include -#include - -#include -#include - -namespace gzyarp -{ - -ControlBoardDataSingleton* ControlBoardDataSingleton::getControlBoardHandler() -{ - std::lock_guard lock(mutex()); - if (!s_handle) - { - s_handle = new ControlBoardDataSingleton(); - if (!s_handle) - { - yError() << "Error while calling gzyarp::ControlBoardDataSingleton constructor"; - } - } - return s_handle; -} - -bool ControlBoardDataSingleton::setControlBoardData(ControlBoardData* _controlBoardPtr) -{ - bool ret = false; - ControlBoardMap::iterator controlBoard - = m_controlBoardMap.find(_controlBoardPtr->controlBoardId); - - if (controlBoard != m_controlBoardMap.end()) - { - yDebug() << "Control board scoped name: " << _controlBoardPtr->controlBoardId - << " already present.\n"; - ret = true; - } else - { - // controlBoard does not exists. Add to map - if (!m_controlBoardMap - .insert(std::pair(_controlBoardPtr->controlBoardId, - _controlBoardPtr)) - .second) - { - yError() << "Error in gzyarp::ControlBoardDataSingleton while inserting a new control " - "board pointer!"; - yError() << "The name of the control board is already present but the pointer does not " - "match " - "with the one already registered!"; - yError() << "This should not happen, as the scoped name should be unique in Gazebo. " - "Fatal error."; - ret = false; - } else - { - ret = true; - yDebug() << "Added control board: " << _controlBoardPtr->controlBoardId; - } - } - return ret; -} - -ControlBoardData* -ControlBoardDataSingleton::getControlBoardData(const std::string& _controlBoardScopedName) const -{ - ControlBoardData* dataPtr; - ControlBoardMap::const_iterator controlBoard = m_controlBoardMap.find(_controlBoardScopedName); - if (controlBoard != m_controlBoardMap.end()) - { - dataPtr = controlBoard->second; - } else - { - dataPtr = nullptr; - } - return dataPtr; -} - -void ControlBoardDataSingleton::removeControlBoard(const std::string& _controlBoardScopedName) -{ - ControlBoardMap::iterator controlBoard = m_controlBoardMap.find(_controlBoardScopedName); - if (controlBoard != m_controlBoardMap.end()) - { - m_controlBoardMap.erase(controlBoard); - } else - { - yError() << "Control board was not found: " << _controlBoardScopedName; - } -} - -std::vector ControlBoardDataSingleton::getControlBoardKeys() const -{ - std::vector keys; - for (const auto& controlBoard : m_controlBoardMap) - { - keys.push_back(controlBoard.first); - } - return keys; -} - -// Private methods - -ControlBoardDataSingleton::ControlBoardDataSingleton() - : m_controlBoardMap() -{ - m_controlBoardMap.clear(); -} - -ControlBoardDataSingleton* ControlBoardDataSingleton::s_handle = nullptr; - -std::mutex& ControlBoardDataSingleton::mutex() -{ - static std::mutex s_mutex; - return s_mutex; -} - -} // namespace gzyarp diff --git a/plugins/controlboard/singleton/ControlBoardDataSingleton.hh b/plugins/controlboard/singleton/ControlBoardDataSingleton.hh deleted file mode 100644 index 33226af..0000000 --- a/plugins/controlboard/singleton/ControlBoardDataSingleton.hh +++ /dev/null @@ -1,41 +0,0 @@ -#pragma once - -#include - -#include -#include -#include -#include - -#include - -#include -#include -#include - -namespace gzyarp -{ - -class ControlBoardDataSingleton -{ - -public: - static ControlBoardDataSingleton* getControlBoardHandler(); - - bool setControlBoardData(ControlBoardData* _controlBoardPtr); - - ControlBoardData* getControlBoardData(const std::string& _controlBoardScopedName) const; - - void removeControlBoard(const std::string& _controlBoardScopedName); - - std::vector getControlBoardKeys() const; - -private: - ControlBoardDataSingleton(); - static ControlBoardDataSingleton* s_handle; - static std::mutex& mutex(); - typedef std::map ControlBoardMap; - ControlBoardMap m_controlBoardMap; // map of control boards with their scoped name as key -}; - -} // namespace gzyarp diff --git a/plugins/controlboard/src/ControlBoard.cpp b/plugins/controlboard/src/ControlBoard.cpp index 47989d5..a7047eb 100644 --- a/plugins/controlboard/src/ControlBoard.cpp +++ b/plugins/controlboard/src/ControlBoard.cpp @@ -2,19 +2,24 @@ #include #include -#include +#include #include +#include #include #include +#include #include #include #include #include #include #include +#include +#include #include #include +#include #include #include @@ -66,8 +71,6 @@ ControlBoard::~ControlBoard() { m_controlBoardDriver.close(); } - ControlBoardDataSingleton::getControlBoardHandler()->removeControlBoard( - m_controlBoardData.controlBoardId); } void ControlBoard::Configure(const Entity& _entity, @@ -113,9 +116,6 @@ void ControlBoard::Configure(const Entity& _entity, m_pluginParameters.put(yarp::dev::gzyarp::YarpControlBoardScopedName.c_str(), m_robotScopedName.c_str()); - // Insert the pointer in the singleton handler for retrieving it in the yarp driver - ControlBoardDataSingleton::getControlBoardHandler()->setControlBoardData(&(m_controlBoardData)); - m_pluginParameters.put("device", "gazebo_controlboard"); m_pluginParameters.put("controlBoardId", m_deviceId); @@ -141,6 +141,17 @@ void ControlBoard::Configure(const Entity& _entity, return; } + IControlBoardData* iControlBoardData = nullptr; + auto viewOk = m_controlBoardDriver.view(iControlBoardData); + + if (!viewOk || !iControlBoardData) + { + yError() << "gz-sim-yarp-controlboard-system Plugin failed: error in getting " + "IControlBoardData interface"; + return; + } + iControlBoardData->setControlBoardData(&m_controlBoardData); + if (!setJointProperties(_ecm)) { yError() << "gz-sim-yarp-controlboard-system: failed setting joint properties"; diff --git a/plugins/controlboard/src/ControlBoardDriver.cpp b/plugins/controlboard/src/ControlBoardDriver.cpp index 3d2ed23..de88952 100644 --- a/plugins/controlboard/src/ControlBoardDriver.cpp +++ b/plugins/controlboard/src/ControlBoardDriver.cpp @@ -1,7 +1,6 @@ #include #include -#include #include #include @@ -12,7 +11,9 @@ #include #include #include +#include #include +#include #include #include #include @@ -35,10 +36,6 @@ bool ControlBoardDriver::open(yarp::os::Searchable& config) m_controlBoardId = pluginParameters.find(YarpControlBoardScopedName).asString(); - m_controlBoardData - = ::gzyarp::ControlBoardDataSingleton::getControlBoardHandler()->getControlBoardData( - m_controlBoardId); - return true; } @@ -1734,6 +1731,13 @@ bool ControlBoardDriver::getEncodersTimed(double* encs, double* time) return true; } +// IControlBoardData + +void ControlBoardDriver::setControlBoardData(::gzyarp::ControlBoardData* controlBoardData) +{ + m_controlBoardData = controlBoardData; +} + } // namespace gzyarp } // namespace dev } // namespace yarp diff --git a/plugins/controlboard/singleton/ControlBoardTrajectory.cpp b/plugins/controlboard/src/ControlBoardTrajectory.cpp similarity index 100% rename from plugins/controlboard/singleton/ControlBoardTrajectory.cpp rename to plugins/controlboard/src/ControlBoardTrajectory.cpp diff --git a/tests/controlboard/CMakeLists.txt b/tests/controlboard/CMakeLists.txt index 60e0c0c..1e17830 100644 --- a/tests/controlboard/CMakeLists.txt +++ b/tests/controlboard/CMakeLists.txt @@ -19,13 +19,12 @@ foreach(TEST ${TESTS}) YARP::YARP_init gz-sim-yarp-commons gz-sim-yarp-device-registry - gz-sim-yarp-controlboard-singleton - ) + gz-sim-yarp-controlboard-system) add_test(NAME ${TEST} COMMAND ${TEST}) set_tests_properties(${TEST} PROPERTIES - ENVIRONMENT "GZ_SIM_SYSTEM_PLUGIN_PATH=$") + ENVIRONMENT "GZ_SIM_SYSTEM_PLUGIN_PATH=$") endforeach() diff --git a/tests/controlboard/ControlBoardCommonsTest.cc b/tests/controlboard/ControlBoardCommonsTest.cc index c286575..3a8bf60 100644 --- a/tests/controlboard/ControlBoardCommonsTest.cc +++ b/tests/controlboard/ControlBoardCommonsTest.cc @@ -1,20 +1,31 @@ -#include +#include "ControlBoardDriver.hh" +#include #include #include -#include +#include +#include +#include +#include +#include +#include #include +#include +#include +#include #include +#include #include #include -#include #include #include #include #include +#include +#include namespace gzyarp { @@ -48,30 +59,43 @@ TEST(ControlBoardCommonsTest, ConfigureMultipleControlBoards) { std::string modelSdfName = "coupled_pendulum_two_controlboards.sdf"; std::string sdfPath = std::string("../../../tests/controlboard/") + modelSdfName; - std::string deviceScopedName = "model/coupled_pendulum/controlboard_plugin_device"; - std::string cb1Key = "model/coupled_pendulum/controlboard_plugin_device"; - std::string cb2Key = "model/coupled_pendulum/controlboard_plugin_device2"; bool configured = false; - gz::sim::Entity modelEntity; - gz::sim::Model model; - yarp::dev::PolyDriver* driver; + std::vector deviceIds; + std::vector controlBoards; + gz::common::Console::SetVerbosity(4); 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*/) { + deviceIds = DeviceRegistry::getHandler()->getDevicesKeys(); + // print the device ids + for (auto& deviceId : deviceIds) + { + std::cerr << "Device id: " << deviceId << std::endl; + } + + // Get the controlboard devices + for (auto& deviceId : deviceIds) + { + auto deviceDriver = DeviceRegistry::getHandler()->getDevice(deviceId); + yarp::dev::gzyarp::ControlBoardDriver* cbDriver = nullptr; + auto viewOk = deviceDriver->view(cbDriver); + if (viewOk && cbDriver) + { + controlBoards.push_back(cbDriver); + } + } + }); testFixture.Finalize(); - auto singleton = ControlBoardDataSingleton::getControlBoardHandler(); - auto keys = singleton->getControlBoardKeys(); - - std::cerr << "ControlBoard singleton keys vector size: " << keys.size() << std::endl; - ASSERT_EQ(keys.size(), 2); - - std::cerr << "ControlBoard ids: " << keys[0] << ", " << keys[1] << std::endl; - ASSERT_TRUE(std::find(keys.begin(), keys.end(), cb1Key) != keys.end()); - ASSERT_TRUE(std::find(keys.begin(), keys.end(), cb2Key) != keys.end()); + std::cerr << "Number of registered control board devices: " << controlBoards.size() + << std::endl; + ASSERT_EQ(controlBoards.size(), 2); } // Check that joint position limits are read correctly from yarp configuration @@ -81,28 +105,20 @@ TEST(ControlBoardCommonsTest, JointPositionLimitsForMultipleJoints) std::string sdfPath = std::string("../../../tests/controlboard/") + modelSdfName; std::string deviceScopedName = "model/coupled_pendulum/controlboard_plugin_device"; yarp::dev::PolyDriver* driver; - yarp::dev::IControlLimits* iControlLimits; + yarp::dev::IControlLimits* iControlLimits = nullptr; + IControlBoardData* iControlBoardData = nullptr; - gz::sim::TestFixture testFixture{sdfPath}; gz::common::Console::SetVerbosity(4); - + gz::sim::TestFixture testFixture{sdfPath}; testFixture.Finalize(); - driver = gzyarp::DeviceRegistry::getHandler()->getDevice(deviceScopedName); + auto deviceIds = DeviceRegistry::getHandler()->getDevicesKeys(); + ASSERT_TRUE(deviceIds.size() == 1); + + driver = DeviceRegistry::getHandler()->getDevice(deviceIds[0]); ASSERT_TRUE(driver != nullptr); ASSERT_TRUE(driver->view(iControlLimits)); - auto singleton = ControlBoardDataSingleton::getControlBoardHandler(); - auto keys = singleton->getControlBoardKeys(); - - std::cerr << "ControlBoard singleton keys vector size: " << keys.size() << std::endl; - ASSERT_EQ(keys.size(), 1); - - std::cerr << "ControlBoard ids: " << keys[0] << std::endl; - ASSERT_TRUE(std::find(keys.begin(), keys.end(), deviceScopedName) != keys.end()); - - auto controlBoardData = singleton->getControlBoardData(deviceScopedName); - auto expectedJointMaxLimits = std::vector{200.0, 10.0}; auto expectedJointMinLimits = std::vector{-200.0, -10.0}; From c345e4cf30d89a92a37a45c3802db81fa877803a Mon Sep 17 00:00:00 2001 From: Alessandro Croci Date: Tue, 16 Apr 2024 09:47:25 +0200 Subject: [PATCH 13/25] - Update device Id generation using ECM pointer to discriminate between different simulations. - Update model used in unit test to a double pendulum (the two simulations outcomes diverge quickly) - Update unit test --- libraries/common/DeviceIdGenerator.cpp | 11 +- libraries/device-registry/DeviceRegistry.cc | 35 ++++- libraries/device-registry/DeviceRegistry.hh | 5 + .../ControlBoardOnMultipleGazeboInstances.cc | 46 ++++--- ...double_pendulum_multiple_gz_instances.sdf} | 123 ++++++++---------- 5 files changed, 126 insertions(+), 94 deletions(-) rename tests/controlboard/{coupled_pendulum_multiple_gz_instances.sdf => double_pendulum_multiple_gz_instances.sdf} (59%) diff --git a/libraries/common/DeviceIdGenerator.cpp b/libraries/common/DeviceIdGenerator.cpp index bb1a3a0..4ecb5a1 100644 --- a/libraries/common/DeviceIdGenerator.cpp +++ b/libraries/common/DeviceIdGenerator.cpp @@ -1,5 +1,10 @@ #include +#include +#include +#include +#include + #include #include #include @@ -14,7 +19,11 @@ std::string DeviceIdGenerator::generateDeviceId(const Entity& entity, const std::string& yarpDeviceName) { auto scopedName = gz::sim::scopedName(entity, ecm, "/"); - return scopedName + "/" + yarpDeviceName; + auto ecmPtr = &ecm; + std::stringstream ss; + ss << ecmPtr; + std::cerr << "==================== ecm address for generated id: " << ss.str() << std::endl; + return ss.str() + "/" + scopedName + "/" + yarpDeviceName; } } // namespace gzyarp diff --git a/libraries/device-registry/DeviceRegistry.cc b/libraries/device-registry/DeviceRegistry.cc index 42af8b7..912dab5 100644 --- a/libraries/device-registry/DeviceRegistry.cc +++ b/libraries/device-registry/DeviceRegistry.cc @@ -1,7 +1,11 @@ #include #include +#include +#include #include +#include +#include #include #include #include @@ -131,10 +135,33 @@ yarp::dev::PolyDriver* DeviceRegistry::getDevice(const std::string& deviceDataba std::vector DeviceRegistry::getDevicesKeys() const { - std::vector keys; - for (auto&& [key, value] : m_devicesMap) - keys.push_back(key); - return keys; + { + std::lock_guard lock(mutex()); + + std::vector keys; + for (auto&& [key, value] : m_devicesMap) + keys.push_back(key); + return keys; + } +} + +std::vector +DeviceRegistry::getDevicesKeys(const gz::sim::EntityComponentManager& ecm) const +{ + std::stringstream EcmPtrSs; + EcmPtrSs << &ecm; + + { + std::lock_guard lock(mutex()); + std::cerr << "==================== ecm address: " << EcmPtrSs.str() << std::endl; + std::vector keys; + for (auto&& [key, value] : m_devicesMap) + { + if (key.find(EcmPtrSs.str()) != std::string::npos) + keys.push_back(key); + } + return keys; + } } // Private methods diff --git a/libraries/device-registry/DeviceRegistry.hh b/libraries/device-registry/DeviceRegistry.hh index b4edd78..9bbc352 100644 --- a/libraries/device-registry/DeviceRegistry.hh +++ b/libraries/device-registry/DeviceRegistry.hh @@ -6,6 +6,8 @@ #include #include +#include + namespace gzyarp { @@ -27,6 +29,9 @@ public: std::vector getDevicesKeys() const; + // Return only the devices that belong to simulation having the ecm passed as argument + std::vector getDevicesKeys(const gz::sim::EntityComponentManager& ecm) const; + private: DeviceRegistry(); static DeviceRegistry* s_handle; diff --git a/tests/controlboard/ControlBoardOnMultipleGazeboInstances.cc b/tests/controlboard/ControlBoardOnMultipleGazeboInstances.cc index 733010f..6efe2e8 100644 --- a/tests/controlboard/ControlBoardOnMultipleGazeboInstances.cc +++ b/tests/controlboard/ControlBoardOnMultipleGazeboInstances.cc @@ -1,21 +1,29 @@ #include #include +#include +#include #include - -#include -#include +#include +#include #include +#include #include #include +#include +#include +#include +#include #include #include #include #include +#include #include #include #include +#include #include #include @@ -33,17 +41,16 @@ namespace test TEST(ControlBoardOnMultipleGazeboInstances, StartConcurrentGazeboInstances) { - std::string deviceScopedName = "model/coupled_pendulum/controlboard_plugin_device"; - auto plannedIterations = 100; + auto plannedIterations = 10'000; yarp::dev::PolyDriver* driver1; yarp::dev::PolyDriver* driver2; yarp::dev::IEncoders* iEncoders1 = nullptr; yarp::dev::IEncoders* iEncoders2 = nullptr; double jointPosition1{}, jointPosition2{}; gz::sim::TestFixture fixture1("../../../tests/controlboard/" - "coupled_pendulum_multiple_gz_instances.sdf"); + "double_pendulum_multiple_gz_instances.sdf"); gz::sim::TestFixture fixture2("../../../tests/controlboard/" - "coupled_pendulum_multiple_gz_instances.sdf"); + "double_pendulum_multiple_gz_instances.sdf"); gz::common::Console::SetVerbosity(4); fixture1 @@ -51,18 +58,16 @@ TEST(ControlBoardOnMultipleGazeboInstances, StartConcurrentGazeboInstances) const std::shared_ptr& /*_sdf*/, gz::sim::EntityComponentManager& _ecm, gz::sim::EventManager& /*_eventMgr*/) { - driver1 = gzyarp::DeviceRegistry::getHandler()->getDevice(deviceScopedName); + auto deviceIds = gzyarp::DeviceRegistry::getHandler()->getDevicesKeys(_ecm); + ASSERT_TRUE(deviceIds.size() == 1); + driver1 = gzyarp::DeviceRegistry::getHandler()->getDevice(deviceIds[0]); ASSERT_TRUE(driver1 != nullptr); iEncoders1 = nullptr; ASSERT_TRUE(driver1->view(iEncoders1)); }) - .OnPreUpdate( - [&](const gz::sim::UpdateInfo& _info, gz::sim::EntityComponentManager& _ecm) {}) .OnPostUpdate( [&](const gz::sim::UpdateInfo& _info, const gz::sim::EntityComponentManager& _ecm) { - std::cerr << "Iteration: " << _info.iterations << std::endl; iEncoders1->getEncoder(0, &jointPosition1); - std::cerr << "Joint position 1: " << jointPosition1 << std::endl; }) .Finalize(); @@ -71,18 +76,16 @@ TEST(ControlBoardOnMultipleGazeboInstances, StartConcurrentGazeboInstances) const std::shared_ptr& /*_sdf*/, gz::sim::EntityComponentManager& _ecm, gz::sim::EventManager& /*_eventMgr*/) { - driver2 = gzyarp::DeviceRegistry::getHandler()->getDevice(deviceScopedName); + auto deviceIds = gzyarp::DeviceRegistry::getHandler()->getDevicesKeys(_ecm); + ASSERT_TRUE(deviceIds.size() == 1); + driver2 = gzyarp::DeviceRegistry::getHandler()->getDevice(deviceIds[0]); ASSERT_TRUE(driver2 != nullptr); iEncoders2 = nullptr; ASSERT_TRUE(driver2->view(iEncoders2)); }) - .OnPreUpdate( - [&](const gz::sim::UpdateInfo& _info, gz::sim::EntityComponentManager& _ecm) {}) .OnPostUpdate( [&](const gz::sim::UpdateInfo& _info, const gz::sim::EntityComponentManager& _ecm) { - std::cerr << "Iteration: " << _info.iterations << std::endl; iEncoders2->getEncoder(0, &jointPosition2); - std::cerr << "Joint position 2: " << jointPosition2 << std::endl; }) .Finalize(); @@ -91,7 +94,8 @@ TEST(ControlBoardOnMultipleGazeboInstances, StartConcurrentGazeboInstances) while (fixture1.Server()->Running() || fixture2.Server()->Running()) { - std::this_thread::sleep_for(std::chrono::milliseconds(100)); + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + std::cerr << "Waiting for Gazebo simulation to finish..." << std::endl; } ASSERT_EQ(fixture1.Server()->IterationCount(), plannedIterations); @@ -99,6 +103,12 @@ TEST(ControlBoardOnMultipleGazeboInstances, StartConcurrentGazeboInstances) // Check that DeviceRegistry has two devices, one for each Gazebo instance ASSERT_EQ(gzyarp::DeviceRegistry::getHandler()->getDevicesKeys().size(), 2); + + // Print final joint positions + std::cerr << std::fixed << std::setprecision(6) + << "Final joint position simulation 1: " << jointPosition1 << std::endl; + std::cerr << std::fixed << std::setprecision(6) + << "Final joint position simulation 2: " << jointPosition2 << std::endl; } } // namespace test diff --git a/tests/controlboard/coupled_pendulum_multiple_gz_instances.sdf b/tests/controlboard/double_pendulum_multiple_gz_instances.sdf similarity index 59% rename from tests/controlboard/coupled_pendulum_multiple_gz_instances.sdf rename to tests/controlboard/double_pendulum_multiple_gz_instances.sdf index fa155d4..15468bf 100644 --- a/tests/controlboard/coupled_pendulum_multiple_gz_instances.sdf +++ b/tests/controlboard/double_pendulum_multiple_gz_instances.sdf @@ -45,25 +45,21 @@ - + + + world + base_link + 0 0 0 0 0 0 - 1000 - - 1 - 0 - 0 - 1 - 0 - 1 - + 8000 0 0 1 0 0 0 - 0.14999999999999999 0.14999999999999999 2.1499999999999999 + 0.15 0.15 2.15 @@ -71,89 +67,81 @@ 0 0 1 0 0 0 - 0.14999999999999999 0.14999999999999999 2.1499999999999999 + 0.15 0.15 2.15 - - 0.14999999999999999 0 1.75 3.1415000000000002 0 0 + + 0.15 0 2 -1.5708 0 0 base_link - link2 + link1 1 0 0 - - -5 - 5 - 100 - 100 - + 0.0 0 - 0 + 0.0 - - 0 0 0 0 0 0 + + 0 0 0 0 0 0 + 0 - 0 0 -0.90000000000000002 -0.30000000000000004 0 0 - 1 - - 1 - 0 - 0 - 1 - 0 - 1 - + 0 0 0.5 0 0 0 + 8000 - - 0 0 -0.5 0 0 0 + + 0 0 0.5 0 0 0 - 0.14999999999999999 0.14999999999999999 1 + 0.15 0.15 1.0 + + + + + 0 0 0.5 0 0 0 + + + 0.15 0.15 1.0 - - -0.14999999999999999 0 1.75 3.1415000000000002 0 0 - base_link - link1 + + 0.15 0 0.9 0 0 0 + link1 + link2 1 0 0 - - -5 - 5 - 100 - 100 - + 0.0 0 - 0 + 0.0 - - 0 0 0 0 0 0 + + 0 0 0 0 0 0 + 0 - 0 0 -0.90000000000000002 0 0 0 - 1 - - 1 - 0 - 0 - 1 - 0 - 1 - + 0 0 0.5 0 0 0 + 8000 - - 0 0 -0.5 0 0 0 + + 0 0 0.5 0 0 0 - 0.14999999999999999 0.14999999999999999 1 + 0.15 0.15 1.0 + + + + + 0 0 0.5 0 0 0 + + + 0.15 0.15 1.0 @@ -161,20 +149,13 @@ - ../../../tests/controlboard/conf/gazebo_controlboard_multiple_joints.ini + ../../../tests/controlboard/conf/gazebo_controlboard2.ini 0.0 - - - 0 0 0 0 0 0 - - - 0 0 0 0 0 0 - From a901e0f938f7a051df3e819bbe4575059346d826 Mon Sep 17 00:00:00 2001 From: Alessandro Croci Date: Wed, 17 Apr 2024 10:15:55 +0200 Subject: [PATCH 14/25] 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 From 6c3b5f94bac23163816f40000325945ce10be07b Mon Sep 17 00:00:00 2001 From: Alessandro Croci Date: Wed, 17 Apr 2024 12:10:48 +0200 Subject: [PATCH 15/25] Fix ControlBoardTorqueControlTest --- .../ControlBoardPositionDirectControlTest.cc | 10 ---------- .../controlboard/ControlBoardTorqueControlTest.cc | 15 +++++++++++++-- 2 files changed, 13 insertions(+), 12 deletions(-) diff --git a/tests/controlboard/ControlBoardPositionDirectControlTest.cc b/tests/controlboard/ControlBoardPositionDirectControlTest.cc index c21003b..aad8a62 100644 --- a/tests/controlboard/ControlBoardPositionDirectControlTest.cc +++ b/tests/controlboard/ControlBoardPositionDirectControlTest.cc @@ -68,10 +68,6 @@ class ControlBoardPositionDirectFixture : public ::testing::Test EXPECT_NE(gz::sim::kNullEntity, modelEntity); model = gz::sim::Model(modelEntity); - 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); @@ -162,13 +158,7 @@ TEST_F(ControlBoardPositionDirectFixture, CheckPositionTrackingUsingPendulumMode Finalize(); int 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. diff --git a/tests/controlboard/ControlBoardTorqueControlTest.cc b/tests/controlboard/ControlBoardTorqueControlTest.cc index bc16d16..7159e6d 100644 --- a/tests/controlboard/ControlBoardTorqueControlTest.cc +++ b/tests/controlboard/ControlBoardTorqueControlTest.cc @@ -1,18 +1,27 @@ #include +#include #include #include #include +#include #include +#include +#include +#include +#include +#include #include #include #include #include +#include #include #include #include +#include #include #include @@ -55,7 +64,10 @@ class ControlBoardTorqueControlFixture : public testing::TestWithParamgetDevice(deviceScopedName); + auto deviceKeys = gzyarp::DeviceRegistry::getHandler()->getDevicesKeys(); + ASSERT_EQ(deviceKeys.size(), 1); + driver = gzyarp::DeviceRegistry::getHandler()->getDevice(deviceKeys[0]); + ASSERT_TRUE(driver != nullptr); iTorqueControl = nullptr; ASSERT_TRUE(driver->view(iTorqueControl)); @@ -95,7 +107,6 @@ class ControlBoardTorqueControlFixture : public testing::TestWithParam Date: Wed, 17 Apr 2024 12:14:12 +0200 Subject: [PATCH 16/25] Fix ControlBoardPositionControlTest to use DeviceRegistry for device retrieval --- .../ControlBoardPositionControlTest.cc | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/tests/controlboard/ControlBoardPositionControlTest.cc b/tests/controlboard/ControlBoardPositionControlTest.cc index d122936..e1a6aed 100644 --- a/tests/controlboard/ControlBoardPositionControlTest.cc +++ b/tests/controlboard/ControlBoardPositionControlTest.cc @@ -6,15 +6,24 @@ #include #include #include +#include #include +#include +#include +#include +#include +#include +#include #include #include #include #include +#include #include #include #include +#include #include #include @@ -56,7 +65,10 @@ class ControlBoardPositionFixture : public testing::Test EXPECT_NE(gz::sim::kNullEntity, modelEntity); model = gz::sim::Model(modelEntity); - driver = gzyarp::DeviceRegistry::getHandler()->getDevice(deviceScopedName); + auto deviceKeys = gzyarp::DeviceRegistry::getHandler()->getDevicesKeys(); + ASSERT_EQ(deviceKeys.size(), 1); + driver = gzyarp::DeviceRegistry::getHandler()->getDevice(deviceKeys[0]); + ASSERT_TRUE(driver != nullptr); iPositionControl = nullptr; ASSERT_TRUE(driver->view(iPositionControl)); From ea4ca0657cbdbbecff919de17e00325917e6e032 Mon Sep 17 00:00:00 2001 From: Alessandro Croci Date: Wed, 17 Apr 2024 14:11:06 +0200 Subject: [PATCH 17/25] Add ConcurrentInstancesTest --- tests/commons/CMakeLists.txt | 17 ++++ tests/commons/ConcurrentInstancesTest.cc | 35 ++++++++ .../ConfigurationParsingFromFileTest.cc | 10 +-- tests/controlboard/CMakeLists.txt | 2 +- ...trolBoardOnMultipleGazeboInstancesTest.cc} | 2 +- tests/controlboard/dummy_box.sdf | 83 +++++++++++++++++++ tests/controlboard/dummy_sphere.sdf | 83 +++++++++++++++++++ .../pendulum_with_initial_configuration.sdf | 2 +- tests/test-helpers/TestHelpers.hh | 1 - 9 files changed, 226 insertions(+), 9 deletions(-) create mode 100644 tests/commons/ConcurrentInstancesTest.cc rename tests/controlboard/{ControlBoardOnMultipleGazeboInstances.cc => ControlBoardOnMultipleGazeboInstancesTest.cc} (99%) create mode 100644 tests/controlboard/dummy_box.sdf create mode 100644 tests/controlboard/dummy_sphere.sdf diff --git a/tests/commons/CMakeLists.txt b/tests/commons/CMakeLists.txt index 74772d1..ab9761c 100644 --- a/tests/commons/CMakeLists.txt +++ b/tests/commons/CMakeLists.txt @@ -1,5 +1,6 @@ add_executable(ConfigurationParsingFromFileTest ConfigurationParsingFromFileTest.cc) add_executable(ConfigurationParsingFromStringTest ConfigurationParsingFromStringTest.cc) +add_executable(ConcurrentInstancesTest ConcurrentInstancesTest.cc) target_link_libraries(ConfigurationParsingFromFileTest PRIVATE @@ -37,9 +38,22 @@ PRIVATE gz-sim-yarp-laser-system ) +target_link_libraries(ConcurrentInstancesTest +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 +) + # add_test(NAME ConfigurationParsingFromFileTest COMMAND ConfigurationParsingFromFileTest) add_test(NAME ConfigurationParsingFromStringTest COMMAND ConfigurationParsingFromStringTest) +add_test(NAME ConcurrentInstancesTest COMMAND ConcurrentInstancesTest) set(_env_vars) list(APPEND _env_vars "LIBGL_ALWAYS_SOFTWARE=1" @@ -52,3 +66,6 @@ list(APPEND _env_vars "LIBGL_ALWAYS_SOFTWARE=1" set_tests_properties(ConfigurationParsingFromStringTest PROPERTIES ENVIRONMENT "${_env_vars}") + +set_tests_properties(ConcurrentInstancesTest PROPERTIES + ENVIRONMENT "${_env_vars}") diff --git a/tests/commons/ConcurrentInstancesTest.cc b/tests/commons/ConcurrentInstancesTest.cc new file mode 100644 index 0000000..e8ea02c --- /dev/null +++ b/tests/commons/ConcurrentInstancesTest.cc @@ -0,0 +1,35 @@ +#include + +#include +#include +#include +#include + +#include +#include + +TEST(ConcurrentInstancesTest, StartConcurrentGazeboInstancesOfDifferentModels) +{ + auto plannedIterations = 1'000; + + gz::sim::TestFixture fixture1("../../../tests/controlboard/" + "dummy_sphere.sdf"); + gz::sim::TestFixture fixture2("../../../tests/controlboard/" + "dummy_box.sdf"); + gz::common::Console::SetVerbosity(4); + + fixture1.Finalize(); + fixture2.Finalize(); + + ASSERT_TRUE(fixture1.Server()->Run(false, plannedIterations, false)); + ASSERT_TRUE(fixture2.Server()->Run(false, plannedIterations, false)); + + while (fixture1.Server()->Running() || fixture2.Server()->Running()) + { + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + std::cerr << "Waiting for Gazebo simulation to finish..." << std::endl; + } + + ASSERT_EQ(fixture1.Server()->IterationCount(), plannedIterations); + ASSERT_EQ(fixture2.Server()->IterationCount(), plannedIterations); +} diff --git a/tests/commons/ConfigurationParsingFromFileTest.cc b/tests/commons/ConfigurationParsingFromFileTest.cc index b490317..5ca43fe 100644 --- a/tests/commons/ConfigurationParsingFromFileTest.cc +++ b/tests/commons/ConfigurationParsingFromFileTest.cc @@ -38,11 +38,11 @@ TEST(ConfigurationParsingTest, LoadPluginsWithYarpConfigurationFile) 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); + 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; diff --git a/tests/controlboard/CMakeLists.txt b/tests/controlboard/CMakeLists.txt index b78f38f..7517bdc 100644 --- a/tests/controlboard/CMakeLists.txt +++ b/tests/controlboard/CMakeLists.txt @@ -3,7 +3,7 @@ set(TESTS ControlBoardPositionDirectControlTest ControlBoardPositionControlTest ControlBoardCommonsTest - ControlBoardOnMultipleGazeboInstances + ControlBoardOnMultipleGazeboInstancesTest ) foreach(TEST ${TESTS}) diff --git a/tests/controlboard/ControlBoardOnMultipleGazeboInstances.cc b/tests/controlboard/ControlBoardOnMultipleGazeboInstancesTest.cc similarity index 99% rename from tests/controlboard/ControlBoardOnMultipleGazeboInstances.cc rename to tests/controlboard/ControlBoardOnMultipleGazeboInstancesTest.cc index 6efe2e8..7a45f16 100644 --- a/tests/controlboard/ControlBoardOnMultipleGazeboInstances.cc +++ b/tests/controlboard/ControlBoardOnMultipleGazeboInstancesTest.cc @@ -41,7 +41,7 @@ namespace test TEST(ControlBoardOnMultipleGazeboInstances, StartConcurrentGazeboInstances) { - auto plannedIterations = 10'000; + auto plannedIterations = 30'000; yarp::dev::PolyDriver* driver1; yarp::dev::PolyDriver* driver2; yarp::dev::IEncoders* iEncoders1 = nullptr; diff --git a/tests/controlboard/dummy_box.sdf b/tests/controlboard/dummy_box.sdf new file mode 100644 index 0000000..a3ea2ad --- /dev/null +++ b/tests/controlboard/dummy_box.sdf @@ -0,0 +1,83 @@ + + + + + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + 100 100 + + + + + 1.0 + + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + + + + + 0.15 0.15 0.15 + + + 0 0 3.0 0 0 0 + + + + + 0.15 0.15 0.15 + + + 0 0 3.0 0 0 0 + + + 1.0 + + + + + 1000 + + + + + diff --git a/tests/controlboard/dummy_sphere.sdf b/tests/controlboard/dummy_sphere.sdf new file mode 100644 index 0000000..d801534 --- /dev/null +++ b/tests/controlboard/dummy_sphere.sdf @@ -0,0 +1,83 @@ + + + + + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + 100 100 + + + + + 1.0 + + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + + + + + 0.15 + + + 0 0 3.0 0 0 0 + + + + + 0.15 + + + 0 0 3.0 0 0 0 + + + 1.0 + + + + + 1000 + + + + + diff --git a/tests/controlboard/pendulum_with_initial_configuration.sdf b/tests/controlboard/pendulum_with_initial_configuration.sdf index c2ebc13..6c211b4 100644 --- a/tests/controlboard/pendulum_with_initial_configuration.sdf +++ b/tests/controlboard/pendulum_with_initial_configuration.sdf @@ -136,7 +136,7 @@ ../../../tests/controlboard/conf/gazebo_controlboard.ini - 0.0 + 0.0 90.0 diff --git a/tests/test-helpers/TestHelpers.hh b/tests/test-helpers/TestHelpers.hh index 3295c89..e623cbd 100644 --- a/tests/test-helpers/TestHelpers.hh +++ b/tests/test-helpers/TestHelpers.hh @@ -15,7 +15,6 @@ public: { auto deviceIds = DeviceRegistry::getHandler()->getDevicesKeys(); std::vector devices{}; - // Get the controlboard devices for (auto& deviceId : deviceIds) { auto polyDriver = DeviceRegistry::getHandler()->getDevice(deviceId); From 2ae346a9a31d0d0e11f424179d6aaea0c3b2236c Mon Sep 17 00:00:00 2001 From: Alessandro Croci Date: Wed, 17 Apr 2024 14:45:33 +0200 Subject: [PATCH 18/25] Remove debug logs in DeviceIdGenerator.cpp --- libraries/common/DeviceIdGenerator.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/libraries/common/DeviceIdGenerator.cpp b/libraries/common/DeviceIdGenerator.cpp index 4ecb5a1..97f1cbb 100644 --- a/libraries/common/DeviceIdGenerator.cpp +++ b/libraries/common/DeviceIdGenerator.cpp @@ -22,7 +22,6 @@ std::string DeviceIdGenerator::generateDeviceId(const Entity& entity, auto ecmPtr = &ecm; std::stringstream ss; ss << ecmPtr; - std::cerr << "==================== ecm address for generated id: " << ss.str() << std::endl; return ss.str() + "/" + scopedName + "/" + yarpDeviceName; } From 44229257dfbee80610ec4ceba03e9946b297c122 Mon Sep 17 00:00:00 2001 From: Alessandro Croci Date: Wed, 17 Apr 2024 15:48:45 +0200 Subject: [PATCH 19/25] Update ControlBoardOnMultipleGazeboInstancesTest In order to be sure that the two simulations are actually distinct the joint is controlled in position and different references are given to the two simulations. --- ...ntrolBoardOnMultipleGazeboInstancesTest.cc | 64 ++++++++++++++++-- .../ControlBoardPositionControlTest.cc | 2 +- ...gazebo_controlboard_multiple_instances.ini | 34 ++++++++++ ...sdf => pendulum_multiple_gz_instances.sdf} | 66 ++++--------------- 4 files changed, 106 insertions(+), 60 deletions(-) create mode 100644 tests/controlboard/conf/gazebo_controlboard_multiple_instances.ini rename tests/controlboard/{double_pendulum_multiple_gz_instances.sdf => pendulum_multiple_gz_instances.sdf} (59%) diff --git a/tests/controlboard/ControlBoardOnMultipleGazeboInstancesTest.cc b/tests/controlboard/ControlBoardOnMultipleGazeboInstancesTest.cc index 7a45f16..28a050f 100644 --- a/tests/controlboard/ControlBoardOnMultipleGazeboInstancesTest.cc +++ b/tests/controlboard/ControlBoardOnMultipleGazeboInstancesTest.cc @@ -27,6 +27,7 @@ #include #include +#include #include #include #include @@ -41,16 +42,27 @@ namespace test TEST(ControlBoardOnMultipleGazeboInstances, StartConcurrentGazeboInstances) { - auto plannedIterations = 30'000; + auto plannedIterations = 10'000; yarp::dev::PolyDriver* driver1; yarp::dev::PolyDriver* driver2; yarp::dev::IEncoders* iEncoders1 = nullptr; yarp::dev::IEncoders* iEncoders2 = nullptr; + yarp::dev::IPositionControl* iPositionControl1 = nullptr; + yarp::dev::IPositionControl* iPositionControl2 = nullptr; + yarp::dev::IControlMode* iControlMode1 = nullptr; + yarp::dev::IControlMode* iControlMode2 = nullptr; double jointPosition1{}, jointPosition2{}; + double refPosition1 = 10; + double refPosition2 = 5; + bool motionDone1 = false; + bool motionDone2 = false; + unsigned int iterationsToCompleteMotion1 = 0; + unsigned int iterationsToCompleteMotion2 = 0; + gz::sim::TestFixture fixture1("../../../tests/controlboard/" - "double_pendulum_multiple_gz_instances.sdf"); + "pendulum_multiple_gz_instances.sdf"); gz::sim::TestFixture fixture2("../../../tests/controlboard/" - "double_pendulum_multiple_gz_instances.sdf"); + "pendulum_multiple_gz_instances.sdf"); gz::common::Console::SetVerbosity(4); fixture1 @@ -64,10 +76,22 @@ TEST(ControlBoardOnMultipleGazeboInstances, StartConcurrentGazeboInstances) ASSERT_TRUE(driver1 != nullptr); iEncoders1 = nullptr; ASSERT_TRUE(driver1->view(iEncoders1)); + iPositionControl1 = nullptr; + ASSERT_TRUE(driver1->view(iPositionControl1)); + iControlMode1 = nullptr; + ASSERT_TRUE(driver1->view(iControlMode1)); + + // Set joint in position control mode + ASSERT_TRUE(iControlMode1->setControlMode(0, VOCAB_CM_POSITION)); }) .OnPostUpdate( [&](const gz::sim::UpdateInfo& _info, const gz::sim::EntityComponentManager& _ecm) { iEncoders1->getEncoder(0, &jointPosition1); + iPositionControl1->checkMotionDone(0, &motionDone1); + if (motionDone1 && iterationsToCompleteMotion1 == 0) + { + iterationsToCompleteMotion1 = fixture1.Server()->IterationCount().value(); + } }) .Finalize(); @@ -82,32 +106,60 @@ TEST(ControlBoardOnMultipleGazeboInstances, StartConcurrentGazeboInstances) ASSERT_TRUE(driver2 != nullptr); iEncoders2 = nullptr; ASSERT_TRUE(driver2->view(iEncoders2)); + iPositionControl2 = nullptr; + ASSERT_TRUE(driver2->view(iPositionControl2)); + iControlMode2 = nullptr; + ASSERT_TRUE(driver2->view(iControlMode2)); + + // Set joint in position control mode + ASSERT_TRUE(iControlMode2->setControlMode(0, VOCAB_CM_POSITION)); }) .OnPostUpdate( [&](const gz::sim::UpdateInfo& _info, const gz::sim::EntityComponentManager& _ecm) { iEncoders2->getEncoder(0, &jointPosition2); + iPositionControl2->checkMotionDone(0, &motionDone2); + if (motionDone2 && iterationsToCompleteMotion2 == 0) + { + iterationsToCompleteMotion2 = fixture2.Server()->IterationCount().value(); + } }) .Finalize(); + // Set reference position + iPositionControl1->positionMove(0, refPosition1); + iPositionControl2->positionMove(0, refPosition2); + ASSERT_TRUE(fixture1.Server()->Run(false, plannedIterations, false)); ASSERT_TRUE(fixture2.Server()->Run(false, plannedIterations, false)); while (fixture1.Server()->Running() || fixture2.Server()->Running()) { - std::this_thread::sleep_for(std::chrono::milliseconds(1000)); std::cerr << "Waiting for Gazebo simulation to finish..." << std::endl; + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); } ASSERT_EQ(fixture1.Server()->IterationCount(), plannedIterations); ASSERT_EQ(fixture2.Server()->IterationCount(), plannedIterations); + if (motionDone1) + { + std::cerr << "Motion done simulation 1 in " << iterationsToCompleteMotion1 << " iterations" + << std::endl; + } + + if (motionDone2) + { + std::cerr << "Motion done simulation 2 in " << iterationsToCompleteMotion2 << " iterations" + << std::endl; + } + // Check that DeviceRegistry has two devices, one for each Gazebo instance ASSERT_EQ(gzyarp::DeviceRegistry::getHandler()->getDevicesKeys().size(), 2); // Print final joint positions - std::cerr << std::fixed << std::setprecision(6) + std::cerr << std::fixed << std::setprecision(10) << "Final joint position simulation 1: " << jointPosition1 << std::endl; - std::cerr << std::fixed << std::setprecision(6) + std::cerr << std::fixed << std::setprecision(10) << "Final joint position simulation 2: " << jointPosition2 << std::endl; } diff --git a/tests/controlboard/ControlBoardPositionControlTest.cc b/tests/controlboard/ControlBoardPositionControlTest.cc index e1a6aed..48bd7d9 100644 --- a/tests/controlboard/ControlBoardPositionControlTest.cc +++ b/tests/controlboard/ControlBoardPositionControlTest.cc @@ -82,7 +82,7 @@ class ControlBoardPositionFixture : public testing::Test EXPECT_NE(gz::sim::kNullEntity, jointEntity); joint = gz::sim::Joint(jointEntity); - // Set joint in torque control mode + // Set joint in position control mode ASSERT_TRUE(iControlMode->setControlMode(0, VOCAB_CM_POSITION)); // Print number of joint configured diff --git a/tests/controlboard/conf/gazebo_controlboard_multiple_instances.ini b/tests/controlboard/conf/gazebo_controlboard_multiple_instances.ini new file mode 100644 index 0000000..b55f6fc --- /dev/null +++ b/tests/controlboard/conf/gazebo_controlboard_multiple_instances.ini @@ -0,0 +1,34 @@ +yarpDeviceName controlboard_plugin_device2 +jointNames (joint1) + +#PIDs: +[POSITION_CONTROL] +controlUnits metric_units +controlLaw joint_pid_gazebo_v1 +kp (3000.0) +kd (2.0) +ki (0.1) +maxInt (99999) +maxOutput (99999) +shift (0.0) +ko (0.0) +stictionUp (0.0) +stictionDwn (0.0) + +[VELOCITY_CONTROL] +controlUnits metric_units +controlLaw joint_pid_gazebo_v1 +velocityControlImplementationType integrator_and_position_pid +kp (500.0) +kd (2.0) +ki (0.1) +maxInt (9999) +maxOutput (9999) +shift (0.0) +ko (0.0) +stictionUp (0.0) +stictionDwn (0.0) + +[LIMITS] +jntPosMax (10000.0) +jntPosMin (-10000.0) diff --git a/tests/controlboard/double_pendulum_multiple_gz_instances.sdf b/tests/controlboard/pendulum_multiple_gz_instances.sdf similarity index 59% rename from tests/controlboard/double_pendulum_multiple_gz_instances.sdf rename to tests/controlboard/pendulum_multiple_gz_instances.sdf index 15468bf..8e2c04f 100644 --- a/tests/controlboard/double_pendulum_multiple_gz_instances.sdf +++ b/tests/controlboard/pendulum_multiple_gz_instances.sdf @@ -45,7 +45,7 @@ - + world base_link @@ -56,24 +56,24 @@ 8000 - 0 0 1 0 0 0 + 0 0 1.5 0 0 0 - 0.15 0.15 2.15 + 0.15 0.15 3.0 - 0 0 1 0 0 0 + 0 0 1.5 0 0 0 - 0.15 0.15 2.15 + 0.15 0.15 3.0 - - 0.15 0 2 -1.5708 0 0 + + 0.15 0 3.0 -1.5708 0 0 base_link link1 @@ -86,62 +86,25 @@ - 0 0 0 0 0 0 + 0 0 0 0 0 0 0 0 0 0.5 0 0 0 8000 - + 0 0 0.5 0 0 0 - 0.15 0.15 1.0 + 0.15 0.15 1 - + 0 0 0.5 0 0 0 - 0.15 0.15 1.0 - - - - - - 0.15 0 0.9 0 0 0 - link1 - link2 - - 1 0 0 - - 0.0 - 0 - 0.0 - - - - - 0 0 0 0 0 0 - 0 - - 0 0 0.5 0 0 0 - 8000 - - - 0 0 0.5 0 0 0 - - - 0.15 0.15 1.0 - - - - - 0 0 0.5 0 0 0 - - - 0.15 0.15 1.0 + 0.15 0.15 1 @@ -149,13 +112,10 @@ - ../../../tests/controlboard/conf/gazebo_controlboard2.ini + ../../../tests/controlboard/conf/gazebo_controlboard_multiple_instances.ini 0.0 - From cd0cd2fb80cb727913a55f7dab560bfb04eec78b Mon Sep 17 00:00:00 2001 From: Alessandro Croci Date: Thu, 18 Apr 2024 21:03:06 +0200 Subject: [PATCH 20/25] Update DeviceRegistry interface and data structure Also: - adapt all plugins and tests to use the new interface - ConfigurationParsingFromFileTest unit test has still an error in the controlboard assertions, it has been commented out for now --- libraries/common/CMakeLists.txt | 2 +- libraries/common/DeviceIdGenerator.cpp | 28 -- libraries/common/DeviceIdGenerator.hh | 19 -- libraries/device-registry/DeviceRegistry.cc | 287 ++++++++++++------ libraries/device-registry/DeviceRegistry.hh | 41 ++- plugins/basestate/BaseState.cc | 13 +- plugins/camera/Camera.cc | 12 +- plugins/clock/Clock.cc | 10 +- plugins/controlboard/include/ControlBoard.hh | 1 + plugins/controlboard/src/ControlBoard.cpp | 22 +- plugins/forcetorque/ForceTorque.cc | 19 +- plugins/imu/Imu.cc | 13 +- plugins/laser/Laser.cc | 13 +- plugins/robotinterface/RobotInterface.cc | 12 +- tests/commons/CMakeLists.txt | 6 +- .../ConfigurationParsingFromFileTest.cc | 29 +- .../ConfigurationParsingFromStringTest.cc | 12 +- tests/controlboard/CMakeLists.txt | 8 +- tests/controlboard/ControlBoardCommonsTest.cc | 24 +- ...ntrolBoardOnMultipleGazeboInstancesTest.cc | 7 +- .../ControlBoardPositionControlTest.cc | 5 +- .../ControlBoardPositionDirectControlTest.cc | 4 +- .../ControlBoardTorqueControlTest.cc | 4 +- tests/forcetorque/ForceTorqueTest.cc | 10 + tests/imu/ImuTest.cc | 3 +- tests/laser/LaserTest.cc | 10 + tests/test-helpers/CMakeLists.txt | 2 +- tests/test-helpers/TestHelpers.hh | 33 +- 28 files changed, 405 insertions(+), 244 deletions(-) delete mode 100644 libraries/common/DeviceIdGenerator.cpp delete mode 100644 libraries/common/DeviceIdGenerator.hh diff --git a/libraries/common/CMakeLists.txt b/libraries/common/CMakeLists.txt index cd4f6e6..cdc0c0f 100644 --- a/libraries/common/CMakeLists.txt +++ b/libraries/common/CMakeLists.txt @@ -1,4 +1,4 @@ -add_library(gz-sim-yarp-commons SHARED Common.hh ConfigurationHelpers.hh ConfigurationHelpers.cpp DeviceIdGenerator.hh DeviceIdGenerator.cpp) +add_library(gz-sim-yarp-commons SHARED Common.hh ConfigurationHelpers.hh ConfigurationHelpers.cpp) target_include_directories(gz-sim-yarp-commons PUBLIC "$") diff --git a/libraries/common/DeviceIdGenerator.cpp b/libraries/common/DeviceIdGenerator.cpp deleted file mode 100644 index 97f1cbb..0000000 --- a/libraries/common/DeviceIdGenerator.cpp +++ /dev/null @@ -1,28 +0,0 @@ -#include - -#include -#include -#include -#include - -#include -#include -#include - -namespace gzyarp -{ -using gz::sim::Entity; -using gz::sim::EntityComponentManager; - -std::string DeviceIdGenerator::generateDeviceId(const Entity& entity, - const EntityComponentManager& ecm, - const std::string& yarpDeviceName) -{ - auto scopedName = gz::sim::scopedName(entity, ecm, "/"); - auto ecmPtr = &ecm; - std::stringstream ss; - ss << ecmPtr; - return ss.str() + "/" + scopedName + "/" + yarpDeviceName; -} - -} // namespace gzyarp diff --git a/libraries/common/DeviceIdGenerator.hh b/libraries/common/DeviceIdGenerator.hh deleted file mode 100644 index f89e080..0000000 --- a/libraries/common/DeviceIdGenerator.hh +++ /dev/null @@ -1,19 +0,0 @@ -#pragma once - -#include - -#include -#include - -namespace gzyarp -{ - -class DeviceIdGenerator -{ -public: - static std::string generateDeviceId(const gz::sim::Entity& entity, - const gz::sim::EntityComponentManager& ecm, - const std::string& yarpDeviceName); -}; - -} // namespace gzyarp diff --git a/libraries/device-registry/DeviceRegistry.cc b/libraries/device-registry/DeviceRegistry.cc index 7af6f09..0fb9365 100644 --- a/libraries/device-registry/DeviceRegistry.cc +++ b/libraries/device-registry/DeviceRegistry.cc @@ -1,7 +1,9 @@ #include #include +#include #include +#include #include #include #include @@ -32,151 +34,250 @@ DeviceRegistry* DeviceRegistry::getHandler() return s_handle; } -bool DeviceRegistry::getDevicesAsPolyDriverList( - const std::string& modelScopedName, - yarp::dev::PolyDriverList& list, - std::vector& deviceScopedNames /*, const std::string& worldName*/) +bool DeviceRegistry::getDevicesAsPolyDriverList(const gz::sim::EntityComponentManager& ecm, + const std::string& modelScopedName, + yarp::dev::PolyDriverList& list, + std::vector& deviceScopedNames) const { deviceScopedNames.resize(0); - list = yarp::dev::PolyDriverList(); - // This map contains only the yarpDeviceName that we actually added - // to the returned yarp::dev::PolyDriverList - std::unordered_map inserted_yarpDeviceName2deviceDatabaseKey; - - for (auto&& devicesMapElem : m_devicesMap) { - std::string deviceDatabaseKey = devicesMapElem.first; - std::string yarpDeviceName; + std::lock_guard lock(mutex()); - // If the deviceDatabaseKey starts with the modelScopedName (device spawned by model - // plugins), then it is eligible for insertion in the returned list - if ((deviceDatabaseKey.find(modelScopedName, 0) != std::string::npos)) + // Check if the the gz instance has been already added to the map + std::string gzInstanceId = getGzInstanceId(ecm); + if (auto gzInstance_it = m_devicesMap.find(gzInstanceId); + gzInstance_it == m_devicesMap.end()) { - // Extract yarpDeviceName from deviceDatabaseKey - yarpDeviceName = deviceDatabaseKey.substr(deviceDatabaseKey.find_last_of("/") + 1); + yError() << "Error in gzyarp::DeviceRegistry::getDevicesAsPolyDriverList: gz instance " + "not found"; + return false; + } - // Check if a device with the same yarpDeviceName was already inserted - auto got = inserted_yarpDeviceName2deviceDatabaseKey.find(yarpDeviceName); + auto& devicesMap = m_devicesMap.at(gzInstanceId); - // If not found, insert and continue - if (got == inserted_yarpDeviceName2deviceDatabaseKey.end()) - { - // If no name collision is found, insert and continue - inserted_yarpDeviceName2deviceDatabaseKey.insert( - {yarpDeviceName, deviceDatabaseKey}); - list.push(devicesMapElem.second, yarpDeviceName.c_str()); - deviceScopedNames.push_back(deviceDatabaseKey); - } else + for (auto&& [key, value] : devicesMap) + { + std::string deviceModelScopedName = getModelScopedName(key); + std::string yarpDeviceName = getYarpDeviceName(key); + + if (deviceModelScopedName == modelScopedName) { - // If a name collision is found, print a clear error and return - yError() << "gzyarp::DeviceRegistry robotinterface getDevicesAsPolyDriverList " - "error: "; - yError() << "two YARP devices with yarpDeviceName " << yarpDeviceName - << " found in model " << modelScopedName; - yError() << "First instance: " << got->second; - yError() << "Second instance: " << deviceDatabaseKey; - yError() << "Please eliminate or rename one of the two instances. "; - list = yarp::dev::PolyDriverList(); - deviceScopedNames.resize(0); - return false; + list.push(value, yarpDeviceName.c_str()); + deviceScopedNames.push_back(key); } } } return true; } -bool DeviceRegistry::setDevice(std::string deviceDatabaseKey, yarp::dev::PolyDriver* device2add) +bool DeviceRegistry::setDevice(const gz::sim::Entity& entity, + const gz::sim::EntityComponentManager& ecm, + const std::string& yarpDeviceName, + yarp::dev::PolyDriver* device2add, + std::string& generatedDeviceDatabaseKey) { bool ret = false; - DevicesMap::iterator device = m_devicesMap.find(deviceDatabaseKey); - if (device != m_devicesMap.end()) + generatedDeviceDatabaseKey = ""; + + if (!device2add) { - if (device->second == device2add) - ret = true; - else + yError() << "Error in gzyarp::DeviceRegistry::setDevice: device2add is nullptr"; + return false; + } + + { + std::lock_guard lock(mutex()); + + // Check if the the gz instance has been already added to the map + std::string gzInstanceId = getGzInstanceId(ecm); + if (auto gzInstance_it = m_devicesMap.find(gzInstanceId); + gzInstance_it == m_devicesMap.end()) { - yError() << " Error in gzyarp::DeviceRegistry while inserting a new yarp device " - "pointer!"; - yError() << " The name of the device is already present but the pointer does not match " - "with the one already registered!"; - yError() << " This should not happen, check the names are correct in your config file. " - "Fatal error."; + // If not, add it + m_devicesMap.insert( + std::pair>( + gzInstanceId, std::unordered_map{})); } - } else - { - // device does not exists. Add to map - if (!m_devicesMap - .insert( - std::pair(deviceDatabaseKey, device2add)) - .second) + + // Check if the device has been already added to the map for the gz instance + std::string deviceDatabaseKey = generateDeviceId(entity, ecm, yarpDeviceName); + auto device_it = m_devicesMap[gzInstanceId].find(deviceDatabaseKey); + if (device_it == m_devicesMap[gzInstanceId].end()) { - yError() << " Error in gzyarp::DeviceRegistry while inserting a new device pointer!"; - ret = false; - } else + // If not, add it + m_devicesMap[gzInstanceId].insert( + std::pair(deviceDatabaseKey, device2add)); + generatedDeviceDatabaseKey = deviceDatabaseKey; + ret = true; + } else + { + if (device_it->second == device2add) + { + generatedDeviceDatabaseKey = deviceDatabaseKey; + ret = true; + } else + { + + yError() << " Error in gzyarp::DeviceRegistry while inserting a new yarp " + "device " + "pointer!"; + yError() << " The name of the device is already present but the pointer does " + "not match " + "with the one already registered!"; + yError() << " This should not happen, check the names are correct in your " + "config file. " + "The device already in the map has model scoped name: " + << getModelScopedName(device_it->first) + << " and yarp device name: " << getYarpDeviceName(device_it->first) + << ". " + "The device you are trying to insert has model scoped name: " + << getModelScopedName(deviceDatabaseKey) + << " and yarp device name: " << getYarpDeviceName(deviceDatabaseKey) + << ". " + "Fatal error."; + ret = false; + } + } } + return ret; } -yarp::dev::PolyDriver* DeviceRegistry::getDevice(const std::string& deviceDatabaseKey) const +bool DeviceRegistry::getDevice(const gz::sim::EntityComponentManager& ecm, + const std::string& deviceDatabaseKey, + yarp::dev::PolyDriver*& driver) const { - yarp::dev::PolyDriver* tmp = NULL; + driver = nullptr; - DevicesMap::const_iterator device = m_devicesMap.find(deviceDatabaseKey); - if (device != m_devicesMap.end()) - tmp = device->second; - else - tmp = NULL; + { + std::lock_guard lock(mutex()); - return tmp; + // Check if the the gz instance exists the map + std::string gzInstanceId = getGzInstanceId(ecm); + if (auto gzInstance_it = m_devicesMap.find(gzInstanceId); + gzInstance_it == m_devicesMap.end()) + { + yError() << "Error in gzyarp::DeviceRegistry::getDevice: gz instance not found"; + return false; + } + + // Check if the device exists in the map + if (auto device_it = m_devicesMap.at(gzInstanceId).find(deviceDatabaseKey); + device_it == m_devicesMap.at(gzInstanceId).end()) + { + yError() << "Error in gzyarp::DeviceRegistry::getDevice: device not found"; + return false; + } + + driver = m_devicesMap.at(gzInstanceId).at(deviceDatabaseKey); + + if (!driver) + { + yError() << "Error in gzyarp::DeviceRegistry::getDevice: driver is " + "nullptr"; + return false; + } + } + + return true; } -std::vector DeviceRegistry::getDevicesKeys() const +std::vector +DeviceRegistry::getDevicesKeys(const gz::sim::EntityComponentManager& ecm) const { + std::vector keys{}; + { std::lock_guard lock(mutex()); - std::vector keys; - for (auto&& [key, value] : m_devicesMap) + // Check if the the gz instance exists the map + std::string gzInstanceId = getGzInstanceId(ecm); + if (auto gzInstance_it = m_devicesMap.find(gzInstanceId); + gzInstance_it == m_devicesMap.end()) + { + yError() << "Error in gzyarp::DeviceRegistry::getDevicesKeys: gz instance not found"; + return keys; + } + + for (auto&& [key, value] : m_devicesMap.at(gzInstanceId)) + { keys.push_back(key); - return keys; + } } + + return keys; } -std::vector -DeviceRegistry::getDevicesKeys(const gz::sim::EntityComponentManager& ecm) const +bool DeviceRegistry::removeDevice(const gz::sim::EntityComponentManager& ecm, + const std::string& deviceDatabaseKey) { - std::stringstream EcmPtrSs; - EcmPtrSs << &ecm; { std::lock_guard lock(mutex()); - std::vector keys; - for (auto&& [key, value] : m_devicesMap) + + // Check if the the gz instance exists the map + std::string gzInstanceId = getGzInstanceId(ecm); + if (auto gzInstance_it = m_devicesMap.find(gzInstanceId); + gzInstance_it == m_devicesMap.end()) + { + yError() << "Error in gzyarp::DeviceRegistry::getDevicesKeys: gz instance not found"; + return false; + } + + // Check if the device exists in the map + if (auto device_it = m_devicesMap.at(gzInstanceId).find(deviceDatabaseKey); + device_it == m_devicesMap.at(gzInstanceId).end()) { - if (key.find(EcmPtrSs.str()) != std::string::npos) - keys.push_back(key); + yError() << "Error in gzyarp::DeviceRegistry::getDevice: device not found"; + return false; + } else + { + if (!device_it->second->close()) + { + yError() << "Error in gzyarp::DeviceRegistry::removeDevice: device could not be " + "closed"; + return false; + } + + m_devicesMap.at(gzInstanceId).erase(deviceDatabaseKey); } - return keys; } + return true; } // Private methods -void DeviceRegistry::removeDevice(const std::string& deviceDatabaseKey) +std::string DeviceRegistry::generateDeviceId(const gz::sim::Entity& entity, + const gz::sim::EntityComponentManager& ecm, + const std::string& yarpDeviceName) { - DevicesMap::iterator device = m_devicesMap.find(deviceDatabaseKey); - if (device != m_devicesMap.end()) - { - device->second->close(); - m_devicesMap.erase(device); - } else - { - yError() << "Could not remove device " << deviceDatabaseKey << ". Device was not found"; - } - return; + auto scopedName = gz::sim::scopedName(entity, ecm, "/"); + return scopedName + "/" + yarpDeviceName; +} + +std::string DeviceRegistry::getGzInstanceId(const gz::sim::EntityComponentManager& ecm) +{ + auto ecmPtr = &ecm; + std::stringstream ss; + ss << ecmPtr; + return ss.str(); +} + +std::string DeviceRegistry::getYarpDeviceName(const std::string& deviceDatabaseKey) +{ + // Extract yarpDeviceName from deviceDatabaseKey + std::string yarpDeviceName = deviceDatabaseKey.substr(deviceDatabaseKey.find_last_of("/") + 1); + return yarpDeviceName; +} + +std::string DeviceRegistry::getModelScopedName(const std::string& deviceDatabaseKey) +{ + // Extract modelScopedName from deviceDatabaseKey + std::string modelScopedName = deviceDatabaseKey.substr(0, deviceDatabaseKey.find_last_of("/")); + return modelScopedName; } DeviceRegistry::DeviceRegistry() diff --git a/libraries/device-registry/DeviceRegistry.hh b/libraries/device-registry/DeviceRegistry.hh index b931dc5..bf03ae3 100644 --- a/libraries/device-registry/DeviceRegistry.hh +++ b/libraries/device-registry/DeviceRegistry.hh @@ -1,8 +1,9 @@ #pragma once -#include +#include #include #include +#include #include #include @@ -18,28 +19,42 @@ class DeviceRegistry public: static DeviceRegistry* getHandler(); - bool getDevicesAsPolyDriverList( - const std::string& modelScopedName, - yarp::dev::PolyDriverList& list, - std::vector& deviceScopedNames /*, const std::string& worldName*/); + bool getDevicesAsPolyDriverList(const gz::sim::EntityComponentManager& ecm, + const std::string& modelScopedName, + yarp::dev::PolyDriverList& list, + std::vector& deviceScopedNames) const; - bool setDevice(std::string deviceDatabaseKey, yarp::dev::PolyDriver* device2add); + bool setDevice(const gz::sim::Entity& entity, + const gz::sim::EntityComponentManager& ecm, + const std::string& yarpDeviceName, + yarp::dev::PolyDriver* device2add, + std::string& generatedDeviceDatabaseKey); - yarp::dev::PolyDriver* getDevice(const std::string& deviceDatabaseKey) const; + bool getDevice(const gz::sim::EntityComponentManager& ecm, + const std::string& deviceDatabaseKey, + yarp::dev::PolyDriver*& driver) const; - void removeDevice(const std::string& deviceDatabaseKey); + bool + removeDevice(const gz::sim::EntityComponentManager& ecm, const std::string& deviceDatabaseKey); - std::vector getDevicesKeys() const; - - // Return only the devices that belong to simulation having the ecm passed as argument std::vector getDevicesKeys(const gz::sim::EntityComponentManager& ecm) const; private: + static std::string generateDeviceId(const gz::sim::Entity& entity, + const gz::sim::EntityComponentManager& ecm, + const std::string& yarpDeviceName); + + static std::string getGzInstanceId(const gz::sim::EntityComponentManager& ecm); + + static std::string getYarpDeviceName(const std::string& deviceDatabaseKey); + + static std::string getModelScopedName(const std::string& deviceDatabaseKey); + DeviceRegistry(); static DeviceRegistry* s_handle; static std::mutex& mutex(); - typedef std::map DevicesMap; - DevicesMap m_devicesMap; // map of known yarp devices + std::unordered_map> + m_devicesMap; // map of known yarp devices }; } // namespace gzyarp diff --git a/plugins/basestate/BaseState.cc b/plugins/basestate/BaseState.cc index 9e9d90e..3be3348 100644 --- a/plugins/basestate/BaseState.cc +++ b/plugins/basestate/BaseState.cc @@ -1,7 +1,6 @@ #include #include #include -#include #include #include @@ -52,7 +51,7 @@ class BaseState : public System, public ISystemConfigure, public ISystemPostUpda { if (m_deviceRegistered) { - DeviceRegistry::getHandler()->removeDevice(m_deviceId); + DeviceRegistry::getHandler()->removeDevice(*m_ecm, m_deviceId); m_deviceRegistered = false; } @@ -70,6 +69,8 @@ class BaseState : public System, public ISystemConfigure, public ISystemPostUpda std::string netWrapper = "analogServer"; + m_ecm = &_ecm; + using BaseStateDriverCreator = ::yarp::dev::DriverCreatorOf<::yarp::dev::gzyarp::BaseStateDriver>; ::yarp::dev::Drivers::factory().add( @@ -97,7 +98,7 @@ class BaseState : public System, public ISystemConfigure, public ISystemPostUpda return; } - std::string deviceName = driver_properties.find("yarpDeviceName").asString(); + std::string yarpDeviceName = driver_properties.find("yarpDeviceName").asString(); std::string baseLinkName = driver_properties.find("baseLink").asString(); auto model = Model(_entity); @@ -148,9 +149,8 @@ class BaseState : public System, public ISystemConfigure, public ISystemPostUpda } iBaseStateData->setBaseStateData(&m_baseStateData); - m_deviceId = DeviceIdGenerator::generateDeviceId(m_baseLinkEntity, _ecm, deviceName); - - if (!DeviceRegistry::getHandler()->setDevice(m_deviceId, &m_baseStateDriver)) + if (!DeviceRegistry::getHandler() + ->setDevice(_entity, _ecm, yarpDeviceName, &m_baseStateDriver, m_deviceId)) { yError() << "gz-sim-yarp-basestate-system: failed setting scopedDeviceName(=" << m_deviceId << ")"; @@ -256,6 +256,7 @@ class BaseState : public System, public ISystemConfigure, public ISystemPostUpda yarp::dev::PolyDriver m_baseStateDriver; BaseStateData m_baseStateData; yarp::os::Network m_yarpNetwork; + EntityComponentManager* m_ecm; }; } // namespace gzyarp diff --git a/plugins/camera/Camera.cc b/plugins/camera/Camera.cc index c213ac8..a147fed 100644 --- a/plugins/camera/Camera.cc +++ b/plugins/camera/Camera.cc @@ -1,7 +1,6 @@ #include #include #include -#include #include #include @@ -60,7 +59,7 @@ class Camera : public System, { if (m_deviceRegistered) { - DeviceRegistry::getHandler()->removeDevice(m_deviceId); + DeviceRegistry::getHandler()->removeDevice(*ecm, m_deviceId); m_deviceRegistered = false; } @@ -83,6 +82,8 @@ class Camera : public System, return; } + ecm = &_ecm; + ::yarp::dev::Drivers::factory().add( new ::yarp::dev::DriverCreatorOf<::yarp::dev::gzyarp::CameraDriver>("gazebo_camera", "grabber", @@ -162,10 +163,10 @@ class Camera : public System, return; } - auto deviceName = driver_properties.find("yarpDeviceName").asString(); - m_deviceId = DeviceIdGenerator::generateDeviceId(sensor, _ecm, deviceName); + auto yarpDeviceName = driver_properties.find("yarpDeviceName").asString(); - if (!DeviceRegistry::getHandler()->setDevice(m_deviceId, &m_cameraDriver)) + if (!DeviceRegistry::getHandler() + ->setDevice(_entity, _ecm, yarpDeviceName, &m_cameraDriver, m_deviceId)) { yError() << "gz-sim-yarp-camera-system: failed setting scopedDeviceName(=" << m_deviceId << ")"; @@ -222,6 +223,7 @@ class Camera : public System, gz::msgs::Image cameraMsg; std::mutex cameraMsgMutex; yarp::dev::IFrameGrabberImage* iFrameGrabberImage; + EntityComponentManager* ecm; }; } // namespace gzyarp diff --git a/plugins/clock/Clock.cc b/plugins/clock/Clock.cc index d984690..446fb45 100644 --- a/plugins/clock/Clock.cc +++ b/plugins/clock/Clock.cc @@ -1,7 +1,15 @@ #include +#include +#include + #include +#include +#include +#include #include +#include #include +#include #include #include @@ -16,8 +24,6 @@ using namespace systems; using yarp::os::Bottle; using yarp::os::BufferedPort; -using yarp::os::Log; -using yarp::os::Network; namespace gzyarp { diff --git a/plugins/controlboard/include/ControlBoard.hh b/plugins/controlboard/include/ControlBoard.hh index 5c87788..372a8a1 100644 --- a/plugins/controlboard/include/ControlBoard.hh +++ b/plugins/controlboard/include/ControlBoard.hh @@ -63,6 +63,7 @@ private: ControlBoardData m_controlBoardData; yarp::os::Network m_yarpNetwork; yarp::os::Property m_pluginParameters; + gz::sim::EntityComponentManager* m_ecm; enum class AngleUnitEnum { diff --git a/plugins/controlboard/src/ControlBoard.cpp b/plugins/controlboard/src/ControlBoard.cpp index a7047eb..6cb5c07 100644 --- a/plugins/controlboard/src/ControlBoard.cpp +++ b/plugins/controlboard/src/ControlBoard.cpp @@ -5,7 +5,6 @@ #include #include #include -#include #include #include @@ -63,7 +62,7 @@ ControlBoard::~ControlBoard() { if (m_deviceRegistered) { - DeviceRegistry::getHandler()->removeDevice(m_deviceId); + DeviceRegistry::getHandler()->removeDevice(*m_ecm, m_deviceId); m_deviceRegistered = false; } @@ -85,6 +84,8 @@ void ControlBoard::Configure(const Entity& _entity, bool wipe = false; + m_ecm = &_ecm; + if (ConfigurationHelpers::loadPluginConfiguration(_sdf, m_pluginParameters)) { if (!m_pluginParameters.check("yarpDeviceName")) @@ -101,23 +102,21 @@ void ControlBoard::Configure(const Entity& _entity, return; } - std::string deviceName = m_pluginParameters.find("yarpDeviceName").asString(); + std::string yarpDeviceName = m_pluginParameters.find("yarpDeviceName").asString(); m_robotScopedName = gz::sim::scopedName(_entity, _ecm, "/"); yDebug() << "gz-sim-yarp-controlboard-system : robot scoped name: " << m_robotScopedName; - - m_deviceId = DeviceIdGenerator::generateDeviceId(_entity, _ecm, deviceName); - yDebug() << "gz-sim-yarp-controlboard-system : deviceId: " << m_deviceId; + yDebug() << "gz-sim-yarp-controlboard-system : yarpDeviceName: " << yarpDeviceName; m_modelEntity = _entity; - m_controlBoardData.controlBoardId = m_deviceId; + // m_controlBoardData.controlBoardId = m_deviceId; m_pluginParameters.put(yarp::dev::gzyarp::YarpControlBoardScopedName.c_str(), m_robotScopedName.c_str()); m_pluginParameters.put("device", "gazebo_controlboard"); - m_pluginParameters.put("controlBoardId", m_deviceId); + // m_pluginParameters.put("controlBoardId", m_deviceId); if (_sdf->HasElement("initialConfiguration")) { @@ -134,10 +133,11 @@ void ControlBoard::Configure(const Entity& _entity, return; } - if (!DeviceRegistry::getHandler()->setDevice(m_deviceId, &m_controlBoardDriver)) + if (!DeviceRegistry::getHandler() + ->setDevice(_entity, _ecm, yarpDeviceName, &m_controlBoardDriver, m_deviceId)) { - yError() << "gz-sim-yarp-basestate-system: failed setting scopedDeviceName(=" << m_deviceId - << ")"; + yError() << "gz-sim-yarp-basestate-system: failed setting scopedDeviceName(=" + << m_robotScopedName << ")"; return; } diff --git a/plugins/forcetorque/ForceTorque.cc b/plugins/forcetorque/ForceTorque.cc index d448027..0864a5b 100644 --- a/plugins/forcetorque/ForceTorque.cc +++ b/plugins/forcetorque/ForceTorque.cc @@ -1,5 +1,5 @@ #include -#include +#include #include #include @@ -52,7 +52,7 @@ class ForceTorque : public System, { if (m_deviceRegistered) { - DeviceRegistry::getHandler()->removeDevice(m_deviceId); + DeviceRegistry::getHandler()->removeDevice(*ecm, m_deviceId); m_deviceRegistered = false; } @@ -73,6 +73,8 @@ class ForceTorque : public System, return; } + ecm = &_ecm; + std::string netWrapper = "analogServer"; ::yarp::dev::Drivers::factory().add( new ::yarp::dev::DriverCreatorOf<::yarp::dev::gzyarp::ForceTorqueDriver>("gazebo_" @@ -140,12 +142,14 @@ class ForceTorque : public System, } ftData->setForceTorqueData(&forceTorqueData); - auto deviceName = driver_properties.find("yarpDeviceName").asString(); - m_deviceId = DeviceIdGenerator::generateDeviceId(sensor, _ecm, deviceName); - if (!DeviceRegistry::getHandler()->setDevice(m_deviceId, &m_forceTorqueDriver)) + auto yarpDeviceName = driver_properties.find("yarpDeviceName").asString(); + + if (!DeviceRegistry::getHandler() + ->setDevice(_entity, _ecm, yarpDeviceName, &m_forceTorqueDriver, m_deviceId)) { - yError() << "gz-sim-yarp-forcetorque-system: failed setting scopedDeviceName(=" - << m_deviceId << ")"; + yError() << "gz-sim-yarp-forcetorque-system: failed setting device with " + "scopedDeviceName(=" + << m_deviceId << ") into DeviceRegistry"; return; } m_deviceRegistered = true; @@ -197,6 +201,7 @@ class ForceTorque : public System, gz::transport::Node node; gz::msgs::Wrench ftMsg; std::mutex ftMsgMutex; + EntityComponentManager* ecm; }; } // namespace gzyarp diff --git a/plugins/imu/Imu.cc b/plugins/imu/Imu.cc index b950463..2f90224 100644 --- a/plugins/imu/Imu.cc +++ b/plugins/imu/Imu.cc @@ -1,5 +1,5 @@ #include -#include +#include #include #include @@ -56,7 +56,7 @@ class Imu : public System, { if (m_deviceRegistered) { - DeviceRegistry::getHandler()->removeDevice(m_deviceId); + DeviceRegistry::getHandler()->removeDevice(*ecm, m_deviceId); m_deviceRegistered = false; } @@ -78,6 +78,8 @@ class Imu : public System, ::yarp::os::Property driver_properties; + ecm = &_ecm; + if (ConfigurationHelpers::loadPluginConfiguration(_sdf, driver_properties)) { if (!driver_properties.check("sensorName")) @@ -134,10 +136,10 @@ class Imu : public System, } iImuDataPtr->setImuData(&imuData); - auto deviceName = driver_properties.find("yarpDeviceName").asString(); - m_deviceId = DeviceIdGenerator::generateDeviceId(sensor, _ecm, deviceName); + auto yarpDeviceName = driver_properties.find("yarpDeviceName").asString(); - if (!DeviceRegistry::getHandler()->setDevice(m_deviceId, &m_imuDriver)) + if (!DeviceRegistry::getHandler() + ->setDevice(_entity, _ecm, yarpDeviceName, &m_imuDriver, m_deviceId)) { yError() << "gz-sim-yarp-imu-system: failed setting scopedDeviceName(=" << m_deviceId << ")"; @@ -202,6 +204,7 @@ class Imu : public System, gz::msgs::IMU imuMsg; std::mutex imuMsgMutex; yarp::os::Network m_yarpNetwork; + EntityComponentManager* ecm; }; } // namespace gzyarp diff --git a/plugins/laser/Laser.cc b/plugins/laser/Laser.cc index 046fc5e..ba06542 100644 --- a/plugins/laser/Laser.cc +++ b/plugins/laser/Laser.cc @@ -1,5 +1,5 @@ #include -#include +#include #include #include @@ -58,7 +58,7 @@ class Laser : public System, { if (m_deviceRegistered) { - DeviceRegistry::getHandler()->removeDevice(m_deviceId); + DeviceRegistry::getHandler()->removeDevice(*ecm, m_deviceId); m_deviceRegistered = false; } @@ -85,6 +85,8 @@ class Laser : public System, "LaserDriver")); ::yarp::os::Property driver_properties; + ecm = &_ecm; + if (ConfigurationHelpers::loadPluginConfiguration(_sdf, driver_properties)) { if (!driver_properties.check("sensorName")) @@ -141,10 +143,10 @@ class Laser : public System, } iLaserData->setLaserData(&laserData); - auto deviceName = driver_properties.find("yarpDeviceName").asString(); - m_deviceId = DeviceIdGenerator::generateDeviceId(sensor, _ecm, deviceName); + auto yarpDeviceNamee = driver_properties.find("yarpDeviceName").asString(); - if (!DeviceRegistry::getHandler()->setDevice(m_deviceId, &m_laserDriver)) + if (!DeviceRegistry::getHandler() + ->setDevice(_entity, _ecm, yarpDeviceNamee, &m_laserDriver, m_deviceId)) { yError() << "gz-sim-yarp-laser-system: failed setting scopedDeviceName(=" << m_deviceId << ")"; @@ -200,6 +202,7 @@ class Laser : public System, gz::transport::Node node; gz::msgs::LaserScan laserMsg; std::mutex laserMsgMutex; + EntityComponentManager* ecm; }; } // namespace gzyarp diff --git a/plugins/robotinterface/RobotInterface.cc b/plugins/robotinterface/RobotInterface.cc index b9b8fb1..4fc2b23 100644 --- a/plugins/robotinterface/RobotInterface.cc +++ b/plugins/robotinterface/RobotInterface.cc @@ -1,7 +1,15 @@ #include #include +#include +#include +#include +#include + #include +#include +#include +#include #include #include #include @@ -11,6 +19,7 @@ #include #include #include +#include #include using namespace gz; @@ -80,7 +89,8 @@ class RobotInterface : public System, public ISystemConfigure yarp::dev::PolyDriverList externalDriverList; - DeviceRegistry::getHandler()->getDevicesAsPolyDriverList(scopedName(model.Entity(), _ecm), + DeviceRegistry::getHandler()->getDevicesAsPolyDriverList(_ecm, + scopedName(model.Entity(), _ecm), externalDriverList, m_deviceScopedNames); diff --git a/tests/commons/CMakeLists.txt b/tests/commons/CMakeLists.txt index ab9761c..48485ed 100644 --- a/tests/commons/CMakeLists.txt +++ b/tests/commons/CMakeLists.txt @@ -51,7 +51,7 @@ PRIVATE ) -# add_test(NAME ConfigurationParsingFromFileTest COMMAND ConfigurationParsingFromFileTest) +add_test(NAME ConfigurationParsingFromFileTest COMMAND ConfigurationParsingFromFileTest) add_test(NAME ConfigurationParsingFromStringTest COMMAND ConfigurationParsingFromStringTest) add_test(NAME ConcurrentInstancesTest COMMAND ConcurrentInstancesTest) @@ -61,8 +61,8 @@ list(APPEND _env_vars "LIBGL_ALWAYS_SOFTWARE=1" "GZ_SIM_RESOURCE_PATH=${GZ_SIM_RESOURCE_PATH}:${CMAKE_CURRENT_SOURCE_DIR}/.." ) -# set_tests_properties(ConfigurationParsingFromFileTest PROPERTIES -# ENVIRONMENT "${_env_vars}") +set_tests_properties(ConfigurationParsingFromFileTest PROPERTIES + ENVIRONMENT "${_env_vars}") set_tests_properties(ConfigurationParsingFromStringTest PROPERTIES ENVIRONMENT "${_env_vars}") diff --git a/tests/commons/ConfigurationParsingFromFileTest.cc b/tests/commons/ConfigurationParsingFromFileTest.cc index 5ca43fe..f02488a 100644 --- a/tests/commons/ConfigurationParsingFromFileTest.cc +++ b/tests/commons/ConfigurationParsingFromFileTest.cc @@ -20,6 +20,7 @@ #include #include +#include #include #include #include @@ -37,47 +38,51 @@ TEST(ConfigurationParsingTest, LoadPluginsWithYarpConfigurationFile) const std::shared_ptr& /*_sdf*/, gz::sim::EntityComponentManager& _ecm, gz::sim::EventManager& /*_eventMgr*/) { + // Test controlboard --> not working + // std::cerr << "Testing ControlBoard configuration" << std::endl; + // std::vector cbDrivers + // = + // gzyarp::testing::TestHelpers::getDevicesOfType( + // _ecm); + // EXPECT_EQ(cbDrivers.size(), 1); + // EXPECT_TRUE(cbDrivers[0] != nullptr); + // Test Camera std::cerr << "Testing Camera configuration" << std::endl; auto cameraDrivers - = gzyarp::testing::TestHelpers::getDevicesOfType(); + = gzyarp::testing::TestHelpers::getDevicesOfType(_ecm); 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(); + = gzyarp::testing::TestHelpers::getDevicesOfType( + _ecm); 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(); + = gzyarp::testing::TestHelpers::getDevicesOfType(_ecm); 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(); + = gzyarp::testing::TestHelpers::getDevicesOfType(_ecm); 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(); + = gzyarp::testing::TestHelpers::getDevicesOfType( + _ecm); 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 index c5894de..62f94b4 100644 --- a/tests/commons/ConfigurationParsingFromStringTest.cc +++ b/tests/commons/ConfigurationParsingFromStringTest.cc @@ -40,35 +40,37 @@ TEST(ConfigurationParsingTest, LoadPluginsWithYarpConfigurationString) // Test Camera std::cerr << "Testing Camera configuration" << std::endl; auto cameraDrivers - = gzyarp::testing::TestHelpers::getDevicesOfType(); + = gzyarp::testing::TestHelpers::getDevicesOfType(_ecm); 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(); + = gzyarp::testing::TestHelpers::getDevicesOfType( + _ecm); 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(); + = gzyarp::testing::TestHelpers::getDevicesOfType(_ecm); 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(); + = gzyarp::testing::TestHelpers::getDevicesOfType(_ecm); 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(); + = gzyarp::testing::TestHelpers::getDevicesOfType( + _ecm); EXPECT_EQ(bsDrivers.size(), 1); EXPECT_TRUE(bsDrivers[0] != nullptr); diff --git a/tests/controlboard/CMakeLists.txt b/tests/controlboard/CMakeLists.txt index 7517bdc..97f3a17 100644 --- a/tests/controlboard/CMakeLists.txt +++ b/tests/controlboard/CMakeLists.txt @@ -6,6 +6,12 @@ set(TESTS ControlBoardOnMultipleGazeboInstancesTest ) +set(_env_vars) +list(APPEND _env_vars "LIBGL_ALWAYS_SOFTWARE=1" + "GZ_SIM_SYSTEM_PLUGIN_PATH=$" + "GZ_SIM_RESOURCE_PATH=${GZ_SIM_RESOURCE_PATH}:${CMAKE_CURRENT_SOURCE_DIR}/.." +) + foreach(TEST ${TESTS}) add_executable(${TEST} ${TEST}.cc) @@ -26,6 +32,6 @@ foreach(TEST ${TESTS}) COMMAND ${TEST}) set_tests_properties(${TEST} PROPERTIES - ENVIRONMENT "GZ_SIM_SYSTEM_PLUGIN_PATH=$") + ENVIRONMENT "${_env_vars}") endforeach() diff --git a/tests/controlboard/ControlBoardCommonsTest.cc b/tests/controlboard/ControlBoardCommonsTest.cc index 3a8bf60..f294ba7 100644 --- a/tests/controlboard/ControlBoardCommonsTest.cc +++ b/tests/controlboard/ControlBoardCommonsTest.cc @@ -71,7 +71,7 @@ TEST(ControlBoardCommonsTest, ConfigureMultipleControlBoards) const std::shared_ptr& /*_sdf*/, gz::sim::EntityComponentManager& _ecm, gz::sim::EventManager& /*_eventMgr*/) { - deviceIds = DeviceRegistry::getHandler()->getDevicesKeys(); + deviceIds = DeviceRegistry::getHandler()->getDevicesKeys(_ecm); // print the device ids for (auto& deviceId : deviceIds) { @@ -81,7 +81,8 @@ TEST(ControlBoardCommonsTest, ConfigureMultipleControlBoards) // Get the controlboard devices for (auto& deviceId : deviceIds) { - auto deviceDriver = DeviceRegistry::getHandler()->getDevice(deviceId); + yarp::dev::PolyDriver* deviceDriver = nullptr; + EXPECT_TRUE(DeviceRegistry::getHandler()->getDevice(_ecm, deviceId, deviceDriver)); yarp::dev::gzyarp::ControlBoardDriver* cbDriver = nullptr; auto viewOk = deviceDriver->view(cbDriver); if (viewOk && cbDriver) @@ -110,14 +111,21 @@ TEST(ControlBoardCommonsTest, JointPositionLimitsForMultipleJoints) gz::common::Console::SetVerbosity(4); gz::sim::TestFixture testFixture{sdfPath}; - testFixture.Finalize(); - auto deviceIds = DeviceRegistry::getHandler()->getDevicesKeys(); - ASSERT_TRUE(deviceIds.size() == 1); + testFixture.OnConfigure([&](const gz::sim::Entity& _worldEntity, + const std::shared_ptr& /*_sdf*/, + gz::sim::EntityComponentManager& _ecm, + gz::sim::EventManager& /*_eventMgr*/) { + auto deviceIds = DeviceRegistry::getHandler()->getDevicesKeys(_ecm); + ASSERT_TRUE(deviceIds.size() == 1); + + yarp::dev::PolyDriver* driver = nullptr; + EXPECT_TRUE(DeviceRegistry::getHandler()->getDevice(_ecm, deviceIds[0], driver)); + ASSERT_TRUE(driver != nullptr); + ASSERT_TRUE(driver->view(iControlLimits)); + }); - driver = DeviceRegistry::getHandler()->getDevice(deviceIds[0]); - ASSERT_TRUE(driver != nullptr); - ASSERT_TRUE(driver->view(iControlLimits)); + testFixture.Finalize(); auto expectedJointMaxLimits = std::vector{200.0, 10.0}; auto expectedJointMinLimits = std::vector{-200.0, -10.0}; diff --git a/tests/controlboard/ControlBoardOnMultipleGazeboInstancesTest.cc b/tests/controlboard/ControlBoardOnMultipleGazeboInstancesTest.cc index 28a050f..717a136 100644 --- a/tests/controlboard/ControlBoardOnMultipleGazeboInstancesTest.cc +++ b/tests/controlboard/ControlBoardOnMultipleGazeboInstancesTest.cc @@ -72,7 +72,7 @@ TEST(ControlBoardOnMultipleGazeboInstances, StartConcurrentGazeboInstances) gz::sim::EventManager& /*_eventMgr*/) { auto deviceIds = gzyarp::DeviceRegistry::getHandler()->getDevicesKeys(_ecm); ASSERT_TRUE(deviceIds.size() == 1); - driver1 = gzyarp::DeviceRegistry::getHandler()->getDevice(deviceIds[0]); + EXPECT_TRUE(DeviceRegistry::getHandler()->getDevice(_ecm, deviceIds[0], driver1)); ASSERT_TRUE(driver1 != nullptr); iEncoders1 = nullptr; ASSERT_TRUE(driver1->view(iEncoders1)); @@ -102,7 +102,7 @@ TEST(ControlBoardOnMultipleGazeboInstances, StartConcurrentGazeboInstances) gz::sim::EventManager& /*_eventMgr*/) { auto deviceIds = gzyarp::DeviceRegistry::getHandler()->getDevicesKeys(_ecm); ASSERT_TRUE(deviceIds.size() == 1); - driver2 = gzyarp::DeviceRegistry::getHandler()->getDevice(deviceIds[0]); + EXPECT_TRUE(DeviceRegistry::getHandler()->getDevice(_ecm, deviceIds[0], driver2)); ASSERT_TRUE(driver2 != nullptr); iEncoders2 = nullptr; ASSERT_TRUE(driver2->view(iEncoders2)); @@ -153,9 +153,6 @@ TEST(ControlBoardOnMultipleGazeboInstances, StartConcurrentGazeboInstances) << std::endl; } - // Check that DeviceRegistry has two devices, one for each Gazebo instance - ASSERT_EQ(gzyarp::DeviceRegistry::getHandler()->getDevicesKeys().size(), 2); - // Print final joint positions std::cerr << std::fixed << std::setprecision(10) << "Final joint position simulation 1: " << jointPosition1 << std::endl; diff --git a/tests/controlboard/ControlBoardPositionControlTest.cc b/tests/controlboard/ControlBoardPositionControlTest.cc index 48bd7d9..1ceee8d 100644 --- a/tests/controlboard/ControlBoardPositionControlTest.cc +++ b/tests/controlboard/ControlBoardPositionControlTest.cc @@ -65,10 +65,9 @@ class ControlBoardPositionFixture : public testing::Test EXPECT_NE(gz::sim::kNullEntity, modelEntity); model = gz::sim::Model(modelEntity); - auto deviceKeys = gzyarp::DeviceRegistry::getHandler()->getDevicesKeys(); + auto deviceKeys = gzyarp::DeviceRegistry::getHandler()->getDevicesKeys(_ecm); ASSERT_EQ(deviceKeys.size(), 1); - driver = gzyarp::DeviceRegistry::getHandler()->getDevice(deviceKeys[0]); - + EXPECT_TRUE(DeviceRegistry::getHandler()->getDevice(_ecm, deviceKeys[0], driver)); ASSERT_TRUE(driver != nullptr); iPositionControl = nullptr; ASSERT_TRUE(driver->view(iPositionControl)); diff --git a/tests/controlboard/ControlBoardPositionDirectControlTest.cc b/tests/controlboard/ControlBoardPositionDirectControlTest.cc index aad8a62..feae5c9 100644 --- a/tests/controlboard/ControlBoardPositionDirectControlTest.cc +++ b/tests/controlboard/ControlBoardPositionDirectControlTest.cc @@ -68,10 +68,10 @@ class ControlBoardPositionDirectFixture : public ::testing::Test EXPECT_NE(gz::sim::kNullEntity, modelEntity); model = gz::sim::Model(modelEntity); - auto devicesKeys = DeviceRegistry::getHandler()->getDevicesKeys(); + auto devicesKeys = DeviceRegistry::getHandler()->getDevicesKeys(_ecm); std::cerr << "Number of Devices: " << devicesKeys.size() << std::endl; auto cbKey = devicesKeys.at(0); - driver = DeviceRegistry::getHandler()->getDevice(cbKey); + EXPECT_TRUE(DeviceRegistry::getHandler()->getDevice(_ecm, devicesKeys[0], driver)); std::cerr << "Driver key: " << cbKey << std::endl; ASSERT_TRUE(driver != nullptr); iPositionDirectControl = nullptr; diff --git a/tests/controlboard/ControlBoardTorqueControlTest.cc b/tests/controlboard/ControlBoardTorqueControlTest.cc index 7159e6d..bf894d0 100644 --- a/tests/controlboard/ControlBoardTorqueControlTest.cc +++ b/tests/controlboard/ControlBoardTorqueControlTest.cc @@ -64,9 +64,9 @@ class ControlBoardTorqueControlFixture : public testing::TestWithParamgetDevicesKeys(); + auto deviceKeys = gzyarp::DeviceRegistry::getHandler()->getDevicesKeys(_ecm); ASSERT_EQ(deviceKeys.size(), 1); - driver = gzyarp::DeviceRegistry::getHandler()->getDevice(deviceKeys[0]); + EXPECT_TRUE(DeviceRegistry::getHandler()->getDevice(_ecm, deviceKeys[0], driver)); ASSERT_TRUE(driver != nullptr); iTorqueControl = nullptr; diff --git a/tests/forcetorque/ForceTorqueTest.cc b/tests/forcetorque/ForceTorqueTest.cc index a9d4fcc..61ebbaa 100644 --- a/tests/forcetorque/ForceTorqueTest.cc +++ b/tests/forcetorque/ForceTorqueTest.cc @@ -1,8 +1,18 @@ +#include +#include +#include +#include + #include + +#include #include + #include #include #include +#include +#include TEST(ForceTorqueTest, PluginTest) { diff --git a/tests/imu/ImuTest.cc b/tests/imu/ImuTest.cc index 4dce931..1b8a79f 100644 --- a/tests/imu/ImuTest.cc +++ b/tests/imu/ImuTest.cc @@ -35,7 +35,8 @@ class ImuFixture : public testing::Test gz::sim::EntityComponentManager& _ecm, gz::sim::EventManager& /*_eventMgr*/) { auto imuDrivers - = gzyarp::testing::TestHelpers::getDevicesOfType(); + = gzyarp::testing::TestHelpers::getDevicesOfType( + _ecm); ASSERT_TRUE(imuDrivers.size() == 1); driver = imuDrivers[0]; ASSERT_TRUE(driver != nullptr); diff --git a/tests/laser/LaserTest.cc b/tests/laser/LaserTest.cc index e3aad28..3e813b7 100644 --- a/tests/laser/LaserTest.cc +++ b/tests/laser/LaserTest.cc @@ -1,8 +1,18 @@ +#include +#include +#include +#include + #include + +#include #include + #include #include #include +#include +#include TEST(LaserTest, PluginTest) { diff --git a/tests/test-helpers/CMakeLists.txt b/tests/test-helpers/CMakeLists.txt index d1d36d0..9420967 100644 --- a/tests/test-helpers/CMakeLists.txt +++ b/tests/test-helpers/CMakeLists.txt @@ -9,5 +9,5 @@ target_link_libraries(test-helpers YARP::YARP_dev YARP::YARP_os YARP::YARP_init - gz-sim-yarp-device-registry + gz-sim-yarp-plugins::gz-sim-yarp-device-registry ) diff --git a/tests/test-helpers/TestHelpers.hh b/tests/test-helpers/TestHelpers.hh index e623cbd..4aba557 100644 --- a/tests/test-helpers/TestHelpers.hh +++ b/tests/test-helpers/TestHelpers.hh @@ -1,29 +1,52 @@ #pragma once +#include + +#include + #include +#include #include -#include - +#include namespace gzyarp::testing { class TestHelpers { public: - template std::vector static getDevicesOfType() + template + static inline std::vector getDevicesOfType(gz::sim::EntityComponentManager& ecm) { - auto deviceIds = DeviceRegistry::getHandler()->getDevicesKeys(); + std::vector deviceIds = DeviceRegistry::getHandler()->getDevicesKeys(ecm); + std::vector devices{}; for (auto& deviceId : deviceIds) { - auto polyDriver = DeviceRegistry::getHandler()->getDevice(deviceId); + yarp::dev::PolyDriver* polyDriver = nullptr; + if (!DeviceRegistry::getHandler()->getDevice(ecm, deviceId, polyDriver)) + { + std::cerr << "Error while getting device " << deviceId << std::endl; + continue; + } + + if (!polyDriver) + { + std::cerr << "Error: device " << deviceId << " is nullptr" << std::endl; + continue; + } + T* driver = nullptr; auto viewOk = polyDriver->view(driver); if (viewOk && driver) { devices.push_back(driver); } + + if (viewOk && !driver) + { + std::cerr << "Error: view of device " << deviceId << " is nullptr" << std::endl; + } } std::cerr << "Total devices registered: " << deviceIds.size() << ", of which of selected type: " << devices.size() << std::endl; From e00c04c5b486e6b47df5b74e02bb0b373a6b92eb Mon Sep 17 00:00:00 2001 From: Alessandro Croci Date: Fri, 19 Apr 2024 11:50:55 +0200 Subject: [PATCH 21/25] Fix ConfigurationParsingFromFile test --- plugins/controlboard/CMakeLists.txt | 1 - .../ConfigurationParsingFromFileTest.cc | 99 +++++++++---------- 2 files changed, 48 insertions(+), 52 deletions(-) diff --git a/plugins/controlboard/CMakeLists.txt b/plugins/controlboard/CMakeLists.txt index e37ac2e..0c868c4 100644 --- a/plugins/controlboard/CMakeLists.txt +++ b/plugins/controlboard/CMakeLists.txt @@ -27,7 +27,6 @@ target_link_libraries(gz-sim-yarp-controlboard-system YARP::YARP_init gz-sim-yarp-commons gz-sim-yarp-device-registry - gz-sim-yarp-commons ) # Add install target diff --git a/tests/commons/ConfigurationParsingFromFileTest.cc b/tests/commons/ConfigurationParsingFromFileTest.cc index f02488a..17c3f85 100644 --- a/tests/commons/ConfigurationParsingFromFileTest.cc +++ b/tests/commons/ConfigurationParsingFromFileTest.cc @@ -1,12 +1,5 @@ #include -#include -#include -#include -#include -#include -#include -#include #include #include @@ -19,71 +12,75 @@ #include #include #include - #include + #include +#include +#include +#include +#include #include #include TEST(ConfigurationParsingTest, LoadPluginsWithYarpConfigurationFile) { + using namespace std::chrono_literals; + 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::EntityComponentManager* ecm; 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 controlboard --> not working - // std::cerr << "Testing ControlBoard configuration" << std::endl; - // std::vector cbDrivers - // = - // gzyarp::testing::TestHelpers::getDevicesOfType( - // _ecm); - // EXPECT_EQ(cbDrivers.size(), 1); - // EXPECT_TRUE(cbDrivers[0] != nullptr); + gz::sim::EventManager& /*_eventMgr*/) { ecm = &_ecm; }); - // Test Camera - std::cerr << "Testing Camera configuration" << std::endl; - auto cameraDrivers - = gzyarp::testing::TestHelpers::getDevicesOfType(_ecm); - EXPECT_EQ(cameraDrivers.size(), 1); - EXPECT_TRUE(cameraDrivers[0] != nullptr); + testFixture.Finalize(); - // Test ForceTorque - std::cerr << "Testing FT configuration" << std::endl; - auto ftDrivers - = gzyarp::testing::TestHelpers::getDevicesOfType( - _ecm); - EXPECT_EQ(ftDrivers.size(), 1); - EXPECT_TRUE(ftDrivers[0] != nullptr); + // Test Camera + std::cerr << "Testing Camera configuration" << std::endl; + auto cameraDrivers + = gzyarp::testing::TestHelpers::getDevicesOfType(*ecm); + EXPECT_EQ(cameraDrivers.size(), 1); + EXPECT_TRUE(cameraDrivers[0] != nullptr); - // Test Imu - std::cerr << "Testing Imu configuration" << std::endl; - auto imuDrivers - = gzyarp::testing::TestHelpers::getDevicesOfType(_ecm); - EXPECT_EQ(imuDrivers.size(), 1); - EXPECT_TRUE(imuDrivers[0] != nullptr); + // Test ForceTorque + std::cerr << "Testing FT configuration" << std::endl; + auto ftDrivers + = gzyarp::testing::TestHelpers::getDevicesOfType( + *ecm); + EXPECT_EQ(ftDrivers.size(), 1); + EXPECT_TRUE(ftDrivers[0] != nullptr); - // Test Laser - std::cerr << "Testing Laser configuration" << std::endl; - auto laserDrivers - = gzyarp::testing::TestHelpers::getDevicesOfType(_ecm); - EXPECT_EQ(laserDrivers.size(), 1); - EXPECT_TRUE(laserDrivers[0] != nullptr); + // Test Imu + std::cerr << "Testing Imu configuration" << std::endl; + auto imuDrivers + = gzyarp::testing::TestHelpers::getDevicesOfType(*ecm); + EXPECT_EQ(imuDrivers.size(), 1); + EXPECT_TRUE(imuDrivers[0] != nullptr); - // Test basestate - std::cerr << "Testing BaseState configuration" << std::endl; - auto bsDrivers - = gzyarp::testing::TestHelpers::getDevicesOfType( - _ecm); - EXPECT_EQ(bsDrivers.size(), 1); - EXPECT_TRUE(bsDrivers[0] != nullptr); - }); + // Test Laser + std::cerr << "Testing Laser configuration" << std::endl; + auto laserDrivers + = gzyarp::testing::TestHelpers::getDevicesOfType(*ecm); + EXPECT_EQ(laserDrivers.size(), 1); + EXPECT_TRUE(laserDrivers[0] != nullptr); - testFixture.Finalize(); + // Test basestate + std::cerr << "Testing BaseState configuration" << std::endl; + auto bsDrivers + = gzyarp::testing::TestHelpers::getDevicesOfType(*ecm); + 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(*ecm); + EXPECT_EQ(cbDrivers.size(), 1); + EXPECT_TRUE(cbDrivers[0] != nullptr); } From fd8043fb3c1ec219c31f9d35cd441383ffcfa57b Mon Sep 17 00:00:00 2001 From: Alessandro Croci Date: Fri, 19 Apr 2024 12:32:47 +0200 Subject: [PATCH 22/25] Minor improvements to ControlBoard --- plugins/controlboard/include/ControlBoard.hh | 2 +- plugins/controlboard/include/ControlBoardData.hh | 1 - plugins/controlboard/src/ControlBoard.cpp | 3 --- 3 files changed, 1 insertion(+), 5 deletions(-) diff --git a/plugins/controlboard/include/ControlBoard.hh b/plugins/controlboard/include/ControlBoard.hh index 372a8a1..2f3ea1c 100644 --- a/plugins/controlboard/include/ControlBoard.hh +++ b/plugins/controlboard/include/ControlBoard.hh @@ -3,11 +3,11 @@ #include #include -#include #include #include #include +#include #include #include #include diff --git a/plugins/controlboard/include/ControlBoardData.hh b/plugins/controlboard/include/ControlBoardData.hh index 483fd39..b2466c1 100644 --- a/plugins/controlboard/include/ControlBoardData.hh +++ b/plugins/controlboard/include/ControlBoardData.hh @@ -66,7 +66,6 @@ class ControlBoardData { public: std::mutex mutex; - std::string controlBoardId; std::vector joints; yarp::os::Stamp simTime; diff --git a/plugins/controlboard/src/ControlBoard.cpp b/plugins/controlboard/src/ControlBoard.cpp index 6cb5c07..81a7fc0 100644 --- a/plugins/controlboard/src/ControlBoard.cpp +++ b/plugins/controlboard/src/ControlBoard.cpp @@ -110,13 +110,10 @@ void ControlBoard::Configure(const Entity& _entity, m_modelEntity = _entity; - // m_controlBoardData.controlBoardId = m_deviceId; - m_pluginParameters.put(yarp::dev::gzyarp::YarpControlBoardScopedName.c_str(), m_robotScopedName.c_str()); m_pluginParameters.put("device", "gazebo_controlboard"); - // m_pluginParameters.put("controlBoardId", m_deviceId); if (_sdf->HasElement("initialConfiguration")) { From d5807215b17f8d4ab0f8f9b8c477923b100e1328 Mon Sep 17 00:00:00 2001 From: Alessandro Croci Date: Fri, 19 Apr 2024 12:46:30 +0200 Subject: [PATCH 23/25] Minor updates to commons tests --- tests/commons/ConcurrentInstancesTest.cc | 4 +- .../ConfigurationParsingFromStringTest.cc | 86 +++++++++---------- tests/{controlboard => commons}/dummy_box.sdf | 0 .../dummy_sphere.sdf | 0 4 files changed, 44 insertions(+), 46 deletions(-) rename tests/{controlboard => commons}/dummy_box.sdf (100%) rename tests/{controlboard => commons}/dummy_sphere.sdf (100%) diff --git a/tests/commons/ConcurrentInstancesTest.cc b/tests/commons/ConcurrentInstancesTest.cc index e8ea02c..3e93527 100644 --- a/tests/commons/ConcurrentInstancesTest.cc +++ b/tests/commons/ConcurrentInstancesTest.cc @@ -12,9 +12,9 @@ TEST(ConcurrentInstancesTest, StartConcurrentGazeboInstancesOfDifferentModels) { auto plannedIterations = 1'000; - gz::sim::TestFixture fixture1("../../../tests/controlboard/" + gz::sim::TestFixture fixture1("../../../tests/commons/" "dummy_sphere.sdf"); - gz::sim::TestFixture fixture2("../../../tests/controlboard/" + gz::sim::TestFixture fixture2("../../../tests/commons/" "dummy_box.sdf"); gz::common::Console::SetVerbosity(4); diff --git a/tests/commons/ConfigurationParsingFromStringTest.cc b/tests/commons/ConfigurationParsingFromStringTest.cc index 62f94b4..b63a0e8 100644 --- a/tests/commons/ConfigurationParsingFromStringTest.cc +++ b/tests/commons/ConfigurationParsingFromStringTest.cc @@ -1,12 +1,5 @@ #include -#include -#include -#include -#include -#include -#include -#include #include #include @@ -21,6 +14,10 @@ #include #include +#include +#include +#include +#include #include #include @@ -29,53 +26,54 @@ TEST(ConfigurationParsingTest, LoadPluginsWithYarpConfigurationString) std::string modelSdfName = "model_configuration_string.sdf"; std::string sdfPath = std::string("../../../tests/commons/") + modelSdfName; yarp::os::NetworkBase::setLocalMode(true); + gz::common::Console::SetVerbosity(4); + + gz::sim::EntityComponentManager* ecm; 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(_ecm); - EXPECT_EQ(cameraDrivers.size(), 1); - EXPECT_TRUE(cameraDrivers[0] != nullptr); + gz::sim::EventManager& /*_eventMgr*/) { ecm = &_ecm; }); - // Test ForceTorque - std::cerr << "Testing FT configuration" << std::endl; - auto ftDrivers - = gzyarp::testing::TestHelpers::getDevicesOfType( - _ecm); - EXPECT_EQ(ftDrivers.size(), 1); - EXPECT_TRUE(ftDrivers[0] != nullptr); + testFixture.Finalize(); - // Test Imu - std::cerr << "Testing Imu configuration" << std::endl; - auto imuDrivers - = gzyarp::testing::TestHelpers::getDevicesOfType(_ecm); - EXPECT_EQ(imuDrivers.size(), 1); - EXPECT_TRUE(imuDrivers[0] != nullptr); + // Test Camera + std::cerr << "Testing Camera configuration" << std::endl; + auto cameraDrivers + = gzyarp::testing::TestHelpers::getDevicesOfType(*ecm); + EXPECT_EQ(cameraDrivers.size(), 1); + EXPECT_TRUE(cameraDrivers[0] != nullptr); - // Test Laser - std::cerr << "Testing Laser configuration" << std::endl; - auto laserDrivers - = gzyarp::testing::TestHelpers::getDevicesOfType(_ecm); - EXPECT_EQ(laserDrivers.size(), 1); - EXPECT_TRUE(laserDrivers[0] != nullptr); + // Test ForceTorque + std::cerr << "Testing FT configuration" << std::endl; + auto ftDrivers + = gzyarp::testing::TestHelpers::getDevicesOfType( + *ecm); + EXPECT_EQ(ftDrivers.size(), 1); + EXPECT_TRUE(ftDrivers[0] != nullptr); - // Test basestate - std::cerr << "Testing BaseState configuration" << std::endl; - auto bsDrivers - = gzyarp::testing::TestHelpers::getDevicesOfType( - _ecm); - EXPECT_EQ(bsDrivers.size(), 1); - EXPECT_TRUE(bsDrivers[0] != nullptr); + // Test Imu + std::cerr << "Testing Imu configuration" << std::endl; + auto imuDrivers + = gzyarp::testing::TestHelpers::getDevicesOfType(*ecm); + EXPECT_EQ(imuDrivers.size(), 1); + EXPECT_TRUE(imuDrivers[0] != nullptr); - // Controlboard test skipped since configuration too complex to pass as string - }); + // Test Laser + std::cerr << "Testing Laser configuration" << std::endl; + auto laserDrivers + = gzyarp::testing::TestHelpers::getDevicesOfType(*ecm); + EXPECT_EQ(laserDrivers.size(), 1); + EXPECT_TRUE(laserDrivers[0] != nullptr); - testFixture.Finalize(); + // Test basestate + std::cerr << "Testing BaseState configuration" << std::endl; + auto bsDrivers + = gzyarp::testing::TestHelpers::getDevicesOfType(*ecm); + EXPECT_EQ(bsDrivers.size(), 1); + EXPECT_TRUE(bsDrivers[0] != nullptr); + + // Controlboard test skipped since configuration too complex to pass as string } diff --git a/tests/controlboard/dummy_box.sdf b/tests/commons/dummy_box.sdf similarity index 100% rename from tests/controlboard/dummy_box.sdf rename to tests/commons/dummy_box.sdf diff --git a/tests/controlboard/dummy_sphere.sdf b/tests/commons/dummy_sphere.sdf similarity index 100% rename from tests/controlboard/dummy_sphere.sdf rename to tests/commons/dummy_sphere.sdf From 42ff8cfd4595b1d9fd2931ebc96c303d39c4432d Mon Sep 17 00:00:00 2001 From: Alessandro Croci Date: Fri, 19 Apr 2024 14:36:57 +0200 Subject: [PATCH 24/25] Update sdf models used in ConcurrentInstancesTest to use DART engine and collision detector to avoid exception in concurrent simulations --- tests/commons/dummy_box.sdf | 25 ++++++++++++++++++++++++- tests/commons/dummy_sphere.sdf | 25 ++++++++++++++++++++++++- 2 files changed, 48 insertions(+), 2 deletions(-) diff --git a/tests/commons/dummy_box.sdf b/tests/commons/dummy_box.sdf index a3ea2ad..f44e59c 100644 --- a/tests/commons/dummy_box.sdf +++ b/tests/commons/dummy_box.sdf @@ -3,7 +3,30 @@ - + + + dart + + + + + gz-physics-dartsim-plugin + + + + + + + + true 0 0 10 0 0 0 diff --git a/tests/commons/dummy_sphere.sdf b/tests/commons/dummy_sphere.sdf index d801534..01cadde 100644 --- a/tests/commons/dummy_sphere.sdf +++ b/tests/commons/dummy_sphere.sdf @@ -3,7 +3,30 @@ - + + + dart + + + + + gz-physics-dartsim-plugin + + + + + + + + true 0 0 10 0 0 0 From b57c41c62e1354cd18445eadf54c327041c432ad Mon Sep 17 00:00:00 2001 From: Alessandro Croci Date: Fri, 19 Apr 2024 15:52:53 +0200 Subject: [PATCH 25/25] Update pendulum_multiple_gz_instances.sdf to use DART --- .../pendulum_multiple_gz_instances.sdf | 25 ++++++++++++++++++- 1 file changed, 24 insertions(+), 1 deletion(-) diff --git a/tests/controlboard/pendulum_multiple_gz_instances.sdf b/tests/controlboard/pendulum_multiple_gz_instances.sdf index 8e2c04f..a9f1e81 100644 --- a/tests/controlboard/pendulum_multiple_gz_instances.sdf +++ b/tests/controlboard/pendulum_multiple_gz_instances.sdf @@ -3,7 +3,30 @@ - + + + dart + + + + + gz-physics-dartsim-plugin + + + + + + + + true 0 0 10 0 0 0