Skip to content

Commit

Permalink
- Update device Id generation using ECM pointer to discriminate betwe…
Browse files Browse the repository at this point in the history 
…en different simulations.

- Update model used in unit test to a double pendulum (the two simulations outcomes diverge quickly)
- Update unit test
  • Loading branch information
@xela-95
xela-95 committed Apr 16, 2024
1 parent f0d2246 commit c345e4c
Show file tree
Hide file tree
Showing 5 changed files with 126 additions and 94 deletions.
11 changes: 10 additions & 1 deletion libraries/common/DeviceIdGenerator.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,10 @@
#include <DeviceIdGenerator.hh>

#include <iostream>
#include <ostream>
#include <sstream>
#include <string>

#include <gz/sim/Entity.hh>
#include <gz/sim/EntityComponentManager.hh>
#include <gz/sim/Util.hh>
Expand All @@ -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
35 changes: 31 additions & 4 deletions libraries/device-registry/DeviceRegistry.cc
View file
Original file line number Diff line number Diff line change
@@ -1,7 +1,11 @@
#include <DeviceRegistry.hh>

#include <cstddef>
#include <gz/sim/EntityComponentManager.hh>
#include <iostream>
#include <mutex>
#include <ostream>
#include <sstream>
#include <string>
#include <unordered_map>
#include <vector>
Expand Down Expand Up @@ -131,10 +135,33 @@ yarp::dev::PolyDriver* DeviceRegistry::getDevice(const std::string& deviceDataba

std::vector<std::string> DeviceRegistry::getDevicesKeys() const
{
std::vector<std::string> keys;
for (auto&& [key, value] : m_devicesMap)
keys.push_back(key);
return keys;
{
std::lock_guard<std::mutex> lock(mutex());

std::vector<std::string> keys;
for (auto&& [key, value] : m_devicesMap)
keys.push_back(key);
return keys;
}
}

std::vector<std::string>
DeviceRegistry::getDevicesKeys(const gz::sim::EntityComponentManager& ecm) const
{
std::stringstream EcmPtrSs;
EcmPtrSs << &ecm;

{
std::lock_guard<std::mutex> lock(mutex());
std::cerr << "==================== ecm address: " << EcmPtrSs.str() << std::endl;
std::vector<std::string> keys;
for (auto&& [key, value] : m_devicesMap)
{
if (key.find(EcmPtrSs.str()) != std::string::npos)
keys.push_back(key);
}
return keys;
}
}

// Private methods
Expand Down
5 changes: 5 additions & 0 deletions libraries/device-registry/DeviceRegistry.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,8 @@
#include <yarp/dev/PolyDriver.h>
#include <yarp/dev/PolyDriverList.h>

#include <gz/sim/EntityComponentManager.hh>

namespace gzyarp
{

Expand All @@ -27,6 +29,9 @@ public:

std::vector<std::string> getDevicesKeys() const;

// Return only the devices that belong to simulation having the ecm passed as argument
std::vector<std::string> getDevicesKeys(const gz::sim::EntityComponentManager& ecm) const;

private:
DeviceRegistry();
static DeviceRegistry* s_handle;
Expand Down
46 changes: 28 additions & 18 deletions tests/controlboard/ControlBoardOnMultipleGazeboInstances.cc
View file
Original file line number Diff line number Diff line change
@@ -1,21 +1,29 @@
#include <Common.hh>
#include <DeviceRegistry.hh>

#include <algorithm>
#include <chrono>
#include <gtest/gtest.h>

#include <cmath>
#include <cstdlib>
#include <iomanip>
#include <ios>
#include <iostream>
#include <memory>
#include <ostream>
#include <string>
#include <thread>

#include <gz/common/Console.hh>
#include <gz/sim/Entity.hh>
#include <gz/sim/EventManager.hh>
#include <gz/sim/Joint.hh>
#include <gz/sim/Link.hh>
#include <gz/sim/Model.hh>
#include <gz/sim/TestFixture.hh>
#include <gz/sim/Types.hh>
#include <gz/sim/Util.hh>
#include <gz/sim/World.hh>
#include <gz/sim/components/JointForceCmd.hh>
#include <sdf/Element.hh>

#include <yarp/dev/IControlMode.h>
#include <yarp/dev/IEncoders.h>
Expand All @@ -33,36 +41,33 @@ 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
.OnConfigure([&](const gz::sim::Entity& _worldEntity,
const std::shared_ptr<const sdf::Element>& /*_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();

Expand All @@ -71,18 +76,16 @@ TEST(ControlBoardOnMultipleGazeboInstances, StartConcurrentGazeboInstances)
const std::shared_ptr<const sdf::Element>& /*_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();

Expand All @@ -91,14 +94,21 @@ 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);
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);

// 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
Expand Down
Loading

0 comments on commit c345e4c

Please sign in to comment.