Skip to content

Commit

Permalink
Fix the YarpRobotInterface regression after 4e9a2ba
Browse files Browse the repository at this point in the history
  • Loading branch information
@GiulioRomualdi
GiulioRomualdi committed Jun 10, 2024
1 parent fab9279 commit 882b961
Show file tree
Hide file tree
Showing 2 changed files with 137 additions and 67 deletions.
4 changes: 2 additions & 2 deletions devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -180,7 +180,7 @@ class YarpRobotLoggerDevice : public yarp::dev::DeviceDriver,
void lookForNewLogs();
void lookForExogenousSignals();

bool initMetadata(const std::string& nameKey, const std::vector<std::string>& metadata);
bool addChannel(const std::string& nameKey, std::size_t vectorSize, const std::vector<std::string>& metadata = {});

bool hasSubstring(const std::string& str, const std::vector<std::string>& substrings) const;
void recordVideo(const std::string& cameraName, VideoWriter& writer);
Expand Down Expand Up @@ -228,7 +228,7 @@ class YarpRobotLoggerDevice : public yarp::dev::DeviceDriver,
const std::vector<std::string> gyroElementNames = {"omega_x", "omega_y", "omega_z"};

const std::string accelerometersName = "accelerometers";
const std::vector<std::string> AccelerometerElementNames = {"a_x", "a_y", "a_z"};
const std::vector<std::string> accelerometerElementNames = {"a_x", "a_y", "a_z"};

const std::string orientationsName = "orientations";
const std::vector<std::string> orientationElementNames = {"r", "p", "y"};
Expand Down
200 changes: 135 additions & 65 deletions devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -635,16 +635,58 @@ bool YarpRobotLoggerDevice::setupRobotCameraBridge(
return true;
}

bool YarpRobotLoggerDevice::initMetadata(const std::string& nameKey,
const std::vector<std::string>& metadataNames)
bool YarpRobotLoggerDevice::addChannel(const std::string& nameKey,
std::size_t vectorSize,
const std::vector<std::string>& metadataNames)
{
bool ok = m_bufferManager.addChannel({nameKey, {metadataNames.size(), 1}, metadataNames});
std::string rtNameKey = robotRtRootName + treeDelim + nameKey;
if (metadataNames.empty() || vectorSize != metadataNames.size())
{
log()->warn("The metadata names are empty or the size of the metadata names is different "
"from the vector size. The default metadata will be used.");
if (!m_bufferManager.addChannel({nameKey, {vectorSize, 1}}))
{
log()->error("Failed to add the channel in buffer manager named: {}", nameKey);
return false;
}
} else
{
if (!m_bufferManager.addChannel({nameKey, {metadataNames.size(), 1}, metadataNames}))
{
log()->error("Failed to add the channel in buffer manager named: {}", nameKey);
return false;
}
}
if (m_sendDataRT)
{
ok = ok && m_vectorCollectionRTDataServer.populateMetadata(rtNameKey, metadataNames);
std::string rtNameKey = robotRtRootName + treeDelim + nameKey;

// if the metadata names are empty then the default metadata will be used
if (!metadataNames.empty())
{
if (!m_vectorCollectionRTDataServer.populateMetadata(rtNameKey, metadataNames))
{
log()->error("[Realtime logging] Failed to populate the metadata for the signal {}",
rtNameKey);
return false;
}

} else
{
std::vector<std::string> defaultMetadata;
for (std::size_t i = 0; i < vectorSize; i++)
{
defaultMetadata.push_back("element_" + std::to_string(i));
}

if (!m_vectorCollectionRTDataServer.populateMetadata(rtNameKey, defaultMetadata))
{
log()->error("[Realtime logging] Failed to populate the metadata for the signal {}",
rtNameKey);
return false;
}
}
}
return ok;
return true;
}

bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly)
Expand Down Expand Up @@ -706,35 +748,35 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly)
// prepare the telemetry
if (m_streamJointStates)
{
ok = ok && initMetadata(jointStatePositionsName, joints);
ok = ok && initMetadata(jointStateVelocitiesName, joints);
ok = ok && initMetadata(jointStateAccelerationsName, joints);
ok = ok && initMetadata(jointStateTorquesName, joints);
ok = ok && addChannel(jointStatePositionsName, joints.size(), joints);
ok = ok && addChannel(jointStateVelocitiesName, joints.size(), joints);
ok = ok && addChannel(jointStateAccelerationsName, joints.size(), joints);
ok = ok && addChannel(jointStateTorquesName, joints.size(), joints);
}
if (m_streamMotorStates)
{
ok = ok && initMetadata(motorStatePositionsName, joints);
ok = ok && initMetadata(motorStateVelocitiesName, joints);
ok = ok && initMetadata(motorStateAccelerationsName, joints);
ok = ok && initMetadata(motorStateCurrentsName, joints);
ok = ok && addChannel(motorStatePositionsName, joints.size(), joints);
ok = ok && addChannel(motorStateVelocitiesName, joints.size(), joints);
ok = ok && addChannel(motorStateAccelerationsName, joints.size(), joints);
ok = ok && addChannel(motorStateCurrentsName, joints.size(), joints);
}

if (m_streamMotorPWM)
{
ok = ok && initMetadata(motorStatePwmName, joints);
ok = ok && addChannel(motorStatePwmName, joints.size(), joints);
}

if (m_streamPIDs)
{
ok = ok && initMetadata(motorStatePidsName, joints);
ok = ok && addChannel(motorStatePidsName, joints.size(), joints);
}

if (m_streamFTSensors)
{
for (const auto& sensorName : m_robotSensorBridge->getSixAxisForceTorqueSensorsList())
{
std::string fullFTSensorName = ftsName + treeDelim + sensorName;
ok = ok && initMetadata(fullFTSensorName, ftElementNames);
ok = ok && addChannel(fullFTSensorName, ftElementNames.size(), ftElementNames);
}
}

Expand All @@ -743,25 +785,34 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly)
for (const auto& sensorName : m_robotSensorBridge->getGyroscopesList())
{
std::string fullGyroSensorName = gyrosName + treeDelim + sensorName;
ok = ok && initMetadata(fullGyroSensorName, gyroElementNames);
ok = ok && addChannel(fullGyroSensorName, gyroElementNames.size(), gyroElementNames);
}

for (const auto& sensorName : m_robotSensorBridge->getLinearAccelerometersList())
{
std::string fullAccelerometerSensorName = accelerometersName + treeDelim + sensorName;
ok = ok && initMetadata(fullAccelerometerSensorName, AccelerometerElementNames);
ok = ok
&& addChannel(fullAccelerometerSensorName,
accelerometerElementNames.size(), //
accelerometerElementNames);
}

for (const auto& sensorName : m_robotSensorBridge->getOrientationSensorsList())
{
std::string fullOrientationsSensorName = orientationsName + treeDelim + sensorName;
ok = ok && initMetadata(fullOrientationsSensorName, orientationElementNames);
ok = ok
&& addChannel(fullOrientationsSensorName,
orientationElementNames.size(), //
orientationElementNames);
}

for (const auto& sensorName : m_robotSensorBridge->getMagnetometersList())
{
std::string fullMagnetometersSensorName = magnetometersName + treeDelim + sensorName;
ok = ok && initMetadata(fullMagnetometersSensorName, magnetometerElementNames);
ok = ok
&& addChannel(fullMagnetometersSensorName,
magnetometerElementNames.size(), //
magnetometerElementNames);
}

// an IMU contains a gyro accelerometer and an orientation sensor
Expand All @@ -770,9 +821,15 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly)
std::string fullAccelerometerSensorName = accelerometersName + treeDelim + sensorName;
std::string fullGyroSensorName = gyrosName + treeDelim + sensorName;
std::string fullOrientationsSensorName = orientationsName + treeDelim + sensorName;
ok = ok && initMetadata(fullAccelerometerSensorName, AccelerometerElementNames);
ok = ok && initMetadata(fullGyroSensorName, gyroElementNames);
ok = ok && initMetadata(fullOrientationsSensorName, orientationElementNames);
ok = ok
&& addChannel(fullAccelerometerSensorName,
accelerometerElementNames.size(), //
accelerometerElementNames);
ok = ok && addChannel(fullGyroSensorName, gyroElementNames.size(), gyroElementNames);
ok = ok
&& addChannel(fullOrientationsSensorName,
orientationElementNames.size(), //
orientationElementNames);
}
}

Expand All @@ -782,7 +839,10 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly)
{
std::string fullCartesianWrenchName
= cartesianWrenchesName + treeDelim + cartesianWrenchName;
ok = ok && initMetadata(fullCartesianWrenchName, cartesianWrenchNames);
ok = ok
&& addChannel(fullCartesianWrenchName,
cartesianWrenchNames.size(), //
cartesianWrenchNames);
}
}

Expand All @@ -791,59 +851,66 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly)
for (const auto& sensorName : m_robotSensorBridge->getTemperatureSensorsList())
{
std::string fullTemperatureSensorName = temperatureName + treeDelim + sensorName;
ok = ok && initMetadata(fullTemperatureSensorName, temperatureNames);
ok = ok
&& addChannel(fullTemperatureSensorName,
temperatureNames.size(), //
temperatureNames);
}
}

std::string signalFullName = "";
std::string rtSignalFullName = "";

for (auto& [name, signal] : m_vectorsCollectionSignals)
// this is required only if the realtime is enabled
if (m_sendDataRT)
{
std::lock_guard<std::mutex> lock(signal.mutex);
BipedalLocomotion::YarpUtilities::VectorsCollection* externalSignalCollection
= signal.client.readData(false);
if (externalSignalCollection != nullptr)
std::string signalFullName = "";

for (auto& [name, signal] : m_vectorsCollectionSignals)
{
if (!signal.dataArrived)
std::lock_guard<std::mutex> lock(signal.mutex);
BipedalLocomotion::YarpUtilities::VectorsCollection* externalSignalCollection
= signal.client.readData(false);
if (externalSignalCollection != nullptr)
{
for (const auto& [key, vector] : externalSignalCollection->vectors)
if (!signal.dataArrived)
{
signalFullName = signal.signalName + treeDelim + key;
const auto& metadata = signal.metadata.vectors.find(key);
if (metadata == signal.metadata.vectors.cend())
bool channelAdded = false;
for (const auto& [key, vector] : externalSignalCollection->vectors)
{
log()->warn("{} Unable to find the metadata for the signal named {}. The "
"default one will be used.",
logPrefix,
signalFullName);
initMetadata(signalFullName, {});
} else
{
initMetadata(signalFullName, {metadata->second});
signalFullName = signal.signalName + treeDelim + key;
const auto& metadata = signal.metadata.vectors.find(key);
if (metadata == signal.metadata.vectors.cend())
{
log()->warn("{} Unable to find the metadata for the signal named {}. "
"The "
"default one will be used.",
logPrefix,
signalFullName);
channelAdded = addChannel(signalFullName, vector.size());

} else
{
channelAdded = addChannel(signalFullName, //
vector.size(),
{metadata->second});
}
}
signal.dataArrived = channelAdded;
}
signal.dataArrived = true;
}
}
}

for (auto& [name, signal] : m_vectorSignals)
{
std::lock_guard<std::mutex> lock(signal.mutex);
yarp::sig::Vector* collection = signal.port.read(false);
if (collection != nullptr)
for (auto& [name, signal] : m_vectorSignals)
{
if (!signal.dataArrived)
std::lock_guard<std::mutex> lock(signal.mutex);
yarp::sig::Vector* vector = signal.port.read(false);
if (vector != nullptr)
{
initMetadata(signalFullName, {});
signal.dataArrived = true;
if (!signal.dataArrived)
{
signal.dataArrived = addChannel(signalFullName, vector->size());
}
}
}
}

if (m_sendDataRT)
{
m_vectorCollectionRTDataServer.finalizeMetadata();
}

Expand Down Expand Up @@ -1555,6 +1622,7 @@ void YarpRobotLoggerDevice::run()
{
if (!signal.dataArrived)
{
bool channelAdded = false;
for (const auto& [key, vector] : collection->vectors)
{
signalFullName = signal.signalName + treeDelim + key;
Expand All @@ -1565,13 +1633,16 @@ void YarpRobotLoggerDevice::run()
"default one will be used.",
logPrefix,
signalFullName);
initMetadata(signalFullName, {});
channelAdded = addChannel(signalFullName, vector.size());

} else
{
initMetadata(signalFullName, {metadata->second});
channelAdded = addChannel(signalFullName, //
vector.size(),
{metadata->second});
}
}
signal.dataArrived = true;
signal.dataArrived = channelAdded;
} else
{
for (const auto& [key, vector] : collection->vectors)
Expand All @@ -1591,8 +1662,7 @@ void YarpRobotLoggerDevice::run()
{
if (!signal.dataArrived)
{
initMetadata(signalFullName, {});
signal.dataArrived = true;
signal.dataArrived = addChannel(signalFullName, vector->size());
}
}
}
Expand Down

0 comments on commit 882b961

Please sign in to comment.