From 8057e2f452e3c758ca75ffc5cc4a2f7ace2c53d7 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Mon, 10 Jun 2024 17:35:20 +0200 Subject: [PATCH] Fix signal logging in YarpRobotLoggerDevice (#849) This fixes the regression https://github.com/ami-iit/bipedal-locomotion-framework/issues/846 introduced in ami-iit/bipedal-locomotion-framework@4e9a2bac07d99d6c648a1206d7b6a33c300557a4 --- .../BipedalLocomotion/YarpRobotLoggerDevice.h | 4 +- .../src/YarpRobotLoggerDevice.cpp | 200 ++++++++++++------ 2 files changed, 137 insertions(+), 67 deletions(-) diff --git a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h index 24e2b87eeb..8ffefbf967 100644 --- a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h +++ b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h @@ -180,7 +180,7 @@ class YarpRobotLoggerDevice : public yarp::dev::DeviceDriver, void lookForNewLogs(); void lookForExogenousSignals(); - bool initMetadata(const std::string& nameKey, const std::vector& metadata); + bool addChannel(const std::string& nameKey, std::size_t vectorSize, const std::vector& metadata = {}); bool hasSubstring(const std::string& str, const std::vector& substrings) const; void recordVideo(const std::string& cameraName, VideoWriter& writer); @@ -228,7 +228,7 @@ class YarpRobotLoggerDevice : public yarp::dev::DeviceDriver, const std::vector gyroElementNames = {"omega_x", "omega_y", "omega_z"}; const std::string accelerometersName = "accelerometers"; - const std::vector AccelerometerElementNames = {"a_x", "a_y", "a_z"}; + const std::vector accelerometerElementNames = {"a_x", "a_y", "a_z"}; const std::string orientationsName = "orientations"; const std::vector orientationElementNames = {"r", "p", "y"}; diff --git a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp index e163baf1cc..d274661592 100644 --- a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp +++ b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp @@ -635,16 +635,58 @@ bool YarpRobotLoggerDevice::setupRobotCameraBridge( return true; } -bool YarpRobotLoggerDevice::initMetadata(const std::string& nameKey, - const std::vector& metadataNames) +bool YarpRobotLoggerDevice::addChannel(const std::string& nameKey, + std::size_t vectorSize, + const std::vector& 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 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) @@ -706,27 +748,27 @@ 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) @@ -734,7 +776,7 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) 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); } } @@ -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 @@ -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); } } @@ -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); } } @@ -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 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 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 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 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(); } @@ -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; @@ -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) @@ -1591,8 +1662,7 @@ void YarpRobotLoggerDevice::run() { if (!signal.dataArrived) { - initMetadata(signalFullName, {}); - signal.dataArrived = true; + signal.dataArrived = addChannel(signalFullName, vector->size()); } } }