Skip to content

Commit

Permalink
Merge pull request #153 from robotology/feature/run-parallel-gz-insta…
Browse files Browse the repository at this point in the history 
…nces

Run two parallel simulations of a gz-sim-yarp-plugins-enabled robot
  • Loading branch information
@xela-95
xela-95 authored Apr 19, 2024
2 parents 373d512 + b57c41c commit 870b1d9
Show file tree
Hide file tree
Showing 75 changed files with 1,845 additions and 1,431 deletions.
293 changes: 216 additions & 77 deletions libraries/device-registry/DeviceRegistry.cc
View file
Original file line number Diff line number Diff line change
@@ -1,9 +1,16 @@
#include <DeviceRegistry.hh>

#include <cstddef>
#include <gz/sim/Entity.hh>
#include <gz/sim/EntityComponentManager.hh>
#include <gz/sim/Util.hh>
#include <iostream>
#include <mutex>
#include <ostream>
#include <sstream>
#include <string>
#include <unordered_map>
#include <utility>
#include <vector>

#include <yarp/dev/PolyDriver.h>
Expand All @@ -21,124 +28,256 @@ 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;
}

bool DeviceRegistry::getDevicesAsPolyDriverList(
const std::string& modelScopedName,
yarp::dev::PolyDriverList& list,
std::vector<std::string>& deviceScopedNames /*, const std::string& worldName*/)
bool DeviceRegistry::getDevicesAsPolyDriverList(const gz::sim::EntityComponentManager& ecm,
const std::string& modelScopedName,
yarp::dev::PolyDriverList& list,
std::vector<std::string>& 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<std::string, std::string> inserted_yarpDeviceName2deviceDatabaseKey;

for (auto&& devicesMapElem : m_devicesMap)
{
std::string deviceDatabaseKey = devicesMapElem.first;
std::string yarpDeviceName;
std::lock_guard<std::mutex> 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.rfind(modelScopedName, 0)
== 0) /*|| (deviceDatabaseKey.rfind(worldName + "/" + modelScopedName, 0) == 0)*/)
// 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::Handler 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<std::mutex> 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::Handler 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<std::string, std::unordered_map<std::string, yarp::dev::PolyDriver*>>(
gzInstanceId, std::unordered_map<std::string, yarp::dev::PolyDriver*>{}));
}
} else
{
// device does not exists. Add to map
if (!m_devicesMap
.insert(
std::pair<std::string, yarp::dev::PolyDriver*>(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::Handler while inserting a new device pointer!";
ret = false;
} else
// If not, add it
m_devicesMap[gzInstanceId].insert(
std::pair<std::string, yarp::dev::PolyDriver*>(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;

{
std::lock_guard<std::mutex> lock(mutex());

// 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;
}

DevicesMap::const_iterator device = m_devicesMap.find(deviceDatabaseKey);
if (device != m_devicesMap.end())
tmp = device->second;
else
tmp = NULL;
driver = m_devicesMap.at(gzInstanceId).at(deviceDatabaseKey);

return tmp;
if (!driver)
{
yError() << "Error in gzyarp::DeviceRegistry::getDevice: driver is "
"nullptr";
return false;
}
}

return true;
}

void DeviceRegistry::removeDevice(const std::string& deviceDatabaseKey)
std::vector<std::string>
DeviceRegistry::getDevicesKeys(const gz::sim::EntityComponentManager& ecm) const
{
DevicesMap::iterator device = m_devicesMap.find(deviceDatabaseKey);
if (device != m_devicesMap.end())
std::vector<std::string> keys{};

{
device->second->close();
m_devicesMap.erase(device);
} else
std::lock_guard<std::mutex> lock(mutex());

// 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;
}

bool DeviceRegistry::removeDevice(const gz::sim::EntityComponentManager& ecm,
const std::string& deviceDatabaseKey)
{

{
yError() << "Could not remove device " << deviceDatabaseKey << ". Device was not found";
std::lock_guard<std::mutex> lock(mutex());

// 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())
{
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;
return true;
}

// Private methods

std::string DeviceRegistry::generateDeviceId(const gz::sim::Entity& entity,
const gz::sim::EntityComponentManager& ecm,
const std::string& yarpDeviceName)
{
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()
Expand Down
Loading

0 comments on commit 870b1d9

Please sign in to comment.